From 9fd47a2a559e71da8df3fcf833e667d187ccc7be Mon Sep 17 00:00:00 2001
From: Your Name <you@example.com>
Date: Wed, 15 Jan 2025 13:33:55 +0100
Subject: [PATCH] update

---
 nodes/odom_publisher.cpp | 47 +++++++++++++++++++++++++---------------
 1 file changed, 29 insertions(+), 18 deletions(-)

diff --git a/nodes/odom_publisher.cpp b/nodes/odom_publisher.cpp
index 6032ff4..ca8c7bc 100644
--- a/nodes/odom_publisher.cpp
+++ b/nodes/odom_publisher.cpp
@@ -2,32 +2,45 @@
 #include <tf/transform_broadcaster.h>
 #include <nav_msgs/Odometry.h>
 
-int main(int argc, char** argv){
+double vx = 0.0;
+double vy = 0.0;
+double vth = 0.0;
+
+void odomCallback(const nav_msgs::Odometry::ConstPtr& msg) {
+  // Extract velocity data from the /odom topic
+  vx = msg->twist.twist.linear.x;
+  vy = msg->twist.twist.linear.y;
+  vth = msg->twist.twist.angular.z;
+}
+
+int main(int argc, char** argv) {
   ros::init(argc, argv, "odometry_publisher");
 
   ros::NodeHandle n;
-  ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
+
+  // 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);
+
   tf::TransformBroadcaster odom_broadcaster;
 
   double x = 0.0;
   double y = 0.0;
   double th = 0.0;
 
-  double vx = 0.1;
-  double vy = -0.1;
-  double vth = 0.1;
-
   ros::Time current_time, last_time;
   current_time = ros::Time::now();
   last_time = ros::Time::now();
 
-  ros::Rate r(1.0);
-  while(n.ok()){
+  ros::Rate r(50.0); // Update rate increased to 50 Hz
 
-    ros::spinOnce();               // check for incoming messages
+  while (ros::ok()) {
+    ros::spinOnce();  // Check for incoming messages
     current_time = ros::Time::now();
 
-    //compute odometry in a typical way given the velocities of the robot
+    // Compute odometry based on subscribed velocity data
     double dt = (current_time - last_time).toSec();
     double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
     double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
@@ -37,10 +50,10 @@ int main(int argc, char** argv){
     y += delta_y;
     th += delta_th;
 
-    //since all odometry is 6DOF we'll need a quaternion created from yaw
+    // Create quaternion from yaw
     geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
 
-    //first, we'll publish the transform over tf
+    // Publish the transform over tf
     geometry_msgs::TransformStamped odom_trans;
     odom_trans.header.stamp = current_time;
     odom_trans.header.frame_id = "odom";
@@ -51,30 +64,28 @@ int main(int argc, char** argv){
     odom_trans.transform.translation.z = 0.0;
     odom_trans.transform.rotation = odom_quat;
 
-    //send the transform
     odom_broadcaster.sendTransform(odom_trans);
 
-    //next, we'll publish the odometry message over ROS
+    // Publish the odometry message
     nav_msgs::Odometry odom;
     odom.header.stamp = current_time;
     odom.header.frame_id = "odom";
 
-    //set the position
+    // 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
+    // Set the velocity
     odom.child_frame_id = "base_link";
     odom.twist.twist.linear.x = vx;
     odom.twist.twist.linear.y = vy;
     odom.twist.twist.angular.z = vth;
 
-    //publish the message
     odom_pub.publish(odom);
 
     last_time = current_time;
     r.sleep();
   }
-}
\ No newline at end of file
+}