python ray定时_Python pybullet.rayTestBatch方法代码示例

# 需要导入模块: import pybullet [as 别名]

# 或者: from pybullet import rayTestBatch [as 别名]

def _laserScan(self):

"""

INTERNAL METHOD, a loop that simulate the laser and update the distance

value of each laser

"""

lastLidarTime = time.time()

while not self._module_termination:

nowLidarTime = time.time()

if (nowLidarTime-lastLidarTime > 1/LASER_FRAMERATE):

results = pybullet.rayTestBatch(

self.ray_from,

self.ray_to,

parentObjectUniqueId=self.robot_model,

parentLinkIndex=self.laser_id,

physicsClientId=self.physics_client)

for i in range(NUM_RAY*len(ANGLE_LIST_POSITION)):

hitObjectUid = results[i][0]

hitFraction = results[i][2]

hitPosition = results[i][3]

self.laser_value[i] = hitFraction * RAY_LENGTH

if self.display:

if not self.ray_ids:

self._createDebugLine()

if (hitFraction == 1.):

pybullet.addUserDebugLine(

self.ray_from[i],

self.ray_to[i],

RAY_MISS_COLOR,

replaceItemUniqueId=self.ray_ids[i],

parentObjectUniqueId=self.robot_model,

parentLinkIndex=self.laser_id,

physicsClientId=self.physics_client)

else: # pragma: no cover

localHitTo = [self.ray_from[i][0]+hitFraction*(

self.ray_to[i][0]-self.ray_from[i][0]),

self.ray_from[i][1]+hitFraction*(

self.ray_to[i][1]-self.ray_from[i][1]),

self.ray_from[i][2]+hitFraction*(

self.ray_to[i][2]-self.ray_from[i][2])]

pybullet.addUserDebugLine(

self.ray_from[i],

localHitTo,

RAY_HIT_COLOR,

replaceItemUniqueId=self.ray_ids[i],

parentObjectUniqueId=self.robot_model,

parentLinkIndex=self.laser_id,

physicsClientId=self.physics_client)

else:

if self.ray_ids:

self._resetDebugLine()

lastLidarTime = nowLidarTime