update

CMakeLists.txt

@@ -47,6 +47,14 @@ include_directories(
 add_executable(odom_publisher nodes/odom_publisher.cpp)
 target_link_libraries(odom_publisher ${catkin_LIBRARIES})
 
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+catkin_install_python(PROGRAMS
+  nodes/odom_publisher_python.py
+  nodes/odom_publisher_linus.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
 ################################################################################
 # Install
 ################################################################################

nodes/odom_publisher_linus.py

@@ -0,0 +1,40 @@
+#!/usr/bin/env python
+
+import rospy
+import tf2_ros
+from nav_msgs.msg import Odometry
+from geometry_msgs.msg import TransformStamped
+
+def odom_callback(msg):
+    # Create a TransformStamped message
+    odom_trans = TransformStamped()
+    odom_trans.header.stamp = msg.header.stamp
+    odom_trans.header.frame_id = msg.header.frame_id
+
+    # Set child_frame_id explicitly (e.g., base_link)
+    odom_trans.child_frame_id = "base_link"
+
+    # Set the translation
+    odom_trans.transform.translation.x = msg.pose.pose.position.x
+    odom_trans.transform.translation.y = msg.pose.pose.position.y
+    odom_trans.transform.translation.z = msg.pose.pose.position.z
+
+    # Set the rotation
+    odom_trans.transform.rotation = msg.pose.pose.orientation
+
+    # Publish the transform
+    tf_broadcaster.sendTransform(odom_trans)
+
+if __name__ == "__main__":
+    rospy.init_node("tf2_broadcaster")
+
+    # Create a TransformBroadcaster
+    tf_broadcaster = tf2_ros.TransformBroadcaster()
+
+    # Subscribe to the /odom topic
+    rospy.Subscriber("/odom", Odometry, odom_callback)
+
+    rospy.loginfo("TF2 broadcaster node is running...")
+
+    # Keep the node running
+    rospy.spin()

nodes/odom_publisher_python.py

@@ -0,0 +1,43 @@
+#!/usr/bin/env python
+
+import rospy
+from nav_msgs.msg import Odometry
+import tf
+import math
+
+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
+
+    # Normalize the quaternion
+    qx, qy, qz, qw = normalize_quaternion(
+        orientation.x, orientation.y, orientation.z, orientation.w
+    )
+
+    # 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"
+
+    # 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
+    )
+
+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...")
+    rospy.spin()
+
+if __name__ == '__main__':
+    main()