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

---
 nodes/odom_publisher.cpp | 32 +++++++++++++-------------------
 1 file changed, 13 insertions(+), 19 deletions(-)

diff --git a/nodes/odom_publisher.cpp b/nodes/odom_publisher.cpp
index 16e13d1..23a2382 100644
--- a/nodes/odom_publisher.cpp
+++ b/nodes/odom_publisher.cpp
@@ -2,14 +2,12 @@
 #include <tf/transform_broadcaster.h>
 #include <nav_msgs/Odometry.h>
 
-double vx = 0.0;
-double vy = 0.0;
-double vth = 0.0;
+double vx = 0.0; // Linear velocity
+double vth = 0.0; // Angular velocity
 
 void odomCallback(const nav_msgs::Odometry::ConstPtr& msg) {
-  // Extract velocity data from the /odom topic
+  // Extract linear and angular velocities
   vx = msg->twist.twist.linear.x;
-  vy = msg->twist.twist.linear.y;
   vth = msg->twist.twist.angular.z;
 }
 
@@ -34,22 +32,27 @@ int main(int argc, char** argv) {
   current_time = ros::Time::now();
   last_time = ros::Time::now();
 
-  ros::Rate r(50.0); // Update rate increased to 50 Hz
+  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 subscribed velocity data
+    // 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_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);
 
@@ -79,13 +82,4 @@ int main(int argc, char** argv) {
 
     // 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;
-
-    odom_pub.publish(odom);
-
-    last_time = current_time;
-    r.sleep();
-  }
-}
+    odom.twist.twist