update

nodes/odom_publisher.cpp

@@ -1,7 +1,6 @@
 #include <ros/ros.h>
 #include <tf/transform_broadcaster.h>
 #include <nav_msgs/Odometry.h>
-#include <geometry_msgs/Quaternion.h>
 #include <geometry_msgs/TransformStamped.h>
 #include <signal.h>
 
@@ -11,80 +10,51 @@ void signal_handler(int signum) {
   flag = 1;
 }
 
+class OdomToBaseLinkPublisher {
+public:
+  OdomToBaseLinkPublisher() : nh_() {
+    // Subscribe to the /odom topic
+    odom_sub_ = nh_.subscribe("/odom", 10, &OdomToBaseLinkPublisher::odomCallback, this);
+  }
+
+  void odomCallback(const nav_msgs::Odometry::ConstPtr& msg) {
+    // Prepare the transform message
+    geometry_msgs::TransformStamped odom_trans;
+    odom_trans.header.stamp = msg->header.stamp;
+    odom_trans.header.frame_id = msg->header.frame_id; // Should be "odom"
+    odom_trans.child_frame_id = msg->child_frame_id;   // Should be "base_link"
+
+    // 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);
+  }
+
+private:
+  ros::NodeHandle nh_;
+  ros::Subscriber odom_sub_;
+  tf::TransformBroadcaster odom_broadcaster_;
+};
+
 int main(int argc, char** argv) {
-  ros::init(argc, argv, "odometry_publisher");
+  ros::init(argc, argv, "odom_to_base_link_publisher");
 
   // Register the signal handler
   signal(SIGINT, signal_handler);
 
-  ros::NodeHandle n;
+  OdomToBaseLinkPublisher publisher;
-  ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
-  tf::TransformBroadcaster odom_broadcaster;
 
-  double x = 0.0;
+  ros::Rate r(50); // Set loop rate
-  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 (ros::ok() && !flag) {
-    ros::spinOnce();               // check for incoming messages
+    ros::spinOnce(); // Handle callbacks
-    current_time = ros::Time::now();
-
-    // Compute odometry
-    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;
-    double delta_th = vth * dt;
-
-    x += delta_x;
-    y += delta_y;
-    th += delta_th;
-
-    // Create quaternion for orientation
-    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
-
-    // Publish 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 odometry message
-    nav_msgs::Odometry odom;
-    odom.header.stamp = current_time;
-    odom.header.frame_id = "odom";
-
-    // Set position and velocity
-    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;
-
-    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();
   }
 
-  ROS_INFO("Odometry node shutting down.");
+  ROS_INFO("Odometry to base_link transform node shutting down.");
   ros::shutdown();
   return 0;
 }