diff --git a/robomaster_pi/connection.py b/robomaster_pi/connection.py index 90c46ae7cb4d4f6351addb21f34a54c0d6f4031e..6400680f553605e7f9cca2711ac85f013c32789e 100644 --- a/robomaster_pi/connection.py +++ b/robomaster_pi/connection.py @@ -41,28 +41,8 @@ def quaternion_from_euler(ai, aj, ak): return q -class Robomaster_odom(Node): - def __init__(self): - self.ep_chassis.sub_position(freq=10, callback=sub_position_handler) - def sub_position_handler(position_info): - super().__init__("robomaster_odom") - tf_broadcaster = TransformBroadcaster() - t = TransformStamped() - t.header.stampep_chassis = time.get_clock().now().to_msg() - t.header.frame_id = 'odom' - t.child_frame_id = "base_footprint" - t.transform.translation.x = position_info.x - t.transform.translation.y = -position_info.y - t.transform.translation.z = 0.0 - q = quaternion_from_euler(0, 0, -position_info.z* math.pi/180) - t.transform.rotation.x = q[0] - t.transform.rotation.y = q[1] - t.transform.rotation.z = q[2] - t.transform.rotation.w = q[3] - tf_broadcaster.sendTransform(t) - print("ok") class vel_from_ros_node(Node): diff --git a/robomaster_pi/robot_ros_odom.py b/robomaster_pi/robot_ros_odom.py index d585b694ee15c65ef5ed5c95f9c6552b68962f77..bfae794eec983e2642cdcf02da77af603d9e9d07 100644 --- a/robomaster_pi/robot_ros_odom.py +++ b/robomaster_pi/robot_ros_odom.py @@ -39,8 +39,8 @@ def quaternion_from_euler(ai, aj, ak): return q class Robomaster_odom(Node): - def __init__(self): - self.ep_chassis.sub_position(freq=10, callback=sub_position_handler) + def __init__(self, ep_chassis): + ep_chassis.sub_position(freq=10, callback=self.sub_position_handler) def sub_position_handler(position_info):