diff --git a/nodes/odom_publisher.cpp b/nodes/odom_publisher.cpp index 3046e10..d8bc51d 100644 --- a/nodes/odom_publisher.cpp +++ b/nodes/odom_publisher.cpp @@ -10,51 +10,48 @@ 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, 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; - 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; - // 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_; -}; + // Publish the transform + odom_broadcaster.sendTransform(odom_trans); +} int main(int argc, char** argv) { - ros::init(argc, argv, "odom_to_base_link_publisher"); + ros::init(argc, argv, "tf_broadcaster"); + ros::NodeHandle nh; // Register the signal handler signal(SIGINT, signal_handler); - OdomToBaseLinkPublisher publisher; + // Create the transform broadcaster + tf::TransformBroadcaster odom_broadcaster; - ros::Rate r(50); // Set loop rate + // Subscribe to the /odom topic + ros::Subscriber odom_sub = nh.subscribe( + "/odom", 10, + boost::bind(odomCallback, _1, boost::ref(odom_broadcaster))); + + ROS_INFO("TF broadcaster node running..."); + + // Spin until the node is stopped + ros::Rate rate(10.0); // Adjust rate if necessary while (ros::ok() && !flag) { - ros::spinOnce(); // Handle callbacks - r.sleep(); + ros::spinOnce(); + rate.sleep(); } - ROS_INFO("Odometry to base_link transform node shutting down."); + ROS_INFO("TF broadcaster node shutting down."); ros::shutdown(); return 0; }