update

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();
   }
-}
+}