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

TrajectoryBuilderStub implementation (#764)

master
gaschler 2017-12-18 11:24:16 +01:00 committed by Wally B. Feed
parent 3fbc642a89
commit 89b49dfefb
3 changed files with 86 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer_grpc/mapping/map_builder_stub.cc
View File

@ -48,7 +48,7 @@ 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_, service_stub_.get())));
client_channel_, result.trajectory_id())));
return result.trajectory_id();
}

75
cartographer_grpc/mapping/trajectory_builder_stub.cc
View File

@ -16,38 +16,99 @@
#include "cartographer_grpc/mapping/trajectory_builder_stub.h"
#include "cartographer_grpc/proto/map_builder_service.pb.h"
#include "glog/logging.h"
namespace cartographer_grpc {
namespace mapping {
TrajectoryBuilderStub::TrajectoryBuilderStub(
std::shared_ptr<grpc::Channel> client_channel,
proto::MapBuilderService::Stub* stub)
: client_channel_(client_channel), stub_(stub) {}
std::shared_ptr<grpc::Channel> client_channel, const int trajectory_id)
: client_channel_(client_channel), trajectory_id_(trajectory_id) {
stub_ = proto::MapBuilderService::NewStub(client_channel_);
CHECK(stub_) << "Failed to create stub.";
}
TrajectoryBuilderStub::~TrajectoryBuilderStub() {
if (rangefinder_writer_.client_writer) {
CHECK(rangefinder_writer_.client_writer->Finish().ok());
}
if (imu_writer_.client_writer) {
CHECK(imu_writer_.client_writer->Finish().ok());
}
if (odometry_writer_.client_writer) {
CHECK(odometry_writer_.client_writer->Finish().ok());
}
if (fixed_frame_writer_.client_writer) {
CHECK(fixed_frame_writer_.client_writer->Finish().ok());
}
}
void TrajectoryBuilderStub::AddSensorData(
const std::string& sensor_id,
const cartographer::sensor::TimedPointCloudData& timed_point_cloud_data) {
LOG(FATAL) << "Not implemented";
if (!rangefinder_writer_.client_writer) {
rangefinder_writer_.client_writer = stub_->AddRangefinderData(
&rangefinder_writer_.client_context, &rangefinder_writer_.response);
CHECK(rangefinder_writer_.client_writer);
}
proto::AddRangefinderDataRequest request;
*request.mutable_sensor_metadata() = CreateSensorMetadata(sensor_id);
*request.mutable_timed_point_cloud_data() =
cartographer::sensor::ToProto(timed_point_cloud_data);
rangefinder_writer_.client_writer->Write(request);
}
void TrajectoryBuilderStub::AddSensorData(
const std::string& sensor_id,
const cartographer::sensor::ImuData& imu_data) {
LOG(FATAL) << "Not implemented";
if (!imu_writer_.client_writer) {
imu_writer_.client_writer =
stub_->AddImuData(&imu_writer_.client_context, &imu_writer_.response);
CHECK(imu_writer_.client_writer);
}
proto::AddImuDataRequest request;
*request.mutable_sensor_metadata() = CreateSensorMetadata(sensor_id);
*request.mutable_imu_data() = cartographer::sensor::ToProto(imu_data);
imu_writer_.client_writer->Write(request);
}
void TrajectoryBuilderStub::AddSensorData(
const std::string& sensor_id,
const cartographer::sensor::OdometryData& odometry_data) {
LOG(FATAL) << "Not implemented";
if (!odometry_writer_.client_writer) {
odometry_writer_.client_writer = stub_->AddOdometryData(
&odometry_writer_.client_context, &odometry_writer_.response);
CHECK(odometry_writer_.client_writer);
}
proto::AddOdometryDataRequest request;
*request.mutable_sensor_metadata() = CreateSensorMetadata(sensor_id);
*request.mutable_odometry_data() =
cartographer::sensor::ToProto(odometry_data);
odometry_writer_.client_writer->Write(request);
}
void TrajectoryBuilderStub::AddSensorData(
const std::string& sensor_id,
const cartographer::sensor::FixedFramePoseData& fixed_frame_pose) {
LOG(FATAL) << "Not implemented";
if (!fixed_frame_writer_.client_writer) {
fixed_frame_writer_.client_writer = stub_->AddFixedFramePoseData(
&fixed_frame_writer_.client_context, &fixed_frame_writer_.response);
CHECK(fixed_frame_writer_.client_writer);
}
proto::AddFixedFramePoseDataRequest request;
*request.mutable_sensor_metadata() = CreateSensorMetadata(sensor_id);
*request.mutable_fixed_frame_pose_data() =
cartographer::sensor::ToProto(fixed_frame_pose);
fixed_frame_writer_.client_writer->Write(request);
}
proto::SensorMetadata TrajectoryBuilderStub::CreateSensorMetadata(
const std::string& sensor_id) {
proto::SensorMetadata sensor_metadata;
sensor_metadata.set_sensor_id(sensor_id);
sensor_metadata.set_trajectory_id(trajectory_id_);
return sensor_metadata;
}
} // namespace mapping

20
cartographer_grpc/mapping/trajectory_builder_stub.h
View File

@ -28,8 +28,8 @@ class TrajectoryBuilderStub
: public cartographer::mapping::TrajectoryBuilderInterface {
public:
TrajectoryBuilderStub(std::shared_ptr<grpc::Channel> client_channel,
proto::MapBuilderService::Stub* stub);
const int trajectory_id);
~TrajectoryBuilderStub() override;
TrajectoryBuilderStub(const TrajectoryBuilderStub&) = delete;
TrajectoryBuilderStub& operator=(const TrajectoryBuilderStub&) = delete;
@ -46,8 +46,22 @@ class TrajectoryBuilderStub
fixed_frame_pose) override;
private:
template <typename RequestType>
struct SensorClientWriter {
grpc::ClientContext client_context;
std::unique_ptr<grpc::ClientWriter<RequestType>> client_writer;
google::protobuf::Empty response;
};
proto::SensorMetadata CreateSensorMetadata(const std::string& sensor_id);
std::shared_ptr<grpc::Channel> client_channel_;
proto::MapBuilderService::Stub* stub_;
const int trajectory_id_;
std::unique_ptr<proto::MapBuilderService::Stub> stub_;
SensorClientWriter<proto::AddRangefinderDataRequest> rangefinder_writer_;
SensorClientWriter<proto::AddImuDataRequest> imu_writer_;
SensorClientWriter<proto::AddOdometryDataRequest> odometry_writer_;
SensorClientWriter<proto::AddFixedFramePoseDataRequest> fixed_frame_writer_;
};
} // namespace mapping