diff --git a/robomaster_pi/pup_position.py b/robomaster_pi/pup_position.py index 0b17782e98ebfb1035771965bea53bf550765842..5684fa96782982d95a677bb5293636691b7ff82a 100644 --- a/robomaster_pi/pup_position.py +++ b/robomaster_pi/pup_position.py @@ -1,6 +1,6 @@ import rclpy from rclpy.node import Node -from geometry_msgs.msg import PoseStamped, Pose +from geometry_msgs.msg import PoseStamped, Point, Quaternion from tf2_ros import TransformListener, Buffer from rclpy.time import Time @@ -18,7 +18,9 @@ class RobotPosePublisher(Node): transform = self.tf_buffer_.lookup_transform('map', 'base_link', Time()) pose_stamped = PoseStamped() pose_stamped.header = transform.header - pose_stamped.pose.position = transform.transform.translation + pose_stamped.pose.position = Point(x=transform.transform.translation.x, + y=transform.transform.translation.y, + z=transform.transform.translation.z) pose_stamped.pose.orientation = transform.transform.rotation self.publisher_.publish(pose_stamped) except Exception as e: