12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Check if we already publihed transformation at same timestamp Fix #1555 (#1556) 

Signed-off-by: stribor14 <mirko.kokot@romb-technologies.hr>

Co-authored-by: stribor14 <mirko.kokot@romb-technologies.hr>
master
stribor14 2020-11-13 13:22:31 +01:00 committed by GitHub
parent 9020ba0247
commit 0b66821840
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 10 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
cartographer_ros/cartographer_ros/node.cc
View File

@ -261,6 +261,15 @@ void Node::PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event) {
node_options_.use_pose_extrapolator node_options_.use_pose_extrapolator
? ToRos(now) ? ToRos(now)
: ToRos(trajectory_data.local_slam_data->time); : 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 = const Rigid3d tracking_to_local_3d =
node_options_.use_pose_extrapolator node_options_.use_pose_extrapolator
? extrapolator.ExtrapolatePose(now) ? extrapolator.ExtrapolatePose(now)

1
cartographer_ros/cartographer_ros/node.h
View File

@ -214,6 +214,7 @@ class Node {
// These are keyed with 'trajectory_id'. // These are keyed with 'trajectory_id'.
std::map<int, ::cartographer::mapping::PoseExtrapolator> extrapolators_; 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, TrajectorySensorSamplers> sensor_samplers_;
std::unordered_map<int, std::vector<Subscriber>> subscribers_; std::unordered_map<int, std::vector<Subscriber>> subscribers_;
std::unordered_set<std::string> subscribed_topics_; std::unordered_set<std::string> subscribed_topics_;