Use LocalSlamResultCallback for trajectory states (#594)

Replaces #556.
Use `LocalSlamResultCallback` googlecartographer/cartographer#574 instead of `PoseEstimate`.

cartographer_ros/cartographer_ros/map_builder_bridge.cc

@@ -65,7 +65,26 @@ void PushAndResetLineMarker(visualization_msgs::Marker* marker,
 MapBuilderBridge::MapBuilderBridge(const NodeOptions& node_options,
                                    tf2_ros::Buffer* const tf_buffer)
     : node_options_(node_options),
-      map_builder_(node_options.map_builder_options, {}),
+      map_builder_(
+          node_options.map_builder_options,
+          cartographer::mapping::MapBuilder::LocalSlamResultCallback(
+              [this](
+                  const int trajectory_id,
+                  const ::cartographer::common::Time time,
+                  const ::cartographer::transform::Rigid3d local_pose,
+                  ::cartographer::sensor::RangeData range_data_in_local,
+                  const std::unique_ptr<const ::cartographer::mapping::NodeId>)
+                  EXCLUDES(mutex_) {
+                    std::shared_ptr<const TrajectoryState::LocalSlamData>
+                        local_slam_data =
+                            std::make_shared<TrajectoryState::LocalSlamData>(
+                                TrajectoryState::LocalSlamData{
+                                    time, local_pose,
+                                    std::move(range_data_in_local)});
+                    cartographer::common::MutexLocker lock(&mutex_);
+                    trajectory_state_data_[trajectory_id] =
+                        std::move(local_slam_data);
+                  })),
       tf_buffer_(tf_buffer) {}
 
 void MapBuilderBridge::LoadMap(const std::string& map_filename) {
@@ -169,24 +188,22 @@ MapBuilderBridge::GetTrajectoryStates() {
     const int trajectory_id = entry.first;
     const SensorBridge& sensor_bridge = *entry.second;
 
-    const cartographer::mapping::TrajectoryBuilder* const trajectory_builder =
+    std::shared_ptr<const TrajectoryState::LocalSlamData> local_slam_data;
-        map_builder_.GetTrajectoryBuilder(trajectory_id);
+    {
-    if (trajectory_builder == nullptr) {
+      cartographer::common::MutexLocker lock(&mutex_);
+      if (trajectory_state_data_.count(trajectory_id) == 0) {
         continue;
       }
-    const cartographer::mapping::TrajectoryBuilder::PoseEstimate pose_estimate =
+      local_slam_data = trajectory_state_data_.at(trajectory_id);
-        trajectory_builder->pose_estimate();
-    if (cartographer::common::ToUniversal(pose_estimate.time) < 0) {
-      continue;
     }
 
     // Make sure there is a trajectory with 'trajectory_id'.
     CHECK_EQ(trajectory_options_.count(trajectory_id), 1);
     trajectory_states[trajectory_id] = {
-        pose_estimate,
+        local_slam_data,
         map_builder_.pose_graph()->GetLocalToGlobalTransform(trajectory_id),
         sensor_bridge.tf_bridge().LookupToTracking(
-            pose_estimate.time,
+            local_slam_data->time,
             trajectory_options_[trajectory_id].published_frame),
         trajectory_options_[trajectory_id]};
   }

cartographer_ros/cartographer_ros/map_builder_bridge.h

@@ -39,7 +39,15 @@ namespace cartographer_ros {
 class MapBuilderBridge {
  public:
   struct TrajectoryState {
-    cartographer::mapping::TrajectoryBuilder::PoseEstimate pose_estimate;
+    // Contains the trajectory state data received from local SLAM, after
+    // it had processed accumulated 'range_data_in_local' and estimated
+    // current 'local_pose' at 'time'.
+    struct LocalSlamData {
+      ::cartographer::common::Time time;
+      ::cartographer::transform::Rigid3d local_pose;
+      ::cartographer::sensor::RangeData range_data_in_local;
+    };
+    std::shared_ptr<const LocalSlamData> local_slam_data;
     cartographer::transform::Rigid3d local_to_map;
     std::unique_ptr<cartographer::transform::Rigid3d> published_to_tracking;
     TrajectoryOptions trajectory_options;
@@ -62,14 +70,18 @@ class MapBuilderBridge {
       cartographer_ros_msgs::SubmapQuery::Response& response);
 
   cartographer_ros_msgs::SubmapList GetSubmapList();
-  std::unordered_map<int, TrajectoryState> GetTrajectoryStates();
+  std::unordered_map<int, TrajectoryState> GetTrajectoryStates()
+      EXCLUDES(mutex_);
   visualization_msgs::MarkerArray GetTrajectoryNodeList();
   visualization_msgs::MarkerArray GetConstraintList();
 
   SensorBridge* sensor_bridge(int trajectory_id);
 
  private:
+  cartographer::common::Mutex mutex_;
   const NodeOptions node_options_;
+  std::unordered_map<int, std::shared_ptr<const TrajectoryState::LocalSlamData>>
+      trajectory_state_data_ GUARDED_BY(mutex_);
   cartographer::mapping::MapBuilder map_builder_;
   tf2_ros::Buffer* const tf_buffer_;
 

cartographer_ros/cartographer_ros/node.cc

@@ -169,23 +169,25 @@ void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) {
     auto& extrapolator = extrapolators_.at(entry.first);
     // 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 (trajectory_state.pose_estimate.time != extrapolator.GetLastPoseTime()) {
+    if (trajectory_state.local_slam_data->time !=
+        extrapolator.GetLastPoseTime()) {
       // TODO(gaschler): Consider using other message without time information.
       carto::sensor::TimedPointCloud point_cloud;
-      point_cloud.reserve(trajectory_state.pose_estimate.point_cloud.size());
+      point_cloud.reserve(
+          trajectory_state.local_slam_data->range_data_in_local.returns.size());
       for (const Eigen::Vector3f point :
-           trajectory_state.pose_estimate.point_cloud) {
+           trajectory_state.local_slam_data->range_data_in_local.returns) {
         Eigen::Vector4f point_time;
         point_time << point, 0.f;
         point_cloud.push_back(point_time);
       }
       scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message(
-          carto::common::ToUniversal(trajectory_state.pose_estimate.time),
+          carto::common::ToUniversal(trajectory_state.local_slam_data->time),
           node_options_.map_frame,
           carto::sensor::TransformTimedPointCloud(
               point_cloud, trajectory_state.local_to_map.cast<float>())));
-      extrapolator.AddPose(trajectory_state.pose_estimate.time,
+      extrapolator.AddPose(trajectory_state.local_slam_data->time,
-                           trajectory_state.pose_estimate.pose);
+                           trajectory_state.local_slam_data->local_pose);
     }
 
     geometry_msgs::TransformStamped stamped_transform;