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

Stub receives LocalSlamResults. (#778) 

[RFC=0002](https://github.com/googlecartographer/rfcs/blob/master/text/0002-cloud-based-mapping-1.md)
PAIR=cschuet
master
gaschler 2017-12-20 12:42:27 +01:00 committed by Wally B. Feed
parent f5e99089a9
commit c881fe90cf
3 changed files with 59 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
cartographer_grpc/mapping/map_builder_stub.cc
View File

@ -49,7 +49,8 @@ int MapBuilderStub::AddTrajectoryBuilder(
std::piecewise_construct, std::forward_as_tuple(result.trajectory_id()),
std::forward_as_tuple(cartographer::common::make_unique<
cartographer_grpc::mapping::TrajectoryBuilderStub>(
client_channel_, result.trajectory_id())));
client_channel_, result.trajectory_id(),
local_slam_result_callback)));
return result.trajectory_id();
}

44
cartographer_grpc/mapping/trajectory_builder_stub.cc
View File

@ -23,13 +23,30 @@ namespace cartographer_grpc {
namespace mapping {
TrajectoryBuilderStub::TrajectoryBuilderStub(
std::shared_ptr<grpc::Channel> client_channel, const int trajectory_id)
std::shared_ptr<grpc::Channel> client_channel, const int trajectory_id,
LocalSlamResultCallback local_slam_result_callback)
: client_channel_(client_channel), trajectory_id_(trajectory_id) {
stub_ = proto::MapBuilderService::NewStub(client_channel_);
CHECK(stub_) << "Failed to create stub.";
if (local_slam_result_callback) {
proto::ReceiveLocalSlamResultsRequest request;
request.set_trajectory_id(trajectory_id);
local_slam_result_reader_.client_reader = stub_->ReceiveLocalSlamResults(
&local_slam_result_reader_.client_context, request);
auto* client_reader_ptr = local_slam_result_reader_.client_reader.get();
local_slam_result_reader_.thread =
cartographer::common::make_unique<std::thread>(
[client_reader_ptr, local_slam_result_callback]() {
RunLocalSlamResultReader(client_reader_ptr,
local_slam_result_callback);
});
}
}
TrajectoryBuilderStub::~TrajectoryBuilderStub() {
if (local_slam_result_reader_.thread) {
local_slam_result_reader_.thread->join();
}
if (rangefinder_writer_.client_writer) {
CHECK(rangefinder_writer_.client_writer->WritesDone());
CHECK(rangefinder_writer_.client_writer->Finish().ok());
@ -115,5 +132,30 @@ proto::SensorMetadata TrajectoryBuilderStub::CreateSensorMetadata(
return sensor_metadata;
}
void TrajectoryBuilderStub::RunLocalSlamResultReader(
grpc::ClientReader<proto::ReceiveLocalSlamResultsResponse>* client_reader,
LocalSlamResultCallback local_slam_result_callback) {
proto::ReceiveLocalSlamResultsResponse response;
while (client_reader->Read(&response)) {
int trajectory_id = response.trajectory_id();
cartographer::common::Time time =
cartographer::common::FromUniversal(response.timestamp());
cartographer::transform::Rigid3d local_pose =
cartographer::transform::ToRigid3(response.local_pose());
cartographer::sensor::RangeData range_data =
cartographer::sensor::FromProto(response.range_data());
auto node_id =
response.has_node_id()
? cartographer::common::make_unique<cartographer::mapping::NodeId>(
cartographer::mapping::NodeId{
response.node_id().trajectory_id(),
response.node_id().node_index()})
: nullptr;
local_slam_result_callback(trajectory_id, time, local_pose, range_data,
std::move(node_id));
}
client_reader->Finish();
}
} // namespace mapping
} // namespace cartographer_grpc

15
cartographer_grpc/mapping/trajectory_builder_stub.h
View File

@ -17,6 +17,8 @@
#ifndef CARTOGRAPHER_GRPC_MAPPING_TRAJECTORY_BUILDER_STUB_H_
#define CARTOGRAPHER_GRPC_MAPPING_TRAJECTORY_BUILDER_STUB_H_
#include <thread>
#include "cartographer/mapping/trajectory_builder_interface.h"
#include "cartographer_grpc/proto/map_builder_service.grpc.pb.h"
#include "grpc++/grpc++.h"
@ -28,7 +30,8 @@ class TrajectoryBuilderStub
: public cartographer::mapping::TrajectoryBuilderInterface {
public:
TrajectoryBuilderStub(std::shared_ptr<grpc::Channel> client_channel,
const int trajectory_id);
const int trajectory_id,
LocalSlamResultCallback local_slam_result_callback);
~TrajectoryBuilderStub() override;
TrajectoryBuilderStub(const TrajectoryBuilderStub&) = delete;
TrajectoryBuilderStub& operator=(const TrajectoryBuilderStub&) = delete;
@ -52,8 +55,17 @@ class TrajectoryBuilderStub
std::unique_ptr<grpc::ClientWriter<RequestType>> client_writer;
google::protobuf::Empty response;
};
struct LocalSlamResultReader {
grpc::ClientContext client_context;
std::unique_ptr<grpc::ClientReader<proto::ReceiveLocalSlamResultsResponse>>
client_reader;
std::unique_ptr<std::thread> thread;
};
proto::SensorMetadata CreateSensorMetadata(const std::string& sensor_id);
static void RunLocalSlamResultReader(
grpc::ClientReader<proto::ReceiveLocalSlamResultsResponse>* client_reader,
LocalSlamResultCallback local_slam_result_callback);
std::shared_ptr<grpc::Channel> client_channel_;
const int trajectory_id_;
@ -62,6 +74,7 @@ class TrajectoryBuilderStub
SensorClientWriter<proto::AddImuDataRequest> imu_writer_;
SensorClientWriter<proto::AddOdometryDataRequest> odometry_writer_;
SensorClientWriter<proto::AddFixedFramePoseDataRequest> fixed_frame_writer_;
LocalSlamResultReader local_slam_result_reader_;
};
} // namespace mapping