def publish_sensor_frame(self, channel, pose=None):
"""
Publish sensor frame in which the point clouds
are drawn with reference to. sensor_frame_msg.id is hashed
by its channel (may be collisions since its right shifted by 32)
"""
# Sensor frames msg
msg = vs.obj_collection_t()
msg.id = self.channel_uid(channel)
msg.name = 'BOTFRAME_' + channel
msg.type = vs.obj_collection_t.AXIS3D
msg.reset = True
# Send sensor pose
pose_msg = vs.obj_t()
roll, pitch, yaw, x, y, z = pose.to_rpyxyz(axes='sxyz')
pose_msg.id = 0
pose_msg.x, pose_msg.y, pose_msg.z, \
pose_msg.roll, pose_msg.pitch, pose_msg.yaw = x, y, z, roll, pitch, yaw
# Save pose
self.set_sensor_pose(channel, pose)
msg.objs = [pose_msg]
msg.nobjs = len(msg.objs)
self.lc.publish("OBJ_COLLECTION", msg.encode())
评论列表
文章目录