Use framework::Client everywhere. (#875)

This switches all gRPC method invocation to using framework::Client. After this change we can remove the gRPC proto compiler from the build files.

[RFC=0002](https://github.com/googlecartographer/rfcs/blob/master/text/0002-cloud-based-mapping-1.md)
master
Christoph Schütte 2018-02-01 17:19:15 +01:00 committed by Wally B. Feed
parent 8ea46857ac
commit 302320b1a9
11 changed files with 207 additions and 232 deletions

View File

@ -1,38 +0,0 @@
/*
* Copyright 2018 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_GRPC_FRAMEWORK_CLIENT_WRITER_H_
#define CARTOGRAPHER_GRPC_FRAMEWORK_CLIENT_WRITER_H_
#include <memory>
#include "google/protobuf/empty.pb.h"
#include "grpc++/grpc++.h"
namespace cartographer_grpc {
namespace framework {
template <typename RequestType>
struct ClientWriter {
grpc::ClientContext client_context;
std::unique_ptr<grpc::ClientWriter<RequestType>> client_writer;
google::protobuf::Empty response;
};
} // namespace framework
} // namespace cartographer_grpc
#endif // CARTOGRAPHER_GRPC_FRAMEWORK_CLIENT_WRITER_H_

View File

@ -20,7 +20,6 @@
#include "cartographer_grpc/framework/client.h" #include "cartographer_grpc/framework/client.h"
#include "cartographer_grpc/framework/execution_context.h" #include "cartographer_grpc/framework/execution_context.h"
#include "cartographer_grpc/framework/proto/math_service.grpc.pb.h"
#include "cartographer_grpc/framework/proto/math_service.pb.h" #include "cartographer_grpc/framework/proto/math_service.pb.h"
#include "cartographer_grpc/framework/rpc_handler.h" #include "cartographer_grpc/framework/rpc_handler.h"
#include "glog/logging.h" #include "glog/logging.h"

View File

@ -15,8 +15,9 @@
*/ */
#include "cartographer_grpc/local_trajectory_uploader.h" #include "cartographer_grpc/local_trajectory_uploader.h"
#include "cartographer/common/make_unique.h" #include "cartographer/common/make_unique.h"
#include "cartographer_grpc/handlers/add_trajectory_handler.h"
#include "cartographer_grpc/handlers/finish_trajectory_handler.h"
#include "cartographer_grpc/proto/map_builder_service.pb.h" #include "cartographer_grpc/proto/map_builder_service.pb.h"
#include "cartographer_grpc/sensor/serialization.h" #include "cartographer_grpc/sensor/serialization.h"
#include "glog/logging.h" #include "glog/logging.h"
@ -31,22 +32,29 @@ const cartographer::common::Duration kPopTimeout =
LocalTrajectoryUploader::LocalTrajectoryUploader( LocalTrajectoryUploader::LocalTrajectoryUploader(
const std::string &uplink_server_address) const std::string &uplink_server_address)
: client_channel_(grpc::CreateChannel(uplink_server_address, : client_channel_(grpc::CreateChannel(
grpc::InsecureChannelCredentials())), uplink_server_address, grpc::InsecureChannelCredentials())) {}
service_stub_(proto::MapBuilderService::NewStub(client_channel_)) {}
LocalTrajectoryUploader::~LocalTrajectoryUploader() { LocalTrajectoryUploader::~LocalTrajectoryUploader() {
if (imu_writer_.client_writer) { if (add_imu_client_) {
CHECK(imu_writer_.client_writer->WritesDone()); CHECK(add_imu_client_->WritesDone());
CHECK(imu_writer_.client_writer->Finish().ok()); CHECK(add_imu_client_->Finish().ok());
} }
if (odometry_writer_.client_writer) { if (add_odometry_client_) {
CHECK(odometry_writer_.client_writer->WritesDone()); CHECK(add_odometry_client_->WritesDone());
CHECK(odometry_writer_.client_writer->Finish().ok()); CHECK(add_odometry_client_->Finish().ok());
} }
if (fixed_frame_pose_writer_.client_writer) { if (add_fixed_frame_pose_client_) {
CHECK(fixed_frame_pose_writer_.client_writer->WritesDone()); CHECK(add_fixed_frame_pose_client_->WritesDone());
CHECK(fixed_frame_pose_writer_.client_writer->Finish().ok()); CHECK(add_fixed_frame_pose_client_->Finish().ok());
}
if (add_local_slam_result_client_) {
CHECK(add_local_slam_result_client_->WritesDone());
CHECK(add_local_slam_result_client_->Finish().ok());
}
if (add_landmark_client_) {
CHECK(add_landmark_client_->WritesDone());
CHECK(add_landmark_client_->Finish().ok());
} }
} }
@ -84,6 +92,10 @@ void LocalTrajectoryUploader::ProcessSendQueue() {
dynamic_cast<proto::AddLocalSlamResultDataRequest *>( dynamic_cast<proto::AddLocalSlamResultDataRequest *>(
data_message.get())) { data_message.get())) {
ProcessLocalSlamResultDataMessage(local_slam_result_data); ProcessLocalSlamResultDataMessage(local_slam_result_data);
} else if (auto *landmark_data =
dynamic_cast<proto::AddLandmarkDataRequest *>(
data_message.get())) {
ProcessLandmarkDataMessage(landmark_data);
} else { } else {
LOG(FATAL) << "Unknown message type: " << data_message->GetTypeName(); LOG(FATAL) << "Unknown message type: " << data_message->GetTypeName();
} }
@ -100,47 +112,51 @@ void LocalTrajectoryUploader::TranslateTrajectoryId(
void LocalTrajectoryUploader::ProcessFixedFramePoseDataMessage( void LocalTrajectoryUploader::ProcessFixedFramePoseDataMessage(
proto::AddFixedFramePoseDataRequest *data_request) { proto::AddFixedFramePoseDataRequest *data_request) {
if (!fixed_frame_pose_writer_.client_writer) { if (!add_fixed_frame_pose_client_) {
fixed_frame_pose_writer_.client_writer = add_fixed_frame_pose_client_ = cartographer::common::make_unique<
service_stub_->AddFixedFramePoseData( framework::Client<handlers::AddFixedFramePoseDataHandler>>(
&fixed_frame_pose_writer_.client_context, client_channel_);
&fixed_frame_pose_writer_.response);
CHECK(fixed_frame_pose_writer_.client_writer);
} }
TranslateTrajectoryId(data_request->mutable_sensor_metadata()); TranslateTrajectoryId(data_request->mutable_sensor_metadata());
fixed_frame_pose_writer_.client_writer->Write(*data_request); CHECK(add_fixed_frame_pose_client_->Write(*data_request));
} }
void LocalTrajectoryUploader::ProcessImuDataMessage( void LocalTrajectoryUploader::ProcessImuDataMessage(
proto::AddImuDataRequest *data_request) { proto::AddImuDataRequest *data_request) {
if (!imu_writer_.client_writer) { if (!add_imu_client_) {
imu_writer_.client_writer = service_stub_->AddImuData( add_imu_client_ = cartographer::common::make_unique<
&imu_writer_.client_context, &imu_writer_.response); framework::Client<handlers::AddImuDataHandler>>(client_channel_);
CHECK(imu_writer_.client_writer);
} }
TranslateTrajectoryId(data_request->mutable_sensor_metadata()); TranslateTrajectoryId(data_request->mutable_sensor_metadata());
imu_writer_.client_writer->Write(*data_request); CHECK(add_imu_client_->Write(*data_request));
} }
void LocalTrajectoryUploader::ProcessOdometryDataMessage( void LocalTrajectoryUploader::ProcessOdometryDataMessage(
proto::AddOdometryDataRequest *data_request) { proto::AddOdometryDataRequest *data_request) {
if (!odometry_writer_.client_writer) { if (!add_odometry_client_) {
odometry_writer_.client_writer = service_stub_->AddOdometryData( add_odometry_client_ = cartographer::common::make_unique<
&odometry_writer_.client_context, &odometry_writer_.response); framework::Client<handlers::AddOdometryDataHandler>>(client_channel_);
CHECK(odometry_writer_.client_writer);
} }
TranslateTrajectoryId(data_request->mutable_sensor_metadata()); TranslateTrajectoryId(data_request->mutable_sensor_metadata());
odometry_writer_.client_writer->Write(*data_request); CHECK(add_odometry_client_->Write(*data_request));
}
void LocalTrajectoryUploader::ProcessLandmarkDataMessage(
proto::AddLandmarkDataRequest *data_request) {
if (!add_landmark_client_) {
add_landmark_client_ = cartographer::common::make_unique<
framework::Client<handlers::AddLandmarkDataHandler>>(client_channel_);
}
TranslateTrajectoryId(data_request->mutable_sensor_metadata());
CHECK(add_landmark_client_->Write(*data_request));
} }
void LocalTrajectoryUploader::ProcessLocalSlamResultDataMessage( void LocalTrajectoryUploader::ProcessLocalSlamResultDataMessage(
proto::AddLocalSlamResultDataRequest *data_request) { proto::AddLocalSlamResultDataRequest *data_request) {
if (!local_slam_result_writer_.client_writer) { if (!add_local_slam_result_client_) {
local_slam_result_writer_.client_writer = add_local_slam_result_client_ = cartographer::common::make_unique<
service_stub_->AddLocalSlamResultData( framework::Client<handlers::AddLocalSlamResultDataHandler>>(
&local_slam_result_writer_.client_context, client_channel_);
&local_slam_result_writer_.response);
CHECK(local_slam_result_writer_.client_writer);
} }
TranslateTrajectoryId(data_request->mutable_sensor_metadata()); TranslateTrajectoryId(data_request->mutable_sensor_metadata());
// A submap also holds a trajectory id that must be translated to uplink's // A submap also holds a trajectory id that must be translated to uplink's
@ -150,16 +166,14 @@ void LocalTrajectoryUploader::ProcessLocalSlamResultDataMessage(
mutable_submap.mutable_submap_id()->set_trajectory_id( mutable_submap.mutable_submap_id()->set_trajectory_id(
data_request->sensor_metadata().trajectory_id()); data_request->sensor_metadata().trajectory_id());
} }
local_slam_result_writer_.client_writer->Write(*data_request); CHECK(add_local_slam_result_client_->Write(*data_request));
} }
void LocalTrajectoryUploader::AddTrajectory( void LocalTrajectoryUploader::AddTrajectory(
int local_trajectory_id, const std::set<SensorId> &expected_sensor_ids, int local_trajectory_id, const std::set<SensorId> &expected_sensor_ids,
const cartographer::mapping::proto::TrajectoryBuilderOptions const cartographer::mapping::proto::TrajectoryBuilderOptions
&trajectory_options) { &trajectory_options) {
grpc::ClientContext client_context;
proto::AddTrajectoryRequest request; proto::AddTrajectoryRequest request;
proto::AddTrajectoryResponse result;
*request.mutable_trajectory_builder_options() = trajectory_options; *request.mutable_trajectory_builder_options() = trajectory_options;
for (const SensorId &sensor_id : expected_sensor_ids) { for (const SensorId &sensor_id : expected_sensor_ids) {
// Range sensors are not forwarded, but combined into a LocalSlamResult. // Range sensors are not forwarded, but combined into a LocalSlamResult.
@ -169,25 +183,21 @@ void LocalTrajectoryUploader::AddTrajectory(
} }
*request.add_expected_sensor_ids() = *request.add_expected_sensor_ids() =
sensor::ToProto(GetLocalSlamResultSensorId(local_trajectory_id)); sensor::ToProto(GetLocalSlamResultSensorId(local_trajectory_id));
grpc::Status status = framework::Client<handlers::AddTrajectoryHandler> client(client_channel_);
service_stub_->AddTrajectory(&client_context, request, &result); CHECK(client.Write(request));
CHECK(status.ok());
CHECK_EQ(local_to_cloud_trajectory_id_map_.count(local_trajectory_id), 0); CHECK_EQ(local_to_cloud_trajectory_id_map_.count(local_trajectory_id), 0);
local_to_cloud_trajectory_id_map_[local_trajectory_id] = local_to_cloud_trajectory_id_map_[local_trajectory_id] =
result.trajectory_id(); client.response().trajectory_id();
} }
void LocalTrajectoryUploader::FinishTrajectory(int local_trajectory_id) { void LocalTrajectoryUploader::FinishTrajectory(int local_trajectory_id) {
CHECK_EQ(local_to_cloud_trajectory_id_map_.count(local_trajectory_id), 1); CHECK_EQ(local_to_cloud_trajectory_id_map_.count(local_trajectory_id), 1);
int cloud_trajectory_id = int cloud_trajectory_id =
local_to_cloud_trajectory_id_map_[local_trajectory_id]; local_to_cloud_trajectory_id_map_[local_trajectory_id];
grpc::ClientContext client_context;
proto::FinishTrajectoryRequest request; proto::FinishTrajectoryRequest request;
google::protobuf::Empty response;
request.set_trajectory_id(cloud_trajectory_id); request.set_trajectory_id(cloud_trajectory_id);
grpc::Status status = framework::Client<handlers::FinishTrajectoryHandler> client(client_channel_);
service_stub_->FinishTrajectory(&client_context, request, &response); CHECK(client.Write(request));
CHECK(status.ok());
} }
void LocalTrajectoryUploader::EnqueueDataRequest( void LocalTrajectoryUploader::EnqueueDataRequest(

View File

@ -26,8 +26,13 @@
#include "cartographer/common/blocking_queue.h" #include "cartographer/common/blocking_queue.h"
#include "cartographer/mapping/proto/trajectory_builder_options.pb.h" #include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
#include "cartographer/mapping/trajectory_builder_interface.h" #include "cartographer/mapping/trajectory_builder_interface.h"
#include "cartographer_grpc/framework/client_writer.h" #include "cartographer_grpc/framework/client.h"
#include "cartographer_grpc/proto/map_builder_service.grpc.pb.h" #include "cartographer_grpc/handlers/add_fixed_frame_pose_data_handler.h"
#include "cartographer_grpc/handlers/add_imu_data_handler.h"
#include "cartographer_grpc/handlers/add_landmark_data_handler.h"
#include "cartographer_grpc/handlers/add_local_slam_result_data_handler.h"
#include "cartographer_grpc/handlers/add_odometry_data_handler.h"
#include "cartographer_grpc/proto/map_builder_service.pb.h"
#include "grpc++/grpc++.h" #include "grpc++/grpc++.h"
namespace cartographer_grpc { namespace cartographer_grpc {
@ -85,21 +90,25 @@ class LocalTrajectoryUploader : public LocalTrajectoryUploaderInterface {
void ProcessLocalSlamResultDataMessage( void ProcessLocalSlamResultDataMessage(
proto::AddLocalSlamResultDataRequest* data_request); proto::AddLocalSlamResultDataRequest* data_request);
void ProcessOdometryDataMessage(proto::AddOdometryDataRequest* data_request); void ProcessOdometryDataMessage(proto::AddOdometryDataRequest* data_request);
void ProcessLandmarkDataMessage(proto::AddLandmarkDataRequest* data_request);
std::shared_ptr<grpc::Channel> client_channel_; std::shared_ptr<grpc::Channel> client_channel_;
std::unique_ptr<proto::MapBuilderService::Stub> service_stub_;
std::map<int, int> local_to_cloud_trajectory_id_map_; std::map<int, int> local_to_cloud_trajectory_id_map_;
cartographer::common::BlockingQueue< cartographer::common::BlockingQueue<
std::unique_ptr<google::protobuf::Message>> std::unique_ptr<google::protobuf::Message>>
send_queue_; send_queue_;
bool shutting_down_ = false; bool shutting_down_ = false;
std::unique_ptr<std::thread> upload_thread_; std::unique_ptr<std::thread> upload_thread_;
framework::ClientWriter<proto::AddFixedFramePoseDataRequest> std::unique_ptr<framework::Client<handlers::AddFixedFramePoseDataHandler>>
fixed_frame_pose_writer_; add_fixed_frame_pose_client_;
framework::ClientWriter<proto::AddImuDataRequest> imu_writer_; std::unique_ptr<framework::Client<handlers::AddImuDataHandler>>
framework::ClientWriter<proto::AddLocalSlamResultDataRequest> add_imu_client_;
local_slam_result_writer_; std::unique_ptr<framework::Client<handlers::AddLocalSlamResultDataHandler>>
framework::ClientWriter<proto::AddOdometryDataRequest> odometry_writer_; add_local_slam_result_client_;
std::unique_ptr<framework::Client<handlers::AddOdometryDataHandler>>
add_odometry_client_;
std::unique_ptr<framework::Client<handlers::AddLandmarkDataHandler>>
add_landmark_client_;
}; };
} // namespace cartographer_grpc } // namespace cartographer_grpc

View File

@ -33,7 +33,6 @@
#include "cartographer_grpc/handlers/receive_local_slam_results_handler.h" #include "cartographer_grpc/handlers/receive_local_slam_results_handler.h"
#include "cartographer_grpc/handlers/run_final_optimization_handler.h" #include "cartographer_grpc/handlers/run_final_optimization_handler.h"
#include "cartographer_grpc/handlers/write_map_handler.h" #include "cartographer_grpc/handlers/write_map_handler.h"
#include "cartographer_grpc/proto/map_builder_service.grpc.pb.h"
#include "cartographer_grpc/sensor/serialization.h" #include "cartographer_grpc/sensor/serialization.h"
#include "glog/logging.h" #include "glog/logging.h"

View File

@ -15,7 +15,11 @@
*/ */
#include "cartographer_grpc/mapping/map_builder_stub.h" #include "cartographer_grpc/mapping/map_builder_stub.h"
#include "cartographer_grpc/handlers/add_trajectory_handler.h"
#include "cartographer_grpc/handlers/finish_trajectory_handler.h"
#include "cartographer_grpc/handlers/get_submap_handler.h"
#include "cartographer_grpc/handlers/load_map_handler.h"
#include "cartographer_grpc/handlers/write_map_handler.h"
#include "cartographer_grpc/proto/map_builder_service.pb.h" #include "cartographer_grpc/proto/map_builder_service.pb.h"
#include "cartographer_grpc/sensor/serialization.h" #include "cartographer_grpc/sensor/serialization.h"
#include "glog/logging.h" #include "glog/logging.h"
@ -26,33 +30,30 @@ namespace mapping {
MapBuilderStub::MapBuilderStub(const std::string& server_address) MapBuilderStub::MapBuilderStub(const std::string& server_address)
: client_channel_(grpc::CreateChannel(server_address, : client_channel_(grpc::CreateChannel(server_address,
grpc::InsecureChannelCredentials())), grpc::InsecureChannelCredentials())),
service_stub_(proto::MapBuilderService::NewStub(client_channel_)), pose_graph_stub_(client_channel_) {}
pose_graph_stub_(client_channel_, service_stub_.get()) {}
int MapBuilderStub::AddTrajectoryBuilder( int MapBuilderStub::AddTrajectoryBuilder(
const std::set<SensorId>& expected_sensor_ids, const std::set<SensorId>& expected_sensor_ids,
const cartographer::mapping::proto::TrajectoryBuilderOptions& const cartographer::mapping::proto::TrajectoryBuilderOptions&
trajectory_options, trajectory_options,
LocalSlamResultCallback local_slam_result_callback) { LocalSlamResultCallback local_slam_result_callback) {
grpc::ClientContext client_context;
proto::AddTrajectoryRequest request; proto::AddTrajectoryRequest request;
proto::AddTrajectoryResponse result;
*request.mutable_trajectory_builder_options() = trajectory_options; *request.mutable_trajectory_builder_options() = trajectory_options;
for (const auto& sensor_id : expected_sensor_ids) { for (const auto& sensor_id : expected_sensor_ids) {
*request.add_expected_sensor_ids() = sensor::ToProto(sensor_id); *request.add_expected_sensor_ids() = sensor::ToProto(sensor_id);
} }
grpc::Status status = framework::Client<handlers::AddTrajectoryHandler> client(client_channel_);
service_stub_->AddTrajectory(&client_context, request, &result); CHECK(client.Write(request));
CHECK(status.ok());
// Construct trajectory builder stub. // Construct trajectory builder stub.
trajectory_builder_stubs_.emplace( trajectory_builder_stubs_.emplace(
std::piecewise_construct, std::forward_as_tuple(result.trajectory_id()), std::piecewise_construct,
std::forward_as_tuple(client.response().trajectory_id()),
std::forward_as_tuple(cartographer::common::make_unique< std::forward_as_tuple(cartographer::common::make_unique<
cartographer_grpc::mapping::TrajectoryBuilderStub>( cartographer_grpc::mapping::TrajectoryBuilderStub>(
client_channel_, result.trajectory_id(), client_channel_, client.response().trajectory_id(),
local_slam_result_callback))); local_slam_result_callback)));
return result.trajectory_id(); return client.response().trajectory_id();
} }
int MapBuilderStub::AddTrajectoryForDeserialization() { int MapBuilderStub::AddTrajectoryForDeserialization() {
@ -65,13 +66,10 @@ MapBuilderStub::GetTrajectoryBuilder(int trajectory_id) const {
} }
void MapBuilderStub::FinishTrajectory(int trajectory_id) { void MapBuilderStub::FinishTrajectory(int trajectory_id) {
grpc::ClientContext client_context;
proto::FinishTrajectoryRequest request; proto::FinishTrajectoryRequest request;
google::protobuf::Empty response;
request.set_trajectory_id(trajectory_id); request.set_trajectory_id(trajectory_id);
grpc::Status status = framework::Client<handlers::FinishTrajectoryHandler> client(client_channel_);
service_stub_->FinishTrajectory(&client_context, request, &response); CHECK(client.Write(request));
CHECK(status.ok());
trajectory_builder_stubs_.erase(trajectory_id); trajectory_builder_stubs_.erase(trajectory_id);
} }
@ -79,22 +77,21 @@ std::string MapBuilderStub::SubmapToProto(
const cartographer::mapping::SubmapId& submap_id, const cartographer::mapping::SubmapId& submap_id,
cartographer::mapping::proto::SubmapQuery::Response* cartographer::mapping::proto::SubmapQuery::Response*
submap_query_response) { submap_query_response) {
grpc::ClientContext client_context;
proto::GetSubmapRequest request; proto::GetSubmapRequest request;
submap_id.ToProto(request.mutable_submap_id()); submap_id.ToProto(request.mutable_submap_id());
proto::GetSubmapResponse response; framework::Client<handlers::GetSubmapHandler> client(client_channel_);
CHECK(service_stub_->GetSubmap(&client_context, request, &response).ok()); CHECK(client.Write(request));
submap_query_response->CopyFrom(response.submap_query_response()); submap_query_response->CopyFrom(client.response().submap_query_response());
return response.error_msg(); return client.response().error_msg();
} }
void MapBuilderStub::SerializeState( void MapBuilderStub::SerializeState(
cartographer::io::ProtoStreamWriterInterface* writer) { cartographer::io::ProtoStreamWriterInterface* writer) {
grpc::ClientContext client_context;
google::protobuf::Empty request; google::protobuf::Empty request;
auto reader = service_stub_->WriteMap(&client_context, request); framework::Client<handlers::WriteMapHandler> client(client_channel_);
CHECK(client.Write(request));
proto::WriteMapResponse response; proto::WriteMapResponse response;
while (reader->Read(&response)) { while (client.Read(&response)) {
writer->WriteProto(response); writer->WriteProto(response);
} }
CHECK(writer->Close()); CHECK(writer->Close());
@ -102,26 +99,22 @@ void MapBuilderStub::SerializeState(
void MapBuilderStub::LoadMap( void MapBuilderStub::LoadMap(
cartographer::io::ProtoStreamReaderInterface* reader) { cartographer::io::ProtoStreamReaderInterface* reader) {
framework::ClientWriter<proto::LoadMapRequest> load_map_writer; framework::Client<handlers::LoadMapHandler> client(client_channel_);
load_map_writer.client_writer = service_stub_->LoadMap(
&load_map_writer.client_context, &load_map_writer.response);
CHECK(load_map_writer.client_writer);
// Request with a PoseGraph proto is sent first. // Request with a PoseGraph proto is sent first.
{ {
proto::LoadMapRequest request; proto::LoadMapRequest request;
CHECK(reader->ReadProto(request.mutable_pose_graph())); CHECK(reader->ReadProto(request.mutable_pose_graph()));
CHECK(load_map_writer.client_writer->Write(request)); CHECK(client.Write(request));
} }
// Multiple requests with SerializedData are sent after. // Multiple requests with SerializedData are sent after.
proto::LoadMapRequest request; proto::LoadMapRequest request;
while (reader->ReadProto(request.mutable_serialized_data())) { while (reader->ReadProto(request.mutable_serialized_data())) {
CHECK(load_map_writer.client_writer->Write(request)); CHECK(client.Write(request));
} }
CHECK(reader->eof()); CHECK(reader->eof());
CHECK(load_map_writer.client_writer->WritesDone()); CHECK(client.WritesDone());
CHECK(client.Finish().ok());
} }
int MapBuilderStub::num_trajectory_builders() const { int MapBuilderStub::num_trajectory_builders() const {

View File

@ -22,7 +22,6 @@
#include "cartographer/mapping/map_builder_interface.h" #include "cartographer/mapping/map_builder_interface.h"
#include "cartographer_grpc/mapping/pose_graph_stub.h" #include "cartographer_grpc/mapping/pose_graph_stub.h"
#include "cartographer_grpc/mapping/trajectory_builder_stub.h" #include "cartographer_grpc/mapping/trajectory_builder_stub.h"
#include "cartographer_grpc/proto/map_builder_service.grpc.pb.h"
#include "grpc++/grpc++.h" #include "grpc++/grpc++.h"
namespace cartographer_grpc { namespace cartographer_grpc {
@ -55,7 +54,6 @@ class MapBuilderStub : public cartographer::mapping::MapBuilderInterface {
private: private:
std::shared_ptr<grpc::Channel> client_channel_; std::shared_ptr<grpc::Channel> client_channel_;
std::unique_ptr<proto::MapBuilderService::Stub> service_stub_;
PoseGraphStub pose_graph_stub_; PoseGraphStub pose_graph_stub_;
std::map<int, std::unique_ptr<TrajectoryBuilderStub>> std::map<int, std::unique_ptr<TrajectoryBuilderStub>>
trajectory_builder_stubs_; trajectory_builder_stubs_;

View File

@ -15,22 +15,26 @@
*/ */
#include "cartographer_grpc/mapping/pose_graph_stub.h" #include "cartographer_grpc/mapping/pose_graph_stub.h"
#include "cartographer/mapping/pose_graph.h" #include "cartographer/mapping/pose_graph.h"
#include "cartographer_grpc/framework/client.h"
#include "cartographer_grpc/handlers/get_all_submap_poses.h"
#include "cartographer_grpc/handlers/get_constraints_handler.h"
#include "cartographer_grpc/handlers/get_local_to_global_transform_handler.h"
#include "cartographer_grpc/handlers/get_trajectory_node_poses_handler.h"
#include "cartographer_grpc/handlers/run_final_optimization_handler.h"
#include "glog/logging.h" #include "glog/logging.h"
namespace cartographer_grpc { namespace cartographer_grpc {
namespace mapping { namespace mapping {
PoseGraphStub::PoseGraphStub(std::shared_ptr<grpc::Channel> client_channel, PoseGraphStub::PoseGraphStub(std::shared_ptr<grpc::Channel> client_channel)
proto::MapBuilderService::Stub* stub) : client_channel_(client_channel) {}
: client_channel_(client_channel), stub_(stub) {}
void PoseGraphStub::RunFinalOptimization() { void PoseGraphStub::RunFinalOptimization() {
grpc::ClientContext client_context;
google::protobuf::Empty request; google::protobuf::Empty request;
google::protobuf::Empty response; framework::Client<handlers::RunFinalOptimizationHandler> client(
CHECK(stub_->RunFinalOptimization(&client_context, request, &response).ok()); client_channel_);
CHECK(client.Write(request));
} }
cartographer::mapping::MapById< cartographer::mapping::MapById<
@ -44,15 +48,14 @@ cartographer::mapping::MapById<
cartographer::mapping::SubmapId, cartographer::mapping::SubmapId,
cartographer::mapping::PoseGraphInterface::SubmapPose> cartographer::mapping::PoseGraphInterface::SubmapPose>
PoseGraphStub::GetAllSubmapPoses() { PoseGraphStub::GetAllSubmapPoses() {
grpc::ClientContext client_context;
google::protobuf::Empty request; google::protobuf::Empty request;
proto::GetAllSubmapPosesResponse response; framework::Client<handlers::GetAllSubmapPosesHandler> client(client_channel_);
stub_->GetAllSubmapPoses(&client_context, request, &response); CHECK(client.Write(request));
cartographer::mapping::MapById< cartographer::mapping::MapById<
cartographer::mapping::SubmapId, cartographer::mapping::SubmapId,
cartographer::mapping::PoseGraphInterface::SubmapPose> cartographer::mapping::PoseGraphInterface::SubmapPose>
submap_poses; submap_poses;
for (const auto& submap_pose : response.submap_poses()) { for (const auto& submap_pose : client.response().submap_poses()) {
submap_poses.Insert( submap_poses.Insert(
cartographer::mapping::SubmapId{submap_pose.submap_id().trajectory_id(), cartographer::mapping::SubmapId{submap_pose.submap_id().trajectory_id(),
submap_pose.submap_id().submap_index()}, submap_pose.submap_id().submap_index()},
@ -65,13 +68,12 @@ PoseGraphStub::GetAllSubmapPoses() {
cartographer::transform::Rigid3d PoseGraphStub::GetLocalToGlobalTransform( cartographer::transform::Rigid3d PoseGraphStub::GetLocalToGlobalTransform(
int trajectory_id) { int trajectory_id) {
grpc::ClientContext client_context;
proto::GetLocalToGlobalTransformRequest request; proto::GetLocalToGlobalTransformRequest request;
request.set_trajectory_id(trajectory_id); request.set_trajectory_id(trajectory_id);
proto::GetLocalToGlobalTransformResponse response; framework::Client<handlers::GetLocalToGlobalTransformHandler> client(
CHECK(stub_->GetLocalToGlobalTransform(&client_context, request, &response) client_channel_);
.ok()); CHECK(client.Write(request));
return cartographer::transform::ToRigid3(response.local_to_global()); return cartographer::transform::ToRigid3(client.response().local_to_global());
} }
cartographer::mapping::MapById<cartographer::mapping::NodeId, cartographer::mapping::MapById<cartographer::mapping::NodeId,
@ -83,14 +85,14 @@ PoseGraphStub::GetTrajectoryNodes() {
cartographer::mapping::MapById<cartographer::mapping::NodeId, cartographer::mapping::MapById<cartographer::mapping::NodeId,
cartographer::mapping::TrajectoryNodePose> cartographer::mapping::TrajectoryNodePose>
PoseGraphStub::GetTrajectoryNodePoses() { PoseGraphStub::GetTrajectoryNodePoses() {
grpc::ClientContext client_context;
google::protobuf::Empty request; google::protobuf::Empty request;
proto::GetTrajectoryNodePosesResponse response; framework::Client<handlers::GetTrajectoryNodePosesHandler> client(
stub_->GetTrajectoryNodePoses(&client_context, request, &response); client_channel_);
CHECK(client.Write(request));
cartographer::mapping::MapById<cartographer::mapping::NodeId, cartographer::mapping::MapById<cartographer::mapping::NodeId,
cartographer::mapping::TrajectoryNodePose> cartographer::mapping::TrajectoryNodePose>
node_poses; node_poses;
for (const auto& node_pose : response.node_poses()) { for (const auto& node_pose : client.response().node_poses()) {
node_poses.Insert( node_poses.Insert(
cartographer::mapping::NodeId{node_pose.node_id().trajectory_id(), cartographer::mapping::NodeId{node_pose.node_id().trajectory_id(),
node_pose.node_id().node_index()}, node_pose.node_id().node_index()},
@ -107,11 +109,10 @@ bool PoseGraphStub::IsTrajectoryFinished(int trajectory_id) {
std::vector<cartographer::mapping::PoseGraphInterface::Constraint> std::vector<cartographer::mapping::PoseGraphInterface::Constraint>
PoseGraphStub::constraints() { PoseGraphStub::constraints() {
grpc::ClientContext client_context;
google::protobuf::Empty request; google::protobuf::Empty request;
proto::GetConstraintsResponse response; framework::Client<handlers::GetConstraintsHandler> client(client_channel_);
stub_->GetConstraints(&client_context, request, &response); CHECK(client.Write(request));
return cartographer::mapping::FromProto(response.constraints()); return cartographer::mapping::FromProto(client.response().constraints());
} }
cartographer::mapping::proto::PoseGraph PoseGraphStub::ToProto() { cartographer::mapping::proto::PoseGraph PoseGraphStub::ToProto() {

View File

@ -18,7 +18,6 @@
#define CARTOGRAPHER_GRPC_MAPPING_POSE_GRAPH_STUB_H_ #define CARTOGRAPHER_GRPC_MAPPING_POSE_GRAPH_STUB_H_
#include "cartographer/mapping/pose_graph_interface.h" #include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer_grpc/proto/map_builder_service.grpc.pb.h"
#include "grpc++/grpc++.h" #include "grpc++/grpc++.h"
namespace cartographer_grpc { namespace cartographer_grpc {
@ -26,8 +25,7 @@ namespace mapping {
class PoseGraphStub : public cartographer::mapping::PoseGraphInterface { class PoseGraphStub : public cartographer::mapping::PoseGraphInterface {
public: public:
PoseGraphStub(std::shared_ptr<grpc::Channel> client_channel, PoseGraphStub(std::shared_ptr<grpc::Channel> client_channel);
proto::MapBuilderService::Stub* stub);
PoseGraphStub(const PoseGraphStub&) = delete; PoseGraphStub(const PoseGraphStub&) = delete;
PoseGraphStub& operator=(const PoseGraphStub&) = delete; PoseGraphStub& operator=(const PoseGraphStub&) = delete;
@ -51,7 +49,6 @@ class PoseGraphStub : public cartographer::mapping::PoseGraphInterface {
private: private:
std::shared_ptr<grpc::Channel> client_channel_; std::shared_ptr<grpc::Channel> client_channel_;
proto::MapBuilderService::Stub* stub_;
}; };
} // namespace mapping } // namespace mapping

View File

@ -27,119 +27,121 @@ namespace mapping {
TrajectoryBuilderStub::TrajectoryBuilderStub( 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) LocalSlamResultCallback local_slam_result_callback)
: client_channel_(client_channel), trajectory_id_(trajectory_id) { : client_channel_(client_channel),
stub_ = proto::MapBuilderService::NewStub(client_channel_); trajectory_id_(trajectory_id),
CHECK(stub_) << "Failed to create stub."; receive_local_slam_results_client_(client_channel_) {
if (local_slam_result_callback) { if (local_slam_result_callback) {
proto::ReceiveLocalSlamResultsRequest request; proto::ReceiveLocalSlamResultsRequest request;
request.set_trajectory_id(trajectory_id); request.set_trajectory_id(trajectory_id);
local_slam_result_reader_.client_reader = stub_->ReceiveLocalSlamResults( receive_local_slam_results_client_.Write(request);
&local_slam_result_reader_.client_context, request); auto* receive_local_slam_results_client_ptr =
auto* client_reader_ptr = local_slam_result_reader_.client_reader.get(); &receive_local_slam_results_client_;
local_slam_result_reader_.thread = receive_local_slam_results_thread_ =
cartographer::common::make_unique<std::thread>( cartographer::common::make_unique<std::thread>(
[client_reader_ptr, local_slam_result_callback]() { [receive_local_slam_results_client_ptr,
RunLocalSlamResultReader(client_reader_ptr, local_slam_result_callback]() {
local_slam_result_callback); RunLocalSlamResultsReader(receive_local_slam_results_client_ptr,
local_slam_result_callback);
}); });
} }
} }
TrajectoryBuilderStub::~TrajectoryBuilderStub() { TrajectoryBuilderStub::~TrajectoryBuilderStub() {
if (local_slam_result_reader_.thread) { if (receive_local_slam_results_thread_) {
local_slam_result_reader_.thread->join(); receive_local_slam_results_thread_->join();
} }
if (rangefinder_writer_.client_writer) { if (add_rangefinder_client_) {
CHECK(rangefinder_writer_.client_writer->WritesDone()); CHECK(add_rangefinder_client_->WritesDone());
CHECK(rangefinder_writer_.client_writer->Finish().ok()); CHECK(add_rangefinder_client_->Finish().ok());
} }
if (imu_writer_.client_writer) { if (add_imu_client_) {
CHECK(imu_writer_.client_writer->WritesDone()); CHECK(add_imu_client_->WritesDone());
CHECK(imu_writer_.client_writer->Finish().ok()); CHECK(add_imu_client_->Finish().ok());
} }
if (odometry_writer_.client_writer) { if (add_odometry_client_) {
CHECK(odometry_writer_.client_writer->WritesDone()); CHECK(add_odometry_client_->WritesDone());
CHECK(odometry_writer_.client_writer->Finish().ok()); CHECK(add_odometry_client_->Finish().ok());
} }
if (fixed_frame_writer_.client_writer) { if (add_landmark_client_) {
CHECK(fixed_frame_writer_.client_writer->WritesDone()); CHECK(add_landmark_client_->WritesDone());
CHECK(fixed_frame_writer_.client_writer->Finish().ok()); CHECK(add_landmark_client_->Finish().ok());
}
if (add_fixed_frame_pose_client_) {
CHECK(add_fixed_frame_pose_client_->WritesDone());
CHECK(add_fixed_frame_pose_client_->Finish().ok());
} }
} }
void TrajectoryBuilderStub::AddSensorData( void TrajectoryBuilderStub::AddSensorData(
const std::string& sensor_id, const std::string& sensor_id,
const cartographer::sensor::TimedPointCloudData& timed_point_cloud_data) { const cartographer::sensor::TimedPointCloudData& timed_point_cloud_data) {
if (!rangefinder_writer_.client_writer) { if (!add_rangefinder_client_) {
rangefinder_writer_.client_writer = stub_->AddRangefinderData( add_rangefinder_client_ = cartographer::common::make_unique<
&rangefinder_writer_.client_context, &rangefinder_writer_.response); framework::Client<handlers::AddRangefinderDataHandler>>(
CHECK(rangefinder_writer_.client_writer); client_channel_);
} }
proto::AddRangefinderDataRequest request; proto::AddRangefinderDataRequest request;
sensor::CreateAddRangeFinderDataRequest( sensor::CreateAddRangeFinderDataRequest(
sensor_id, trajectory_id_, sensor_id, trajectory_id_,
cartographer::sensor::ToProto(timed_point_cloud_data), &request); cartographer::sensor::ToProto(timed_point_cloud_data), &request);
rangefinder_writer_.client_writer->Write(request); add_rangefinder_client_->Write(request);
} }
void TrajectoryBuilderStub::AddSensorData( void TrajectoryBuilderStub::AddSensorData(
const std::string& sensor_id, const std::string& sensor_id,
const cartographer::sensor::ImuData& imu_data) { const cartographer::sensor::ImuData& imu_data) {
if (!imu_writer_.client_writer) { if (!add_imu_client_) {
imu_writer_.client_writer = add_imu_client_ = cartographer::common::make_unique<
stub_->AddImuData(&imu_writer_.client_context, &imu_writer_.response); framework::Client<handlers::AddImuDataHandler>>(client_channel_);
CHECK(imu_writer_.client_writer);
} }
proto::AddImuDataRequest request; proto::AddImuDataRequest request;
sensor::CreateAddImuDataRequest(sensor_id, trajectory_id_, sensor::CreateAddImuDataRequest(sensor_id, trajectory_id_,
cartographer::sensor::ToProto(imu_data), cartographer::sensor::ToProto(imu_data),
&request); &request);
imu_writer_.client_writer->Write(request); add_imu_client_->Write(request);
} }
void TrajectoryBuilderStub::AddSensorData( void TrajectoryBuilderStub::AddSensorData(
const std::string& sensor_id, const std::string& sensor_id,
const cartographer::sensor::OdometryData& odometry_data) { const cartographer::sensor::OdometryData& odometry_data) {
if (!odometry_writer_.client_writer) { if (!add_odometry_client_) {
odometry_writer_.client_writer = stub_->AddOdometryData( add_odometry_client_ = cartographer::common::make_unique<
&odometry_writer_.client_context, &odometry_writer_.response); framework::Client<handlers::AddOdometryDataHandler>>(client_channel_);
CHECK(odometry_writer_.client_writer);
} }
proto::AddOdometryDataRequest request; proto::AddOdometryDataRequest request;
sensor::CreateAddOdometryDataRequest( sensor::CreateAddOdometryDataRequest(
sensor_id, trajectory_id_, cartographer::sensor::ToProto(odometry_data), sensor_id, trajectory_id_, cartographer::sensor::ToProto(odometry_data),
&request); &request);
odometry_writer_.client_writer->Write(request); add_odometry_client_->Write(request);
} }
void TrajectoryBuilderStub::AddSensorData( void TrajectoryBuilderStub::AddSensorData(
const std::string& sensor_id, const std::string& sensor_id,
const cartographer::sensor::FixedFramePoseData& fixed_frame_pose) { const cartographer::sensor::FixedFramePoseData& fixed_frame_pose) {
if (!fixed_frame_writer_.client_writer) { if (!add_fixed_frame_pose_client_) {
fixed_frame_writer_.client_writer = stub_->AddFixedFramePoseData( add_fixed_frame_pose_client_ = cartographer::common::make_unique<
&fixed_frame_writer_.client_context, &fixed_frame_writer_.response); framework::Client<handlers::AddFixedFramePoseDataHandler>>(
CHECK(fixed_frame_writer_.client_writer); client_channel_);
} }
proto::AddFixedFramePoseDataRequest request; proto::AddFixedFramePoseDataRequest request;
sensor::CreateAddFixedFramePoseDataRequest( sensor::CreateAddFixedFramePoseDataRequest(
sensor_id, trajectory_id_, sensor_id, trajectory_id_,
cartographer::sensor::ToProto(fixed_frame_pose), &request); cartographer::sensor::ToProto(fixed_frame_pose), &request);
fixed_frame_writer_.client_writer->Write(request); add_fixed_frame_pose_client_->Write(request);
} }
void TrajectoryBuilderStub::AddSensorData( void TrajectoryBuilderStub::AddSensorData(
const std::string& sensor_id, const std::string& sensor_id,
const cartographer::sensor::LandmarkData& landmark_data) { const cartographer::sensor::LandmarkData& landmark_data) {
if (!landmark_writer_.client_writer) { if (!add_landmark_client_) {
landmark_writer_.client_writer = stub_->AddLandmarkData( add_landmark_client_ = cartographer::common::make_unique<
&landmark_writer_.client_context, &landmark_writer_.response); framework::Client<handlers::AddLandmarkDataHandler>>(client_channel_);
CHECK(landmark_writer_.client_writer);
} }
proto::AddLandmarkDataRequest request; proto::AddLandmarkDataRequest request;
sensor::CreateAddLandmarkDataRequest( sensor::CreateAddLandmarkDataRequest(
sensor_id, trajectory_id_, cartographer::sensor::ToProto(landmark_data), sensor_id, trajectory_id_, cartographer::sensor::ToProto(landmark_data),
&request); &request);
landmark_writer_.client_writer->Write(request); add_landmark_client_->Write(request);
} }
void TrajectoryBuilderStub::AddLocalSlamResultData( void TrajectoryBuilderStub::AddLocalSlamResultData(
@ -148,11 +150,11 @@ void TrajectoryBuilderStub::AddLocalSlamResultData(
LOG(FATAL) << "Not implemented"; LOG(FATAL) << "Not implemented";
} }
void TrajectoryBuilderStub::RunLocalSlamResultReader( void TrajectoryBuilderStub::RunLocalSlamResultsReader(
grpc::ClientReader<proto::ReceiveLocalSlamResultsResponse>* client_reader, framework::Client<handlers::ReceiveLocalSlamResultsHandler>* client,
LocalSlamResultCallback local_slam_result_callback) { LocalSlamResultCallback local_slam_result_callback) {
proto::ReceiveLocalSlamResultsResponse response; proto::ReceiveLocalSlamResultsResponse response;
while (client_reader->Read(&response)) { while (client->Read(&response)) {
int trajectory_id = response.trajectory_id(); int trajectory_id = response.trajectory_id();
cartographer::common::Time time = cartographer::common::Time time =
cartographer::common::FromUniversal(response.timestamp()); cartographer::common::FromUniversal(response.timestamp());
@ -170,7 +172,7 @@ void TrajectoryBuilderStub::RunLocalSlamResultReader(
local_slam_result_callback(trajectory_id, time, local_pose, range_data, local_slam_result_callback(trajectory_id, time, local_pose, range_data,
std::move(insertion_result)); std::move(insertion_result));
} }
client_reader->Finish(); client->Finish();
} }
} // namespace mapping } // namespace mapping

View File

@ -21,8 +21,14 @@
#include "cartographer/mapping/local_slam_result_data.h" #include "cartographer/mapping/local_slam_result_data.h"
#include "cartographer/mapping/trajectory_builder_interface.h" #include "cartographer/mapping/trajectory_builder_interface.h"
#include "cartographer_grpc/framework/client_writer.h" #include "cartographer_grpc/framework/client.h"
#include "cartographer_grpc/proto/map_builder_service.grpc.pb.h" #include "cartographer_grpc/handlers/add_fixed_frame_pose_data_handler.h"
#include "cartographer_grpc/handlers/add_imu_data_handler.h"
#include "cartographer_grpc/handlers/add_landmark_data_handler.h"
#include "cartographer_grpc/handlers/add_local_slam_result_data_handler.h"
#include "cartographer_grpc/handlers/add_odometry_data_handler.h"
#include "cartographer_grpc/handlers/add_rangefinder_data_handler.h"
#include "cartographer_grpc/handlers/receive_local_slam_results_handler.h"
#include "grpc++/grpc++.h" #include "grpc++/grpc++.h"
namespace cartographer_grpc { namespace cartographer_grpc {
@ -57,27 +63,26 @@ class TrajectoryBuilderStub
local_slam_result_data) override; local_slam_result_data) override;
private: private:
struct LocalSlamResultReader { static void RunLocalSlamResultsReader(
grpc::ClientContext client_context; framework::Client<handlers::ReceiveLocalSlamResultsHandler>*
std::unique_ptr<grpc::ClientReader<proto::ReceiveLocalSlamResultsResponse>> client_reader,
client_reader;
std::unique_ptr<std::thread> thread;
};
static void RunLocalSlamResultReader(
grpc::ClientReader<proto::ReceiveLocalSlamResultsResponse>* client_reader,
LocalSlamResultCallback local_slam_result_callback); LocalSlamResultCallback local_slam_result_callback);
std::shared_ptr<grpc::Channel> client_channel_; std::shared_ptr<grpc::Channel> client_channel_;
const int trajectory_id_; const int trajectory_id_;
std::unique_ptr<proto::MapBuilderService::Stub> stub_; std::unique_ptr<framework::Client<handlers::AddRangefinderDataHandler>>
framework::ClientWriter<proto::AddRangefinderDataRequest> rangefinder_writer_; add_rangefinder_client_;
framework::ClientWriter<proto::AddImuDataRequest> imu_writer_; std::unique_ptr<framework::Client<handlers::AddImuDataHandler>>
framework::ClientWriter<proto::AddOdometryDataRequest> odometry_writer_; add_imu_client_;
framework::ClientWriter<proto::AddFixedFramePoseDataRequest> std::unique_ptr<framework::Client<handlers::AddOdometryDataHandler>>
fixed_frame_writer_; add_odometry_client_;
framework::ClientWriter<proto::AddLandmarkDataRequest> landmark_writer_; std::unique_ptr<framework::Client<handlers::AddFixedFramePoseDataHandler>>
LocalSlamResultReader local_slam_result_reader_; add_fixed_frame_pose_client_;
std::unique_ptr<framework::Client<handlers::AddLandmarkDataHandler>>
add_landmark_client_;
framework::Client<handlers::ReceiveLocalSlamResultsHandler>
receive_local_slam_results_client_;
std::unique_ptr<std::thread> receive_local_slam_results_thread_;
}; };
} // namespace mapping } // namespace mapping