diff --git a/nodes/odom_publisher_python.py b/nodes/odom_publisher_python.py index 6b94d29..330f120 100644 --- a/nodes/odom_publisher_python.py +++ b/nodes/odom_publisher_python.py @@ -1,43 +1,47 @@ #!/usr/bin/env python import rospy -from nav_msgs.msg import Odometry -import tf -import math +import tf2_ros +from geometry_msgs.msg import TransformStamped, PoseStamped +from tf.transformations import quaternion_from_euler -def normalize_quaternion(x, y, z, w): - norm = math.sqrt(x**2 + y**2 + z**2 + w**2) - return x / norm, y / norm, z / norm, w / norm -def odom_callback(msg): - # Extract position and orientation from the odometry message - position = msg.pose.pose.position - orientation = msg.pose.pose.orientation +def odom_callback(msg, broadcaster): + # Create a TransformStamped object + transform = TransformStamped() - # Normalize the quaternion - qx, qy, qz, qw = normalize_quaternion( - orientation.x, orientation.y, orientation.z, orientation.w - ) + # Header + transform.header.stamp = rospy.Time.now() + transform.header.frame_id = "odom" # Parent frame + transform.child_frame_id = "base_link" # Child frame - # Get frame IDs from the message - parent_frame_id = msg.header.frame_id if msg.header.frame_id else "Odometry" - child_frame_id = msg.child_frame_id if msg.child_frame_id else "base_link" + # Translation from Pose + transform.transform.translation.x = msg.pose.position.x + transform.transform.translation.y = msg.pose.position.y + transform.transform.translation.z = msg.pose.position.z - # Broadcast the transformation - br = tf.TransformBroadcaster() - br.sendTransform( - (position.x, position.y, position.z), # Translation - (qx, qy, qz, qw), # Normalized rotation (quaternion) - rospy.Time.now(), # Timestamp - child_frame_id, # Child frame ID - parent_frame_id # Parent frame ID - ) + # Rotation (quaternion already in the pose message) + transform.transform.rotation = msg.pose.orientation -def main(): - rospy.init_node('odom_tf_publisher', anonymous=True) - rospy.Subscriber('/odom', Odometry, odom_callback) - rospy.loginfo("TF Publisher for /odom topic is running...") + # Broadcast the transform + broadcaster.sendTransform(transform) + + +def publish_transform_from_odom(): + rospy.init_node("odom_to_base_link_broadcaster") + + # Create a TransformBroadcaster object + broadcaster = tf2_ros.TransformBroadcaster() + + # Subscribe to /odom topic + rospy.Subscriber("/odom", PoseStamped, odom_callback, broadcaster) + + # Spin to keep the node running and processing callbacks rospy.spin() -if __name__ == '__main__': - main() + +if __name__ == "__main__": + try: + publish_transform_from_odom() + except rospy.ROSInterruptException: + pass