Add InsertionResult to LocalSlamResult. (#801)

cartographer/internal/mapping/global_trajectory_builder.h

@@ -58,18 +58,23 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {
       // The range data has not been fully accumulated yet.
       return;
     }
-    std::unique_ptr<mapping::NodeId> node_id;
+    std::unique_ptr<InsertionResult> insertion_result;
     if (matching_result->insertion_result != nullptr) {
-      node_id = ::cartographer::common::make_unique<mapping::NodeId>(
+      auto node_id = pose_graph_->AddNode(
-          pose_graph_->AddNode(
           matching_result->insertion_result->constant_data, trajectory_id_,
-              matching_result->insertion_result->insertion_submaps));
+          matching_result->insertion_result->insertion_submaps);
-      CHECK_EQ(node_id->trajectory_id, trajectory_id_);
+      CHECK_EQ(node_id.trajectory_id, trajectory_id_);
+      insertion_result = common::make_unique<InsertionResult>(InsertionResult{
+          node_id, matching_result->insertion_result->constant_data,
+          std::vector<std::shared_ptr<const Submap>>(
+              matching_result->insertion_result->insertion_submaps.begin(),
+              matching_result->insertion_result->insertion_submaps.end())});
     }
     if (local_slam_result_callback_) {
       local_slam_result_callback_(
           trajectory_id_, matching_result->time, matching_result->local_pose,
-          std::move(matching_result->range_data_in_local), std::move(node_id));
+          std::move(matching_result->range_data_in_local),
+          std::move(insertion_result));
     }
   }
 

cartographer/mapping/map_builder_test.cc

@@ -81,7 +81,9 @@ class MapBuilderTest : public ::testing::Test {
     return [=](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>) {
+               const std::unique_ptr<
+                   const cartographer::mapping::TrajectoryBuilderInterface::
+                       InsertionResult>) {
       local_slam_result_poses_.push_back(local_pose);
     };
   }

cartographer/mapping/trajectory_builder_interface.h

@@ -44,6 +44,12 @@ proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions(
 // optimized pose estimates.
 class TrajectoryBuilderInterface {
  public:
+  struct InsertionResult {
+    NodeId node_id;
+    std::shared_ptr<const TrajectoryNode::Data> constant_data;
+    std::vector<std::shared_ptr<const Submap>> insertion_submaps;
+  };
+
   // A callback which is called after local SLAM processes an accumulated
   // 'sensor::RangeData'. If the data was inserted into a submap, reports the
   // assigned 'NodeId', otherwise 'nullptr' if the data was filtered out.
@@ -51,7 +57,7 @@ class TrajectoryBuilderInterface {
       std::function<void(int /* trajectory ID */, common::Time,
                          transform::Rigid3d /* local pose estimate */,
                          sensor::RangeData /* in local frame */,
-                         std::unique_ptr<const mapping::NodeId>)>;
+                         std::unique_ptr<const InsertionResult>)>;
 
   TrajectoryBuilderInterface() {}
   virtual ~TrajectoryBuilderInterface() {}

cartographer_grpc/client_server_test.cc

@@ -111,10 +111,12 @@ class ClientServerTest : public ::testing::Test {
         cartographer::mapping::CreateTrajectoryBuilderOptions(
             trajectory_builder_parameters.get());
     local_slam_result_callback_ =
-        [this](int, cartographer::common::Time,
+        [this](
+            int, cartographer::common::Time,
             cartographer::transform::Rigid3d local_pose,
             cartographer::sensor::RangeData,
-               std::unique_ptr<const cartographer::mapping::NodeId>) {
+            std::unique_ptr<const cartographer::mapping::
+                                TrajectoryBuilderInterface::InsertionResult>) {
           std::unique_lock<std::mutex> lock(local_slam_result_mutex_);
           local_slam_result_poses_.push_back(local_pose);
           lock.unlock();

cartographer_grpc/handlers/receive_local_slam_results_handler.h

@@ -68,11 +68,9 @@ class ReceiveLocalSlamResultsHandler
       *response->mutable_range_data() =
           cartographer::sensor::ToProto(*local_slam_result->range_data);
     }
-    if (local_slam_result->node_id) {
+    if (local_slam_result->insertion_result) {
-      response->mutable_node_id()->set_trajectory_id(
+      local_slam_result->insertion_result->node_id.ToProto(
-          local_slam_result->node_id->trajectory_id);
+          response->mutable_insertion_result()->mutable_node_id());
-      response->mutable_node_id()->set_node_index(
-          local_slam_result->node_id->node_index);
     }
     return response;
   }

cartographer_grpc/map_builder_server.cc

@@ -62,10 +62,12 @@ MapBuilderServer::MapBuilderContext::
              int trajectory_id, cartographer::common::Time time,
              cartographer::transform::Rigid3d local_pose,
              cartographer::sensor::RangeData range_data,
-             std::unique_ptr<const cartographer::mapping::NodeId> node_id) {
+             std::unique_ptr<const cartographer::mapping::
+                                 TrajectoryBuilderInterface::InsertionResult>
+                 insertion_result) {
     map_builder_server->OnLocalSlamResult(trajectory_id, time, local_pose,
                                           std::move(range_data),
-                                          std::move(node_id));
+                                          std::move(insertion_result));
   };
 }
 
@@ -184,19 +186,23 @@ void MapBuilderServer::OnLocalSlamResult(
     int trajectory_id, cartographer::common::Time time,
     cartographer::transform::Rigid3d local_pose,
     cartographer::sensor::RangeData range_data,
-    std::unique_ptr<const cartographer::mapping::NodeId> node_id) {
+    std::unique_ptr<const cartographer::mapping::TrajectoryBuilderInterface::
+                        InsertionResult>
+        insertion_result) {
   auto shared_range_data =
       std::make_shared<cartographer::sensor::RangeData>(std::move(range_data));
   cartographer::common::MutexLocker locker(&local_slam_subscriptions_lock_);
   for (auto& entry : local_slam_subscriptions_[trajectory_id]) {
-    auto copy_of_node_id =
+    auto copy_of_insertion_result =
-        node_id ? cartographer::common::make_unique<
+        insertion_result
-                      const cartographer::mapping::NodeId>(*node_id)
+            ? cartographer::common::make_unique<
+                  const cartographer::mapping::TrajectoryBuilderInterface::
+                      InsertionResult>(*insertion_result)
             : nullptr;
     LocalSlamSubscriptionCallback callback = entry.second;
     callback(cartographer::common::make_unique<LocalSlamResult>(
         LocalSlamResult{trajectory_id, time, local_pose, shared_range_data,
-                        std::move(copy_of_node_id)}));
+                        std::move(copy_of_insertion_result)}));
   }
 }
 

