Introduce client_id in SensorMetadata (#1257)

cartographer/cloud/client/map_builder_stub.cc

@@ -75,7 +75,7 @@ int MapBuilderStub::AddTrajectoryBuilder(
       std::piecewise_construct,
       std::forward_as_tuple(client.response().trajectory_id()),
       std::forward_as_tuple(make_unique<TrajectoryBuilderStub>(
-          client_channel_, client.response().trajectory_id(),
+          client_channel_, client.response().trajectory_id(), client_id_,
           local_slam_result_callback)));
   return client.response().trajectory_id();
 }

cartographer/cloud/internal/client/trajectory_builder_stub.cc

@@ -27,9 +27,11 @@ namespace cloud {
 
 TrajectoryBuilderStub::TrajectoryBuilderStub(
     std::shared_ptr<::grpc::Channel> client_channel, const int trajectory_id,
+    const std::string& client_id,
     LocalSlamResultCallback local_slam_result_callback)
     : client_channel_(client_channel),
       trajectory_id_(trajectory_id),
+      client_id_(client_id),
       receive_local_slam_results_client_(client_channel_) {
   if (local_slam_result_callback) {
     proto::ReceiveLocalSlamResultsRequest request;
@@ -80,7 +82,7 @@ void TrajectoryBuilderStub::AddSensorData(
         client_channel_);
   }
   proto::AddRangefinderDataRequest request;
-  CreateAddRangeFinderDataRequest(sensor_id, trajectory_id_,
+  CreateAddRangeFinderDataRequest(sensor_id, trajectory_id_, client_id_,
                                   sensor::ToProto(timed_point_cloud_data),
                                   &request);
   add_rangefinder_client_->Write(request);
@@ -94,8 +96,8 @@ void TrajectoryBuilderStub::AddSensorData(const std::string& sensor_id,
             client_channel_);
   }
   proto::AddImuDataRequest request;
-  CreateAddImuDataRequest(sensor_id, trajectory_id_, sensor::ToProto(imu_data),
+  CreateAddImuDataRequest(sensor_id, trajectory_id_, client_id_,
-                          &request);
+                          sensor::ToProto(imu_data), &request);
   add_imu_client_->Write(request);
 }
 
@@ -107,7 +109,7 @@ void TrajectoryBuilderStub::AddSensorData(
         client_channel_);
   }
   proto::AddOdometryDataRequest request;
-  CreateAddOdometryDataRequest(sensor_id, trajectory_id_,
+  CreateAddOdometryDataRequest(sensor_id, trajectory_id_, client_id_,
                                sensor::ToProto(odometry_data), &request);
   add_odometry_client_->Write(request);
 }
@@ -121,8 +123,9 @@ void TrajectoryBuilderStub::AddSensorData(
         client_channel_);
   }
   proto::AddFixedFramePoseDataRequest request;
-  CreateAddFixedFramePoseDataRequest(
+  CreateAddFixedFramePoseDataRequest(sensor_id, trajectory_id_, client_id_,
-      sensor_id, trajectory_id_, sensor::ToProto(fixed_frame_pose), &request);
+                                     sensor::ToProto(fixed_frame_pose),
+                                     &request);
   add_fixed_frame_pose_client_->Write(request);
 }
 
@@ -134,7 +137,7 @@ void TrajectoryBuilderStub::AddSensorData(
         client_channel_);
   }
   proto::AddLandmarkDataRequest request;
-  CreateAddLandmarkDataRequest(sensor_id, trajectory_id_,
+  CreateAddLandmarkDataRequest(sensor_id, trajectory_id_, client_id_,
                                sensor::ToProto(landmark_data), &request);
   add_landmark_client_->Write(request);
 }

cartographer/cloud/internal/client/trajectory_builder_stub.h

