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

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