From 1418902493115cad90d6ee93877c5a602a78614f Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Thu, 6 Oct 2016 15:11:17 +0200 Subject: [PATCH] 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. --- cartographer_ros/src/cartographer_node_main.cc | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/cartographer_ros/src/cartographer_node_main.cc b/cartographer_ros/src/cartographer_node_main.cc index d1429a5..7edcae8 100644 --- a/cartographer_ros/src/cartographer_node_main.cc +++ b/cartographer_ros/src/cartographer_node_main.cc @@ -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()))); + // 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()))); + last_scan_matched_point_cloud_time_ = last_pose_estimate.time; + } } void Node::SpinOccupancyGridThreadForever() {