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() {