12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Introduce client_id in SensorMetadata (#1257)

master
Christoph Schütte 2018-07-12 00:21:31 +02:00 committed by Wally B. Feed
parent 5ad2088561
commit 1b88fb8e90
9 changed files with 56 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer/cloud/client/map_builder_stub.cc
View File

@ -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();
}

17
cartographer/cloud/internal/client/trajectory_builder_stub.cc
View File

@ -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);
}

3
cartographer/cloud/internal/client/trajectory_builder_stub.h
View File

@ -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>>

13
cartographer/cloud/internal/map_builder_context_impl.h
View File

@ -39,15 +39,18 @@ MapBuilderContext<SubmapType>::sensor_data_queue() {
template <class SubmapType>
mapping::TrajectoryBuilderInterface::LocalSlamResultCallback
MapBuilderContext<SubmapType>::GetLocalSlamResultCallbackForSubscriptions() {
MapBuilderServer* map_builder_server = map_builder_server_;
return [map_builder_server](
int trajectory_id, common::Time time,
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) {
map_builder_server->OnLocalSlamResult(trajectory_id, time, local_pose,
std::move(range_data),
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));
};
}

8
cartographer/cloud/internal/map_builder_server.cc
View File

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

4
cartographer/cloud/internal/map_builder_server.h
View File

@ -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);

23
cartographer/cloud/internal/sensor/serialization.cc
View File

@ -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));

10
cartographer/cloud/internal/sensor/serialization.h
View File

@ -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);

1
cartographer/cloud/proto/map_builder_service.proto
View File

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