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

Do not republish unchanged scan matched point clouds. (#84) 

Before, republishing the point clouds at high frequency caused unnecessary
computational load.

This also fixes an issue seen in RViz:
After replaying a bag, point clouds with the same timestamp were continued to
be published. These were never dropped by the PointCloud2 display leading to
excessive memory consumption and bad visualization performance.
master
Wolfgang Hess 2016-10-06 15:11:17 +02:00 committed by GitHub
parent f4e89a070e
commit 1418902493
1 changed files with 13 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
cartographer_ros/src/cartographer_node_main.cc
View File

@ -166,6 +166,8 @@ class Node {
::ros::Publisher submap_list_publisher_;
::ros::ServiceServer submap_query_server_;
::ros::Publisher scan_matched_point_cloud_publisher_;
carto::common::Time last_scan_matched_point_cloud_time_ =
carto::common::Time::min();
::ros::ServiceServer finish_trajectory_server_;
tf2_ros::Buffer tf_buffer_;
@ -558,11 +560,17 @@ void Node::PublishPoseAndScanMatchedPointCloud(
LOG(WARNING) << ex.what();
}
scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message(
carto::common::ToUniversal(last_pose_estimate.time),
options_.tracking_frame, carto::sensor::TransformPointCloud(
last_pose_estimate.point_cloud,
tracking_to_local.inverse().cast<float>())));
// We only publish a point cloud if it has changed. It is not needed at high
// frequency, and republishing it would be computationally wasteful.
if (last_pose_estimate.time != last_scan_matched_point_cloud_time_) {
scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message(
carto::common::ToUniversal(last_pose_estimate.time),
options_.tracking_frame,
carto::sensor::TransformPointCloud(
last_pose_estimate.point_cloud,
tracking_to_local.inverse().cast<float>())));
last_scan_matched_point_cloud_time_ = last_pose_estimate.time;
}
}
void Node::SpinOccupancyGridThreadForever() {