Check if we already publihed transformation at same timestamp Fix car…

…tographer-project#1555 (cartographer-project#1556) Signed-off-by: stribor14 <[email protected]> Co-authored-by: stribor14 <[email protected]>
wohe · Nov 13, 2020 · 0b66821 · 0b66821
1 parent 9020ba0
commit 0b66821
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 0 deletions.
diff --git 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
@@ -214,6 +214,7 @@ class Node {
 
   // These are keyed with 'trajectory_id'.
   std::map<int, ::cartographer::mapping::PoseExtrapolator> extrapolators_;
+  std::map<int, ::ros::Time> last_published_tf_stamps_;
   std::unordered_map<int, TrajectorySensorSamplers> sensor_samplers_;
   std::unordered_map<int, std::vector<Subscriber>> subscribers_;
   std::unordered_set<std::string> subscribed_topics_;