@@ -38,7 +38,7 @@ namespace cloud {
 class TrajectoryBuilderStub : public mapping::TrajectoryBuilderInterface {
  public:
   TrajectoryBuilderStub(std::shared_ptr<::grpc::Channel> client_channel,
-                        const int trajectory_id,
+                        const int trajectory_id, const std::string& client_id,
                         LocalSlamResultCallback local_slam_result_callback);
   ~TrajectoryBuilderStub() override;
   TrajectoryBuilderStub(const TrajectoryBuilderStub&) = delete;
@@ -67,6 +67,7 @@ class TrajectoryBuilderStub : public mapping::TrajectoryBuilderInterface {
 
   std::shared_ptr<::grpc::Channel> client_channel_;
   const int trajectory_id_;
+  const std::string client_id_;
   std::unique_ptr<async_grpc::Client<handlers::AddRangefinderDataSignature>>
       add_rangefinder_client_;
   std::unique_ptr<async_grpc::Client<handlers::AddImuDataSignature>>

cartographer/cloud/internal/map_builder_context_impl.h

@@ -39,16 +39,19 @@ MapBuilderContext<SubmapType>::sensor_data_queue() {
 template <class SubmapType>
 mapping::TrajectoryBuilderInterface::LocalSlamResultCallback
 MapBuilderContext<SubmapType>::GetLocalSlamResultCallbackForSubscriptions() {
-  MapBuilderServer* map_builder_server = map_builder_server_;
+  return [this](int trajectory_id, common::Time time,
-  return [map_builder_server](
+                transform::Rigid3d local_pose, sensor::RangeData range_data,
-             int trajectory_id, common::Time time,
+                std::unique_ptr<
-             transform::Rigid3d local_pose, sensor::RangeData range_data,
+                    const mapping::TrajectoryBuilderInterface::InsertionResult>
-             std::unique_ptr<
+                    insertion_result) {
-                 const mapping::TrajectoryBuilderInterface::InsertionResult>
+    auto it = client_ids_.find(trajectory_id);
-                 insertion_result) {
+    if (it == client_ids_.end()) {
-    map_builder_server->OnLocalSlamResult(trajectory_id, time, local_pose,
+      LOG(ERROR) << "Unknown trajectory_id " << trajectory_id << ". Ignoring.";
-                                          std::move(range_data),
+      return;
-                                          std::move(insertion_result));
+    }
+    map_builder_server_->OnLocalSlamResult(trajectory_id, it->second, time,
+                                           local_pose, std::move(range_data),
+                                           std::move(insertion_result));
   };
 }
 

cartographer/cloud/internal/map_builder_server.cc

@@ -171,8 +171,8 @@ void MapBuilderServer::StartSlamThread() {
 }
 
 void MapBuilderServer::OnLocalSlamResult(
-    int trajectory_id, common::Time time, transform::Rigid3d local_pose,
+    int trajectory_id, const std::string client_id, common::Time time,
-    sensor::RangeData range_data,
+    transform::Rigid3d local_pose, sensor::RangeData range_data,
     std::unique_ptr<const mapping::TrajectoryBuilderInterface::InsertionResult>
         insertion_result) {
   auto shared_range_data =
@@ -188,8 +188,8 @@ void MapBuilderServer::OnLocalSlamResult(
         grpc_server_->GetUnsynchronizedContext<MapBuilderContextInterface>()
             ->local_trajectory_uploader()
             ->GetLocalSlamResultSensorId(trajectory_id);
-    CreateSensorDataForLocalSlamResult(sensor_id.id, trajectory_id, time,
+    CreateSensorDataForLocalSlamResult(sensor_id.id, trajectory_id, client_id,
-                                       starting_submap_index_,
+                                       time, starting_submap_index_,
                                        *insertion_result, sensor_data.get());
     // TODO(cschuet): Make this more robust.
     if (insertion_result->insertion_submaps.front()->finished()) {

cartographer/cloud/internal/map_builder_server.h

@@ -110,8 +110,8 @@ class MapBuilderServer : public MapBuilderServerInterface {
   void ProcessSensorDataQueue();
   void StartSlamThread();
   void OnLocalSlamResult(
-      int trajectory_id, common::Time time, transform::Rigid3d local_pose,
+      int trajectory_id, const std::string client_id, common::Time time,
-      sensor::RangeData range_data,
+      transform::Rigid3d local_pose, sensor::RangeData range_data,
       std::unique_ptr<
           const mapping::TrajectoryBuilderInterface::InsertionResult>
           insertion_result);

cartographer/cloud/internal/sensor/serialization.cc

@@ -20,63 +20,70 @@ namespace cartographer {
 namespace cloud {
 
 void CreateSensorMetadata(const std::string& sensor_id, const int trajectory_id,
+                          const std::string& client_id,
                           proto::SensorMetadata* proto) {
   proto->set_sensor_id(sensor_id);
   proto->set_trajectory_id(trajectory_id);
+  proto->set_client_id(client_id);
 }
 
 void CreateAddFixedFramePoseDataRequest(
     const std::string& sensor_id, int trajectory_id,
+    const std::string& client_id,
     const sensor::proto::FixedFramePoseData& fixed_frame_pose_data,
     proto::AddFixedFramePoseDataRequest* proto) {
-  CreateSensorMetadata(sensor_id, trajectory_id,
+  CreateSensorMetadata(sensor_id, trajectory_id, client_id,
                        proto->mutable_sensor_metadata());
   *proto->mutable_fixed_frame_pose_data() = fixed_frame_pose_data;
 }
 
 void CreateAddImuDataRequest(const std::string& sensor_id,
                              const int trajectory_id,
+                             const std::string& client_id,
                              const sensor::proto::ImuData& imu_data,
                              proto::AddImuDataRequest* proto) {
-  CreateSensorMetadata(sensor_id, trajectory_id,
+  CreateSensorMetadata(sensor_id, trajectory_id, client_id,
                        proto->mutable_sensor_metadata());
   *proto->mutable_imu_data() = imu_data;
 }
 
 void CreateAddOdometryDataRequest(
     const std::string& sensor_id, int trajectory_id,
+    const std::string& client_id,
     const sensor::proto::OdometryData& odometry_data,
     proto::AddOdometryDataRequest* proto) {
-  CreateSensorMetadata(sensor_id, trajectory_id,
+  CreateSensorMetadata(sensor_id, trajectory_id, client_id,
                        proto->mutable_sensor_metadata());
   *proto->mutable_odometry_data() = odometry_data;
 }
 
 void CreateAddRangeFinderDataRequest(
     const std::string& sensor_id, int trajectory_id,
+    const std::string& client_id,
     const sensor::proto::TimedPointCloudData& timed_point_cloud_data,
     proto::AddRangefinderDataRequest* proto) {
-  CreateSensorMetadata(sensor_id, trajectory_id,
+  CreateSensorMetadata(sensor_id, trajectory_id, client_id,
                        proto->mutable_sensor_metadata());
   *proto->mutable_timed_point_cloud_data() = timed_point_cloud_data;
 }
 
 void CreateAddLandmarkDataRequest(
     const std::string& sensor_id, int trajectory_id,
+    const std::string& client_id,
     const sensor::proto::LandmarkData& landmark_data,
     proto::AddLandmarkDataRequest* proto) {
-  CreateSensorMetadata(sensor_id, trajectory_id,
+  CreateSensorMetadata(sensor_id, trajectory_id, client_id,
                        proto->mutable_sensor_metadata());
   *proto->mutable_landmark_data() = landmark_data;
 }
 
 void CreateSensorDataForLocalSlamResult(
-    const std::string& sensor_id, int trajectory_id, common::Time time,
+    const std::string& sensor_id, int trajectory_id,
-    int starting_submap_index,
+    const std::string& client_id, common::Time time, int starting_submap_index,
     const mapping::TrajectoryBuilderInterface::InsertionResult&
         insertion_result,
     proto::SensorData* proto) {
-  CreateSensorMetadata(sensor_id, trajectory_id,
+  CreateSensorMetadata(sensor_id, trajectory_id, client_id,
                        proto->mutable_sensor_metadata());
   proto->mutable_local_slam_result_data()->set_timestamp(
       common::ToUniversal(time));

cartographer/cloud/internal/sensor/serialization.h

@@ -30,30 +30,36 @@ namespace cartographer {
 namespace cloud {
 
 void CreateSensorMetadata(const std::string& sensor_id, int trajectory_id,
+                          const std::string& client_id,
                           proto::SensorMetadata* proto);
 
 void CreateAddFixedFramePoseDataRequest(
     const std::string& sensor_id, int trajectory_id,
+    const std::string& client_id,
     const sensor::proto::FixedFramePoseData& fixed_frame_pose_data,
     proto::AddFixedFramePoseDataRequest* proto);
 void CreateAddImuDataRequest(const std::string& sensor_id, int trajectory_id,
+                             const std::string& client_id,
                              const sensor::proto::ImuData& imu_data,
                              proto::AddImuDataRequest* proto);
 void CreateAddOdometryDataRequest(
     const std::string& sensor_id, int trajectory_id,
+    const std::string& client_id,
     const sensor::proto::OdometryData& odometry_data,
     proto::AddOdometryDataRequest* proto);
 void CreateAddRangeFinderDataRequest(
     const std::string& sensor_id, int trajectory_id,
+    const std::string& client_id,
     const sensor::proto::TimedPointCloudData& timed_point_cloud_data,
     proto::AddRangefinderDataRequest* proto);
 void CreateAddLandmarkDataRequest(
     const std::string& sensor_id, int trajectory_id,
+    const std::string& client_id,
     const sensor::proto::LandmarkData& landmark_data,
     proto::AddLandmarkDataRequest* proto);
 void CreateSensorDataForLocalSlamResult(
-    const std::string& sensor_id, int trajectory_id, common::Time time,
+    const std::string& sensor_id, int trajectory_id,
-    int starting_submap_index,
+    const std::string& client_id, common::Time time, int starting_submap_index,
     const mapping::TrajectoryBuilderInterface::InsertionResult&
         insertion_result,
     proto::SensorData* proto);

cartographer/cloud/proto/map_builder_service.proto

@@ -55,6 +55,7 @@ message AddTrajectoryRequest {
 message SensorMetadata {
   int32 trajectory_id = 1;
   string sensor_id = 2;
+  string client_id = 3;
 }
 
 message SensorData {