diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 3354729..067ab5a 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -261,6 +261,15 @@ void Node::PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event) { node_options_.use_pose_extrapolator ? ToRos(now) : ToRos(trajectory_data.local_slam_data->time); + + // Suppress publishing if we already published a transform at this time. + // Due to 2020-07 changes to geometry2, tf buffer will issue warnings for + // repeated transforms with the same timestamp. + if (last_published_tf_stamps_.count(entry.first) && + last_published_tf_stamps_[entry.first] == stamped_transform.header.stamp) + continue; + last_published_tf_stamps_[entry.first] = stamped_transform.header.stamp; + const Rigid3d tracking_to_local_3d = node_options_.use_pose_extrapolator ? extrapolator.ExtrapolatePose(now) diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index b02ca96..0f7bead 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -214,6 +214,7 @@ class Node { // These are keyed with 'trajectory_id'. std::map extrapolators_; + std::map last_published_tf_stamps_; std::unordered_map sensor_samplers_; std::unordered_map> subscribers_; std::unordered_set subscribed_topics_;