CMakeLists.txt

@@ -52,7 +52,6 @@ target_link_libraries(odom_publisher ${catkin_LIBRARIES})
 catkin_install_python(PROGRAMS
   nodes/odom_publisher_python.py
   nodes/odom_publisher_linus.py
-  nodes/odom_publisher_fromTwist.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 

launch/rsp.launch

@@ -9,5 +9,5 @@
     <!-- Start the joint state publisher node -->
     <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" output="screen"/>
 
-    <!-- <node name="odom_publisher_fromTwist" pkg="cps_rmp220_support" type="odom_publisher_fromTwist.py" /> -->
+    <!-- <node pkg="tf" type="static_transform_publisher" name="odom_to_base_link" args="0 0 0 0 0 0 odom base_link 40" /> -->
 </launch>

nodes/odom_publisher.cpp

@@ -1,94 +1,57 @@
 #include <ros/ros.h>
 #include <tf/transform_broadcaster.h>
 #include <nav_msgs/Odometry.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <signal.h>
 
-double vx = 0.0; // Linear velocity
+volatile sig_atomic_t flag = 0;
-double vth = 0.0; // Angular velocity
 
-void odomCallback(const nav_msgs::Odometry::ConstPtr& msg) {
+void signal_handler(int signum) {
-  // Extract linear and angular velocities
+  flag = 1;
-  vx = msg->twist.twist.linear.x;
+}
-  vth = msg->twist.twist.angular.z;
+
+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;
+
+  // 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;
+
+  // Publish the transform
+  odom_broadcaster.sendTransform(odom_trans);
 }
 
 int main(int argc, char** argv) {
-  ros::init(argc, argv, "odometry_publisher");
+  ros::init(argc, argv, "tf_broadcaster");
+  ros::NodeHandle nh;
 
-  ros::NodeHandle n;
+  // Register the signal handler
-
+  signal(SIGINT, signal_handler);
-  // Subscriber for /odom topic
-  ros::Subscriber odom_sub = n.subscribe("odom", 50, odomCallback);
-
-  // Publisher for odom topic
-  ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom_out", 50);
 
+  // Create the transform broadcaster
   tf::TransformBroadcaster odom_broadcaster;
 
-  double x = 0.0;
+  // Subscribe to the /odom topic
-  double y = 0.0;
+  ros::Subscriber odom_sub = nh.subscribe<nav_msgs::Odometry>(
-  double th = 0.0;
+      "/odom", 10,
+      boost::bind(odomCallback, _1, boost::ref(odom_broadcaster)));
 
-  ros::Time current_time, last_time;
+  ROS_INFO("TF broadcaster node running...");
-  current_time = ros::Time::now();
-  last_time = ros::Time::now();
 
-  ros::Rate r(50.0); // Update rate: 50 Hz
+  // Spin until the node is stopped
-
+  ros::Rate rate(10.0); // Adjust rate if necessary
-  while (ros::ok()) {
+  while (ros::ok() && !flag) {
-    ros::spinOnce();  // Check for incoming messages
+    ros::spinOnce();
-    current_time = ros::Time::now();
+    rate.sleep();
-
-    // Compute odometry based on velocities
-    double dt = (current_time - last_time).toSec();
-    double delta_x = vx * cos(th) * dt;
-    double delta_y = vx * sin(th) * dt;
-    double delta_th = vth * dt;
-
-    x += delta_x;
-    y += delta_y;
-    th += delta_th;
-
-    // Normalize the yaw angle to [-π, π]
-    th = fmod(th + M_PI, 2 * M_PI);
-    if (th < 0) th += 2 * M_PI;
-    th -= M_PI;
-
-    // Create quaternion from yaw
-    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
-
-    // Publish the transform over tf
-    geometry_msgs::TransformStamped odom_trans;
-    odom_trans.header.stamp = current_time;
-    odom_trans.header.frame_id = "odom";
-    odom_trans.child_frame_id = "base_link";
-
-    odom_trans.transform.translation.x = x;
-    odom_trans.transform.translation.y = y;
-    odom_trans.transform.translation.z = 0.0;
-    odom_trans.transform.rotation = odom_quat;
-
-    odom_broadcaster.sendTransform(odom_trans);
-
-    // Publish the odometry message
-    nav_msgs::Odometry odom;
-    odom.header.stamp = current_time;
-    odom.header.frame_id = "odom";
-
-    // Set the position
-    odom.pose.pose.position.x = x;
-    odom.pose.pose.position.y = y;
-    odom.pose.pose.position.z = 0.0;
-    odom.pose.pose.orientation = odom_quat;
-
-    // Set the velocity
-    odom.child_frame_id = "base_link";
-    odom.twist.twist.linear.x = vx;
-    odom.twist.twist.linear.y = 0.0; // No lateral velocity
-    odom.twist.twist.angular.z = vth;
-
-    odom_pub.publish(odom);
-
-    last_time = current_time;
-    r.sleep();
   }
+
+  ROS_INFO("TF broadcaster node shutting down.");
+  ros::shutdown();
+  return 0;
 }

nodes/odom_publisher_fromTwist.py

@@ -1,80 +0,0 @@
-#!/usr/bin/env python
-
-import rospy
-from nav_msgs.msg import Odometry
-from geometry_msgs.msg import TransformStamped
-import tf2_ros
-import math
-from tf.transformations import quaternion_from_euler  # Import the function
-
-# Global variables for cumulative pose
-x = 0.0
-y = 0.0
-theta = 0.0  # Orientation in radians
-
-def odom_callback(msg):
-    global x, y, theta, br
-
-    # Time interval
-    current_time = rospy.Time.now()
-    dt = (current_time - odom_callback.last_time).to_sec()
-    odom_callback.last_time = current_time
-
-    # Extract linear and angular velocities
-    linear_x = msg.twist.twist.linear.x
-    linear_y = msg.twist.twist.linear.y
-    angular_z = msg.twist.twist.angular.z
-
-    # Compute the change in position and orientation
-    delta_x = (linear_x * math.cos(theta) - linear_y * math.sin(theta)) * dt
-    delta_y = (linear_x * math.sin(theta) + linear_y * math.cos(theta)) * dt
-    delta_theta = angular_z * dt
-
-    # Update cumulative position and orientation
-    x += delta_x
-    y += delta_y
-    theta += delta_theta
-
-    # Normalize theta to the range [-pi, pi]
-    theta = (theta + math.pi) % (2 * math.pi) - math.pi
-
-    # Create a TransformStamped message
-    t = TransformStamped()
-    t.header.stamp = current_time
-    t.header.frame_id = "odom"
-    t.child_frame_id = "base_link"
-
-    # Set translation
-    t.transform.translation.x = x
-    t.transform.translation.y = y
-    t.transform.translation.z = 0.0
-
-    # Set rotation (quaternion)
-    q = quaternion_from_euler(0, 0, theta)
-    t.transform.rotation.x = q[0]
-    t.transform.rotation.y = q[1]
-    t.transform.rotation.z = q[2]
-    t.transform.rotation.w = q[3]
-
-    # Broadcast the transform
-    br.sendTransform(t)
-
-def main():
-    global br
-
-    rospy.init_node("odom_to_base_link_tf_from_twist")
-    
-    # Initialize TransformBroadcaster
-    br = tf2_ros.TransformBroadcaster()
-    
-    # Initialize last time for integration
-    odom_callback.last_time = rospy.Time.now()
-    
-    # Subscribe to the /odom topic
-    rospy.Subscriber("/odom", Odometry, odom_callback)
-    
-    rospy.loginfo("Broadcasting /odom to /base_link transform using twist")
-    rospy.spin()
-
-if __name__ == "__main__":
-    main()

nodes/odom_publisher_linus.py

@@ -25,20 +25,6 @@ def odom_callback(msg):
     # Publish the transform
     tf_broadcaster.sendTransform(odom_trans)
 
-    # print debug info
-    rospy.loginfo("Position: x={}, y={}, z={}".format(
-        msg.pose.pose.position.x,
-        msg.pose.pose.position.y,
-        msg.pose.pose.position.z
-    ))
-
-    rospy.loginfo("Orientation: x={}, y={}, z={}, w={}".format(
-        msg.pose.pose.orientation.x,
-        msg.pose.pose.orientation.y,
-        msg.pose.pose.orientation.z,
-        msg.pose.pose.orientation.w
-    ))
-
 if __name__ == "__main__":
     rospy.init_node("tf2_broadcaster")
 

nodes/odom_publisher_python.py

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