diff --git a/cartographer/cloud/client/map_builder_stub.cc b/cartographer/cloud/client/map_builder_stub.cc index 88e7f9e..0f216ef 100644 --- a/cartographer/cloud/client/map_builder_stub.cc +++ b/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( - client_channel_, client.response().trajectory_id(), + client_channel_, client.response().trajectory_id(), client_id_, local_slam_result_callback))); return client.response().trajectory_id(); } diff --git a/cartographer/cloud/internal/client/trajectory_builder_stub.cc b/cartographer/cloud/internal/client/trajectory_builder_stub.cc index efc7342..4c81ae9 100644 --- a/cartographer/cloud/internal/client/trajectory_builder_stub.cc +++ b/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), - &request); + CreateAddImuDataRequest(sensor_id, trajectory_id_, client_id_, + 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( - sensor_id, trajectory_id_, sensor::ToProto(fixed_frame_pose), &request); + CreateAddFixedFramePoseDataRequest(sensor_id, trajectory_id_, client_id_, + 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); } diff --git a/cartographer/cloud/internal/client/trajectory_builder_stub.h b/cartographer/cloud/internal/client/trajectory_builder_stub.h index 62f1b86..95251a9 100644 --- a/cartographer/cloud/internal/client/trajectory_builder_stub.h +++ b/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> add_rangefinder_client_; std::unique_ptr> diff --git a/cartographer/cloud/internal/map_builder_context_impl.h b/cartographer/cloud/internal/map_builder_context_impl.h index 983ded4..d3f1a9b 100644 --- a/cartographer/cloud/internal/map_builder_context_impl.h +++ b/cartographer/cloud/internal/map_builder_context_impl.h @@ -39,16 +39,19 @@ MapBuilderContext::sensor_data_queue() { template mapping::TrajectoryBuilderInterface::LocalSlamResultCallback MapBuilderContext::GetLocalSlamResultCallbackForSubscriptions() { - MapBuilderServer* map_builder_server = map_builder_server_; - return [map_builder_server]( - int trajectory_id, common::Time time, - transform::Rigid3d local_pose, sensor::RangeData range_data, - std::unique_ptr< - const mapping::TrajectoryBuilderInterface::InsertionResult> - insertion_result) { - map_builder_server->OnLocalSlamResult(trajectory_id, time, local_pose, - std::move(range_data), - std::move(insertion_result)); + return [this](int trajectory_id, common::Time time, + transform::Rigid3d local_pose, sensor::RangeData range_data, + std::unique_ptr< + const mapping::TrajectoryBuilderInterface::InsertionResult> + insertion_result) { + auto it = client_ids_.find(trajectory_id); + if (it == client_ids_.end()) { + LOG(ERROR) << "Unknown trajectory_id " << trajectory_id << ". Ignoring."; + return; + } + map_builder_server_->OnLocalSlamResult(trajectory_id, it->second, time, + local_pose, std::move(range_data), + std::move(insertion_result)); }; } diff --git a/cartographer/cloud/internal/map_builder_server.cc b/cartographer/cloud/internal/map_builder_server.cc index 88ca1a1..f0a04f4 100644 --- a/cartographer/cloud/internal/map_builder_server.cc +++ b/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, - sensor::RangeData range_data, + int trajectory_id, const std::string client_id, common::Time time, + transform::Rigid3d local_pose, sensor::RangeData range_data, std::unique_ptr insertion_result) { auto shared_range_data = @@ -188,8 +188,8 @@ void MapBuilderServer::OnLocalSlamResult( grpc_server_->GetUnsynchronizedContext() ->local_trajectory_uploader() ->GetLocalSlamResultSensorId(trajectory_id); - CreateSensorDataForLocalSlamResult(sensor_id.id, trajectory_id, time, - starting_submap_index_, + CreateSensorDataForLocalSlamResult(sensor_id.id, trajectory_id, client_id, + time, starting_submap_index_, *insertion_result, sensor_data.get()); // TODO(cschuet): Make this more robust. if (insertion_result->insertion_submaps.front()->finished()) { diff --git a/cartographer/cloud/internal/map_builder_server.h b/cartographer/cloud/internal/map_builder_server.h index af014fb..d709c3c 100644 --- a/cartographer/cloud/internal/map_builder_server.h +++ b/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, - sensor::RangeData range_data, + int trajectory_id, const std::string client_id, common::Time time, + transform::Rigid3d local_pose, sensor::RangeData range_data, std::unique_ptr< const mapping::TrajectoryBuilderInterface::InsertionResult> insertion_result); diff --git a/cartographer/cloud/internal/sensor/serialization.cc b/cartographer/cloud/internal/sensor/serialization.cc index 364e047..8127c4e 100644 --- a/cartographer/cloud/internal/sensor/serialization.cc +++ b/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, - int starting_submap_index, + const std::string& sensor_id, int trajectory_id, + 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)); diff --git a/cartographer/cloud/internal/sensor/serialization.h b/cartographer/cloud/internal/sensor/serialization.h index 0789957..46ee126 100644 --- a/cartographer/cloud/internal/sensor/serialization.h +++ b/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, - int starting_submap_index, + const std::string& sensor_id, int trajectory_id, + const std::string& client_id, common::Time time, int starting_submap_index, const mapping::TrajectoryBuilderInterface::InsertionResult& insertion_result, proto::SensorData* proto); diff --git a/cartographer/cloud/proto/map_builder_service.proto b/cartographer/cloud/proto/map_builder_service.proto index ac764ff..761078a 100644 --- a/cartographer/cloud/proto/map_builder_service.proto +++ b/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 {