draw_utils.py 文件源码

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())