update

nodes/odom_publisher_linus.py

@@ -1,43 +1,57 @@
-#!/usr/bin/env python
+#include <ros/ros.h>
+#include <tf/transform_broadcaster.h>
+#include <nav_msgs/Odometry.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <signal.h>
 
-import rospy
-from nav_msgs.msg import Odometry
-import tf
-import math
+volatile sig_atomic_t flag = 0;
 
-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
+void signal_handler(int signum) {
+  flag = 1;
+}
 
-def odom_callback(msg):
-    # Extract position and orientation from the odometry message
-    position = msg.pose.pose.position
-    orientation = msg.pose.pose.orientation
+void odomCallback(const nav_msgs::Odometry::ConstPtr& msg, tf::TransformBroadcaster& odom_broadcaster) {
+  // Create transform message
+  geometry_msgs::TransformStamped odom_trans;
+  odom_trans.header.stamp = msg->header.stamp;
+  odom_trans.header.frame_id = msg->header.frame_id;
+  odom_trans.child_frame_id = msg->child_frame_id;
 
-    # Normalize the quaternion
-    qx, qy, qz, qw = normalize_quaternion(
-        orientation.x, orientation.y, orientation.z, orientation.w
-    )
+  // Copy translation and rotation
+  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;
+  odom_trans.transform.rotation = msg->pose.pose.orientation;
 
-    # Get frame IDs from the message
-    parent_frame_id = msg.header.frame_id if msg.header.frame_id else "odom"
-    child_frame_id = msg.child_frame_id if msg.child_frame_id else "base_link"
+  // Publish the transform
+  odom_broadcaster.sendTransform(odom_trans);
+}
 
-    # 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
-    )
+int main(int argc, char** argv) {
+  ros::init(argc, argv, "tf_broadcaster");
+  ros::NodeHandle nh;
 
-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()
+  // Register the signal handler
+  signal(SIGINT, signal_handler);
 
-if __name__ == '__main__':
-    main()
+  // Create the transform broadcaster
+  tf::TransformBroadcaster odom_broadcaster;
+
+  // Subscribe to the /odom topic
+  ros::Subscriber odom_sub = nh.subscribe<nav_msgs::Odometry>(
+      "/odom", 10,
+      boost::bind(odomCallback, _1, boost::ref(odom_broadcaster)));
+
+  ROS_INFO("TF broadcaster node running...");
+
+  // Spin until the node is stopped
+  ros::Rate rate(10.0); // Adjust rate if necessary
+  while (ros::ok() && !flag) {
+    ros::spinOnce();
+    rate.sleep();
+  }
+
+  ROS_INFO("TF broadcaster node shutting down.");
+  ros::shutdown();
+  return 0;
+}