update

CMakeLists.txt

@@ -52,6 +52,7 @@ 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 pkg="tf" type="static_transform_publisher" name="odom_to_base_link" args="0 0 0 0 0 0 odom base_link 40" /> -->
+    <!-- <node name="odom_publisher_fromTwist" pkg="cps_rmp220_support" type="odom_publisher_fromTwist.py" /> -->
 </launch>

nodes/odom_publisher.cpp

@@ -1,57 +1,94 @@
 #include <ros/ros.h>
 #include <tf/transform_broadcaster.h>
 #include <nav_msgs/Odometry.h>
-#include <geometry_msgs/TransformStamped.h>
-#include <signal.h>
 
-volatile sig_atomic_t flag = 0;
+double vx = 0.0; // Linear velocity
+double vth = 0.0; // Angular velocity
 
-void signal_handler(int signum) {
-  flag = 1;
-}
-
-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);
+void odomCallback(const nav_msgs::Odometry::ConstPtr& msg) {
+  // Extract linear and angular velocities
+  vx = msg->twist.twist.linear.x;
+  vth = msg->twist.twist.angular.z;
 }
 
 int main(int argc, char** argv) {
-  ros::init(argc, argv, "tf_broadcaster");
-  ros::NodeHandle nh;
+  ros::init(argc, argv, "odometry_publisher");
 
-  // Register the signal handler
-  signal(SIGINT, signal_handler);
+  ros::NodeHandle n;
+
+  // 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;
 
-  // Subscribe to the /odom topic
-  ros::Subscriber odom_sub = nh.subscribe<nav_msgs::Odometry>(
-      "/odom", 10,
-      boost::bind(odomCallback, _1, boost::ref(odom_broadcaster)));
+  double x = 0.0;
+  double y = 0.0;
+  double th = 0.0;
 
-  ROS_INFO("TF broadcaster node running...");
+  ros::Time current_time, last_time;
+  current_time = ros::Time::now();
+  last_time = ros::Time::now();
 
-  // Spin until the node is stopped
-  ros::Rate rate(10.0); // Adjust rate if necessary
-  while (ros::ok() && !flag) {
-    ros::spinOnce();
-    rate.sleep();
+  ros::Rate r(50.0); // Update rate: 50 Hz
+
+  while (ros::ok()) {
+    ros::spinOnce();  // Check for incoming messages
+    current_time = ros::Time::now();
+
+    // 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

@@ -0,0 +1,80 @@
+#!/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,6 +25,20 @@ 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,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