From cafb3d1d5c7cd9b40f1831c64be151e5c63dfe25 Mon Sep 17 00:00:00 2001 From: Yoshiki Date: Mon, 20 May 2024 20:13:30 +0900 Subject: [PATCH] Removed duplications of the odom tf publication --- .../include/robot_controllers/diff_drive_base.h | 2 ++ robot_controllers/src/diff_drive_base.cpp | 6 +++++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/robot_controllers/include/robot_controllers/diff_drive_base.h b/robot_controllers/include/robot_controllers/diff_drive_base.h index e8e3e21..ce675ee 100644 --- a/robot_controllers/include/robot_controllers/diff_drive_base.h +++ b/robot_controllers/include/robot_controllers/diff_drive_base.h @@ -183,6 +183,8 @@ class DiffDriveBaseController : public Controller boost::shared_ptr broadcaster_; bool publish_tf_; + // The time of the most recent published state + ros::Time last_published_tf_stamp_; bool enabled_; bool ready_; diff --git a/robot_controllers/src/diff_drive_base.cpp b/robot_controllers/src/diff_drive_base.cpp index c3c5dc0..931fb73 100644 --- a/robot_controllers/src/diff_drive_base.cpp +++ b/robot_controllers/src/diff_drive_base.cpp @@ -375,7 +375,8 @@ void DiffDriveBaseController::publishCallback(const ros::TimerEvent& event) msg.header.stamp = ros::Time::now(); odom_pub_.publish(msg); - if (publish_tf_) + // As of ROS Noetic, TF2 will issue warnings whenever we try to publish with the same time stamp. + if (publish_tf_ && msg.header.stamp > last_published_tf_stamp_) { tf::Transform transform; transform.setOrigin(tf::Vector3(msg.pose.pose.position.x, msg.pose.pose.position.y, 0.0)); @@ -389,6 +390,9 @@ void DiffDriveBaseController::publishCallback(const ros::TimerEvent& event) */ broadcaster_->sendTransform(tf::StampedTransform(transform, msg.header.stamp, msg.header.frame_id, msg.child_frame_id)); } + + // Retain the last published stamp for detecting repeatedtransforms in future cycles + last_published_tf_stamp_ = msg.header.stamp; } void DiffDriveBaseController::scanCallback(