update

nodes/odom_publisher_python.py

@@ -1,43 +1,47 @@
 #!/usr/bin/env python
 
 import rospy
-from nav_msgs.msg import Odometry
+import tf2_ros
-import tf
+from geometry_msgs.msg import TransformStamped, PoseStamped
-import math
+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):
+def odom_callback(msg, broadcaster):
-    # Extract position and orientation from the odometry message
+    # Create a TransformStamped object
-    position = msg.pose.pose.position
+    transform = TransformStamped()
-    orientation = msg.pose.pose.orientation
 
-    # Normalize the quaternion
+    # Header
-    qx, qy, qz, qw = normalize_quaternion(
+    transform.header.stamp = rospy.Time.now()
-        orientation.x, orientation.y, orientation.z, orientation.w
+    transform.header.frame_id = "odom"  # Parent frame
-    )
+    transform.child_frame_id = "base_link"  # Child frame
 
-    # Get frame IDs from the message
+    # Translation from Pose
-    parent_frame_id = msg.header.frame_id if msg.header.frame_id else "Odometry"
+    transform.transform.translation.x = msg.pose.position.x
-    child_frame_id = msg.child_frame_id if msg.child_frame_id else "base_link"
+    transform.transform.translation.y = msg.pose.position.y
+    transform.transform.translation.z = msg.pose.position.z
 
-    # Broadcast the transformation
+    # Rotation (quaternion already in the pose message)
-    br = tf.TransformBroadcaster()
+    transform.transform.rotation = msg.pose.orientation
-    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
-    )
 
-def main():
+    # Broadcast the transform
-    rospy.init_node('odom_tf_publisher', anonymous=True)
+    broadcaster.sendTransform(transform)
-    rospy.Subscriber('/odom', Odometry, odom_callback)
+
-    rospy.loginfo("TF Publisher for /odom topic is running...")
+
+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