cartographer_grpc/map_builder_server.h

@@ -35,7 +35,9 @@ class MapBuilderServer {
     cartographer::common::Time time;
     cartographer::transform::Rigid3d local_pose;
     std::shared_ptr<const cartographer::sensor::RangeData> range_data;
-    std::unique_ptr<const cartographer::mapping::NodeId> node_id;
+    std::unique_ptr<const cartographer::mapping::TrajectoryBuilderInterface::
+                        InsertionResult>
+        insertion_result;
   };
   // Calling with 'nullptr' signals subscribers that the subscription has ended.
   using LocalSlamSubscriptionCallback =
@@ -106,7 +108,9 @@ class MapBuilderServer {
       int trajectory_id, cartographer::common::Time time,
       cartographer::transform::Rigid3d local_pose,
       cartographer::sensor::RangeData range_data,
-      std::unique_ptr<const cartographer::mapping::NodeId> node_id);
+      std::unique_ptr<const cartographer::mapping::TrajectoryBuilderInterface::
+                          InsertionResult>
+          insertion_result);
   SubscriptionId SubscribeLocalSlamResults(
       int trajectory_id, LocalSlamSubscriptionCallback callback);
   void UnsubscribeLocalSlamResults(const SubscriptionId& subscription_id);

cartographer_grpc/mapping/trajectory_builder_stub.cc

@@ -144,15 +144,15 @@ void TrajectoryBuilderStub::RunLocalSlamResultReader(
         cartographer::transform::ToRigid3(response.local_pose());
     cartographer::sensor::RangeData range_data =
         cartographer::sensor::FromProto(response.range_data());
-    auto node_id =
+    auto insertion_result =
-        response.has_node_id()
+        response.has_insertion_result()
-            ? cartographer::common::make_unique<cartographer::mapping::NodeId>(
+            ? cartographer::common::make_unique<InsertionResult>(
-                  cartographer::mapping::NodeId{
+                  InsertionResult{cartographer::mapping::NodeId{
-                      response.node_id().trajectory_id(),
+                      response.insertion_result().node_id().trajectory_id(),
-                      response.node_id().node_index()})
+                      response.insertion_result().node_id().node_index()}})
             : nullptr;
     local_slam_result_callback(trajectory_id, time, local_pose, range_data,
-                               std::move(node_id));
+                               std::move(insertion_result));
   }
   client_reader->Finish();
 }

cartographer_grpc/proto/map_builder_service.proto

@@ -66,12 +66,16 @@ message ReceiveLocalSlamResultsRequest {
   int32 trajectory_id = 1;
 }
 
+message LocalSlamInsertionResult {
+  cartographer.mapping.proto.NodeId node_id = 1;
+}
+
 message ReceiveLocalSlamResultsResponse {
   int32 trajectory_id = 1;
   int64 timestamp = 2;
   cartographer.transform.proto.Rigid3d local_pose = 3;
   cartographer.sensor.proto.RangeData range_data = 4;
-  cartographer.mapping.proto.NodeId node_id = 5;
+  LocalSlamInsertionResult insertion_result = 5;
 }
 
 message GetSubmapRequest {