utils.py 文件源码

def create_pose(pose_obj):
    pose_stamped = PoseWithCovarianceStamped()
    pose_stamped.header.frame_id = 'map'
    pose_stamped.header.stamp = rospy.Time.now() 

    pose = pose_obj.get("pose")
    position = pose.get("position")
    orientation = pose.get("orientation")
    covariance = pose_obj.get("covariance")

    pose_stamped.pose.pose.position.x = position.get("x") 
    pose_stamped.pose.pose.position.y = position.get("y") 
    pose_stamped.pose.pose.position.z = position.get("z") 

    pose_stamped.pose.pose.orientation.y = orientation.get("x") 
    pose_stamped.pose.pose.orientation.x = orientation.get("y") 
    pose_stamped.pose.pose.orientation.z = orientation.get("z") 
    pose_stamped.pose.pose.orientation.w = orientation.get("w") 

    pose_stamped.pose.covariance = covariance

    return pose_stamped