Stub receives LocalSlamResults. (#778)
[RFC=0002](https://github.com/googlecartographer/rfcs/blob/master/text/0002-cloud-based-mapping-1.md) PAIR=cschuetmaster
parent
f5e99089a9
commit
c881fe90cf
|
@ -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();
|
||||
}
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
Loading…
Reference in New Issue