detection.py 文件源码

def observationToPose(self, observation):
        pose    = geometry_msgs.msg.Pose()

        if observation['orientation'] == None:
            # if observation is ball
            observation['orientation']  = 0.0

        if self.do_side_invert == True:
            observation['x']    = -observation['x']
            observation['y']    = -observation['y']
            observation['orientation']    = observation['orientation'] + math.pi

        pose.position.x = observation['x'] / 1000
        pose.position.y = observation['y'] / 1000
        pose.position.z = observation['z'] / 1000

        quat = quaternion_from_euler(0.0, 0.0, observation['orientation'])
        pose.orientation.x = quat[0]
        pose.orientation.y = quat[1]
        pose.orientation.z = quat[2]
        pose.orientation.w = quat[3]

        return  pose