diff --git a/robomaster_pi/conn_odom.py b/robomaster_pi/conn_odom.py index 7d85a93a2845557735eb92ac42bcec627b989560..322f538b51ef09afbf30269c60607bafb9523858 100644 --- a/robomaster_pi/conn_odom.py +++ b/robomaster_pi/conn_odom.py @@ -21,11 +21,11 @@ class OdometryPublisher(Node): def sub_position_handler(self, position_info): self.x, self.y, self.z = position_info - self.publish_odometry() print("ok") + self.publish_odometry() def publish_odometry(self): - print("ja") + q = self.quaternion_from_euler(0, 0, -self.z* math.pi/180) msg = Odometry() msg.header.stamp = self.get_clock().now().to_msg() @@ -37,6 +37,7 @@ class OdometryPublisher(Node): msg.pose.pose.orientation.z = q[2] msg.pose.pose.orientation.w = 0 self.publisher_.publish(msg) + print("ja") def quaternion_from_euler(self, ai, aj, ak): ai /= 2.0