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

Enable loading unfrozen state (#891) 

Enable loading unfrozen state
master
Juraj Oršulić 2018-02-27 20:38:57 +01:00 committed by Christoph Schütte
parent 29875117b3
commit 32b8bd3581
16 changed files with 128 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cartographer/io/in_memory_proto_stream.cc
View File

@ -30,8 +30,8 @@ bool ForwardingProtoStreamWriter::Close() { return writer_callback_(nullptr); }
bool InMemoryProtoStreamReader::ReadProto(google::protobuf::Message* proto) { bool InMemoryProtoStreamReader::ReadProto(google::protobuf::Message* proto) {
if (eof()) return false; if (eof()) return false;
proto->CopyFrom(*map_chunks_.front()); proto->CopyFrom(*state_chunks_.front());
map_chunks_.pop(); state_chunks_.pop();
return true; return true;
} }

10
cartographer/io/in_memory_proto_stream.h
View File

@ -51,8 +51,8 @@ class InMemoryProtoStreamReader
: public cartographer::io::ProtoStreamReaderInterface { : public cartographer::io::ProtoStreamReaderInterface {
public: public:
explicit InMemoryProtoStreamReader( explicit InMemoryProtoStreamReader(
std::queue<std::unique_ptr<google::protobuf::Message>>&& map_chunks) std::queue<std::unique_ptr<google::protobuf::Message>>&& state_chunks)
: map_chunks_(std::move(map_chunks)){}; : state_chunks_(std::move(state_chunks)){};
InMemoryProtoStreamReader() = default; InMemoryProtoStreamReader() = default;
~InMemoryProtoStreamReader() = default; ~InMemoryProtoStreamReader() = default;
@ -62,14 +62,14 @@ class InMemoryProtoStreamReader
template <typename MessageType> template <typename MessageType>
void AddProto(const MessageType& proto) { void AddProto(const MessageType& proto) {
map_chunks_.push(common::make_unique<MessageType>(proto)); state_chunks_.push(common::make_unique<MessageType>(proto));
} }
bool ReadProto(google::protobuf::Message* proto) override; bool ReadProto(google::protobuf::Message* proto) override;
bool eof() const override { return map_chunks_.empty(); } bool eof() const override { return state_chunks_.empty(); }
private: private:
std::queue<std::unique_ptr<google::protobuf::Message>> map_chunks_; std::queue<std::unique_ptr<google::protobuf::Message>> state_chunks_;
}; };
} // namespace io } // namespace io

2
cartographer/mapping/2d/pose_graph_2d.cc
View File

@ -439,7 +439,6 @@ void PoseGraph2D::AddSubmapFromProto(
submap_id, submap_id,
pose_graph::OptimizationProblem2D::SubmapData{global_submap_pose_2d}); pose_graph::OptimizationProblem2D::SubmapData{global_submap_pose_2d});
AddWorkItem([this, submap_id, global_submap_pose_2d]() REQUIRES(mutex_) { AddWorkItem([this, submap_id, global_submap_pose_2d]() REQUIRES(mutex_) {
CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1);
submap_data_.at(submap_id).state = SubmapState::kFinished; submap_data_.at(submap_id).state = SubmapState::kFinished;
optimization_problem_.InsertSubmap(submap_id, global_submap_pose_2d); optimization_problem_.InsertSubmap(submap_id, global_submap_pose_2d);
}); });
@ -457,7 +456,6 @@ void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d& global_pose,
trajectory_nodes_.Insert(node_id, TrajectoryNode{constant_data, global_pose}); trajectory_nodes_.Insert(node_id, TrajectoryNode{constant_data, global_pose});
AddWorkItem([this, node_id, global_pose]() REQUIRES(mutex_) { AddWorkItem([this, node_id, global_pose]() REQUIRES(mutex_) {
CHECK_EQ(frozen_trajectories_.count(node_id.trajectory_id), 1);
const auto& constant_data = trajectory_nodes_.at(node_id).constant_data; const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
const auto gravity_alignment_inverse = transform::Rigid3d::Rotation( const auto gravity_alignment_inverse = transform::Rigid3d::Rotation(
constant_data->gravity_alignment.inverse()); constant_data->gravity_alignment.inverse());

2
cartographer/mapping/3d/pose_graph_3d.cc
View File

@ -455,7 +455,6 @@ void PoseGraph3D::AddSubmapFromProto(
submap_id, submap_id,
pose_graph::OptimizationProblem3D::SubmapData{global_submap_pose}); pose_graph::OptimizationProblem3D::SubmapData{global_submap_pose});
AddWorkItem([this, submap_id, global_submap_pose]() REQUIRES(mutex_) { AddWorkItem([this, submap_id, global_submap_pose]() REQUIRES(mutex_) {
CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1);
submap_data_.at(submap_id).state = SubmapState::kFinished; submap_data_.at(submap_id).state = SubmapState::kFinished;
optimization_problem_.InsertSubmap(submap_id, global_submap_pose); optimization_problem_.InsertSubmap(submap_id, global_submap_pose);
}); });
@ -473,7 +472,6 @@ void PoseGraph3D::AddNodeFromProto(const transform::Rigid3d& global_pose,
trajectory_nodes_.Insert(node_id, TrajectoryNode{constant_data, global_pose}); trajectory_nodes_.Insert(node_id, TrajectoryNode{constant_data, global_pose});
AddWorkItem([this, node_id, global_pose]() REQUIRES(mutex_) { AddWorkItem([this, node_id, global_pose]() REQUIRES(mutex_) {
CHECK_EQ(frozen_trajectories_.count(node_id.trajectory_id), 1);
const auto& constant_data = trajectory_nodes_.at(node_id).constant_data; const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
optimization_problem_.InsertTrajectoryNode( optimization_problem_.InsertTrajectoryNode(
node_id, constant_data->time, constant_data->local_pose, global_pose); node_id, constant_data->time, constant_data->local_pose, global_pose);

71
cartographer/mapping/map_builder.cc
View File

@ -276,9 +276,11 @@ void MapBuilder::SerializeState(io::ProtoStreamWriterInterface* const writer) {
writer->WriteProto(proto); writer->WriteProto(proto);
} }
} }
// TODO(pifon2a, ojura): serialize landmarks
} }
void MapBuilder::LoadMap(io::ProtoStreamReaderInterface* const reader) { void MapBuilder::LoadState(io::ProtoStreamReaderInterface* const reader,
bool load_frozen_state) {
proto::PoseGraph pose_graph_proto; proto::PoseGraph pose_graph_proto;
CHECK(reader->ReadProto(&pose_graph_proto)); CHECK(reader->ReadProto(&pose_graph_proto));
proto::AllTrajectoryBuilderOptions all_builder_options_proto; proto::AllTrajectoryBuilderOptions all_builder_options_proto;
@ -298,7 +300,17 @@ void MapBuilder::LoadMap(io::ProtoStreamReaderInterface* const reader) {
.second) .second)
<< "Duplicate trajectory ID: " << trajectory_proto.trajectory_id(); << "Duplicate trajectory ID: " << trajectory_proto.trajectory_id();
trajectory_proto.set_trajectory_id(new_trajectory_id); trajectory_proto.set_trajectory_id(new_trajectory_id);
pose_graph_->FreezeTrajectory(new_trajectory_id); if (load_frozen_state) {
pose_graph_->FreezeTrajectory(new_trajectory_id);
}
}
// Apply the calculated remapping to constraints in the pose graph proto.
for (auto& constraint_proto : *pose_graph_proto.mutable_constraint()) {
constraint_proto.mutable_submap_id()->set_trajectory_id(
trajectory_remapping.at(constraint_proto.submap_id().trajectory_id()));
constraint_proto.mutable_node_id()->set_trajectory_id(
trajectory_remapping.at(constraint_proto.node_id().trajectory_id()));
} }
MapById<SubmapId, transform::Rigid3d> submap_poses; MapById<SubmapId, transform::Rigid3d> submap_poses;
@ -348,24 +360,49 @@ void MapBuilder::LoadMap(io::ProtoStreamReaderInterface* const reader) {
trajectory_remapping.at(proto.trajectory_data().trajectory_id())); trajectory_remapping.at(proto.trajectory_data().trajectory_id()));
pose_graph_->SetTrajectoryDataFromProto(proto.trajectory_data()); pose_graph_->SetTrajectoryDataFromProto(proto.trajectory_data());
} }
// TODO(ojura): Deserialize IMU, odometry and fixed frame pose data when if (!load_frozen_state) {
// loading unfrozen trajectories. if (proto.has_imu_data()) {
pose_graph_->AddImuData(
trajectory_remapping.at(proto.imu_data().trajectory_id()),
sensor::FromProto(proto.imu_data().imu_data()));
}
if (proto.has_odometry_data()) {
pose_graph_->AddOdometryData(
trajectory_remapping.at(proto.odometry_data().trajectory_id()),
sensor::FromProto(proto.odometry_data().odometry_data()));
}
if (proto.has_fixed_frame_pose_data()) {
pose_graph_->AddFixedFramePoseData(
trajectory_remapping.at(
proto.fixed_frame_pose_data().trajectory_id()),
sensor::FromProto(
proto.fixed_frame_pose_data().fixed_frame_pose_data()));
}
// TODO(pifon2a, ojura): deserialize landmarks
}
} }
// Add information about which nodes belong to which submap. if (load_frozen_state) {
for (const proto::PoseGraph::Constraint& constraint_proto : // Add information about which nodes belong to which submap.
pose_graph_proto.constraint()) { // Required for 3D pure localization.
if (constraint_proto.tag() != for (const proto::PoseGraph::Constraint& constraint_proto :
mapping::proto::PoseGraph::Constraint::INTRA_SUBMAP) { pose_graph_proto.constraint()) {
continue; if (constraint_proto.tag() !=
mapping::proto::PoseGraph::Constraint::INTRA_SUBMAP) {
continue;
}
pose_graph_->AddNodeToSubmap(
NodeId{constraint_proto.node_id().trajectory_id(),
constraint_proto.node_id().node_index()},
SubmapId{constraint_proto.submap_id().trajectory_id(),
constraint_proto.submap_id().submap_index()});
} }
const NodeId node_id{ } else {
trajectory_remapping.at(constraint_proto.node_id().trajectory_id()), // When loading unfrozen trajectories, 'AddSerializedConstraints' will
constraint_proto.node_id().node_index()}; // take care of adding information about which nodes belong to which
const SubmapId submap_id{ // submap.
trajectory_remapping.at(constraint_proto.submap_id().trajectory_id()), pose_graph_->AddSerializedConstraints(
constraint_proto.submap_id().submap_index()}; FromProto(pose_graph_proto.constraint()));
pose_graph_->AddNodeToSubmap(node_id, submap_id);
} }
CHECK(reader->eof()); CHECK(reader->eof());
} }

3
cartographer/mapping/map_builder.h
View File

@ -63,7 +63,8 @@ class MapBuilder : public MapBuilderInterface {
void SerializeState(io::ProtoStreamWriterInterface* writer) override; void SerializeState(io::ProtoStreamWriterInterface* writer) override;
void LoadMap(io::ProtoStreamReaderInterface* reader) override; void LoadState(io::ProtoStreamReaderInterface* reader,
bool load_frozen_state) override;
int num_trajectory_builders() const override; int num_trajectory_builders() const override;

5
cartographer/mapping/map_builder_interface.h
View File

@ -80,8 +80,9 @@ class MapBuilderInterface {
// Serializes the current state to a proto stream. // Serializes the current state to a proto stream.
virtual void SerializeState(io::ProtoStreamWriterInterface* writer) = 0; virtual void SerializeState(io::ProtoStreamWriterInterface* writer) = 0;
// Loads submaps from a proto stream into a new frozen trajectory. // Loads the SLAM state from a proto stream.
virtual void LoadMap(io::ProtoStreamReaderInterface* reader) = 0; virtual void LoadState(io::ProtoStreamReaderInterface* reader,
bool load_frozen_state) = 0;
virtual int num_trajectory_builders() const = 0; virtual int num_trajectory_builders() const = 0;

19
cartographer_grpc/handlers/load_map_handler.cc → cartographer_grpc/handlers/load_state_handler.cc
View File

@ -14,7 +14,7 @@
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer_grpc/handlers/load_map_handler.h" #include "cartographer_grpc/handlers/load_state_handler.h"
#include "cartographer/common/make_unique.h" #include "cartographer/common/make_unique.h"
#include "cartographer/io/in_memory_proto_stream.h" #include "cartographer/io/in_memory_proto_stream.h"
@ -26,24 +26,25 @@
namespace cartographer_grpc { namespace cartographer_grpc {
namespace handlers { namespace handlers {
void LoadMapHandler::OnRequest(const proto::LoadMapRequest& request) { void LoadStateHandler::OnRequest(const proto::LoadStateRequest& request) {
switch (request.map_chunk_case()) { switch (request.state_chunk_case()) {
case proto::LoadMapRequest::kPoseGraph: case proto::LoadStateRequest::kPoseGraph:
reader_.AddProto(request.pose_graph()); reader_.AddProto(request.pose_graph());
break; break;
case proto::LoadMapRequest::kAllTrajectoryBuilderOptions: case proto::LoadStateRequest::kAllTrajectoryBuilderOptions:
reader_.AddProto(request.all_trajectory_builder_options()); reader_.AddProto(request.all_trajectory_builder_options());
break; break;
case proto::LoadMapRequest::kSerializedData: case proto::LoadStateRequest::kSerializedData:
reader_.AddProto(request.serialized_data()); reader_.AddProto(request.serialized_data());
break; break;
default: default:
LOG(FATAL) << "Unhandled proto::LoadMapRequest case."; LOG(FATAL) << "Unhandled proto::LoadStateRequest case.";
} }
} }
void LoadMapHandler::OnReadsDone() { void LoadStateHandler::OnReadsDone() {
GetContext<MapBuilderContextInterface>()->map_builder().LoadMap(&reader_); GetContext<MapBuilderContextInterface>()->map_builder().LoadState(&reader_,
true);
Send(cartographer::common::make_unique<google::protobuf::Empty>()); Send(cartographer::common::make_unique<google::protobuf::Empty>());
} }

14
cartographer_grpc/handlers/load_map_handler.h → cartographer_grpc/handlers/load_state_handler.h
View File

@ -14,8 +14,8 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_GRPC_HANDLERS_LOAD_MAP_HANDLER_H #ifndef CARTOGRAPHER_GRPC_HANDLERS_LOAD_STATE_HANDLER_H
#define CARTOGRAPHER_GRPC_HANDLERS_LOAD_MAP_HANDLER_H #define CARTOGRAPHER_GRPC_HANDLERS_LOAD_STATE_HANDLER_H
#include "cartographer/io/in_memory_proto_stream.h" #include "cartographer/io/in_memory_proto_stream.h"
#include "cartographer_grpc/framework/rpc_handler.h" #include "cartographer_grpc/framework/rpc_handler.h"
@ -25,14 +25,14 @@
namespace cartographer_grpc { namespace cartographer_grpc {
namespace handlers { namespace handlers {
class LoadMapHandler class LoadStateHandler
: public framework::RpcHandler<framework::Stream<proto::LoadMapRequest>, : public framework::RpcHandler<framework::Stream<proto::LoadStateRequest>,
google::protobuf::Empty> { google::protobuf::Empty> {
public: public:
std::string method_name() const override { std::string method_name() const override {
return "/cartographer_grpc.proto.MapBuilderService/LoadMap"; return "/cartographer_grpc.proto.MapBuilderService/LoadState";
} }
void OnRequest(const proto::LoadMapRequest& request) override; void OnRequest(const proto::LoadStateRequest& request) override;
void OnReadsDone() override; void OnReadsDone() override;
private: private:
@ -42,4 +42,4 @@ class LoadMapHandler
} // namespace handlers } // namespace handlers
} // namespace cartographer_grpc } // namespace cartographer_grpc
#endif // CARTOGRAPHER_GRPC_HANDLERS_LOAD_MAP_HANDLER_H #endif // CARTOGRAPHER_GRPC_HANDLERS_LOAD_STATE_HANDLER_H

6
cartographer_grpc/handlers/write_map_handler.cc → cartographer_grpc/handlers/write_state_handler.cc
View File

@ -14,7 +14,7 @@
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer_grpc/handlers/write_map_handler.h" #include "cartographer_grpc/handlers/write_state_handler.h"
#include "cartographer/common/make_unique.h" #include "cartographer/common/make_unique.h"
#include "cartographer/io/in_memory_proto_stream.h" #include "cartographer/io/in_memory_proto_stream.h"
@ -26,7 +26,7 @@
namespace cartographer_grpc { namespace cartographer_grpc {
namespace handlers { namespace handlers {
void WriteMapHandler::OnRequest(const google::protobuf::Empty& request) { void WriteStateHandler::OnRequest(const google::protobuf::Empty& request) {
auto writer = GetWriter(); auto writer = GetWriter();
cartographer::io::ForwardingProtoStreamWriter proto_stream_writer( cartographer::io::ForwardingProtoStreamWriter proto_stream_writer(
[writer](const google::protobuf::Message* proto) { [writer](const google::protobuf::Message* proto) {
@ -36,7 +36,7 @@ void WriteMapHandler::OnRequest(const google::protobuf::Empty& request) {
} }
auto response = auto response =
cartographer::common::make_unique<proto::WriteMapResponse>(); cartographer::common::make_unique<proto::WriteStateResponse>();
if (proto->GetTypeName() == "cartographer.mapping.proto.PoseGraph") { if (proto->GetTypeName() == "cartographer.mapping.proto.PoseGraph") {
response->mutable_pose_graph()->CopyFrom(*proto); response->mutable_pose_graph()->CopyFrom(*proto);
} else if (proto->GetTypeName() == } else if (proto->GetTypeName() ==

14
cartographer_grpc/handlers/write_map_handler.h → cartographer_grpc/handlers/write_state_handler.h
View File

@ -14,8 +14,8 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_GRPC_HANDLERS_WRITE_MAP_HANDLER_H #ifndef CARTOGRAPHER_GRPC_HANDLERS_WRITE_STATE_HANDLER_H
#define CARTOGRAPHER_GRPC_HANDLERS_WRITE_MAP_HANDLER_H #define CARTOGRAPHER_GRPC_HANDLERS_WRITE_STATE_HANDLER_H
#include "cartographer_grpc/framework/rpc_handler.h" #include "cartographer_grpc/framework/rpc_handler.h"
#include "cartographer_grpc/proto/map_builder_service.pb.h" #include "cartographer_grpc/proto/map_builder_service.pb.h"
@ -24,12 +24,12 @@
namespace cartographer_grpc { namespace cartographer_grpc {
namespace handlers { namespace handlers {
class WriteMapHandler class WriteStateHandler : public framework::RpcHandler<
: public framework::RpcHandler<google::protobuf::Empty, google::protobuf::Empty,
framework::Stream<proto::WriteMapResponse>> { framework::Stream<proto::WriteStateResponse>> {
public: public:
std::string method_name() const override { std::string method_name() const override {
return "/cartographer_grpc.proto.MapBuilderService/WriteMap"; return "/cartographer_grpc.proto.MapBuilderService/WriteState";
} }
void OnRequest(const google::protobuf::Empty& request) override; void OnRequest(const google::protobuf::Empty& request) override;
}; };
@ -37,4 +37,4 @@ class WriteMapHandler
} // namespace handlers } // namespace handlers
} // namespace cartographer_grpc } // namespace cartographer_grpc
#endif // CARTOGRAPHER_GRPC_HANDLERS_WRITE_MAP_HANDLER_H #endif // CARTOGRAPHER_GRPC_HANDLERS_WRITE_STATE_HANDLER_H

8
cartographer_grpc/map_builder_server.cc
View File

@ -30,10 +30,10 @@
#include "cartographer_grpc/handlers/get_local_to_global_transform_handler.h" #include "cartographer_grpc/handlers/get_local_to_global_transform_handler.h"
#include "cartographer_grpc/handlers/get_submap_handler.h" #include "cartographer_grpc/handlers/get_submap_handler.h"
#include "cartographer_grpc/handlers/get_trajectory_node_poses_handler.h" #include "cartographer_grpc/handlers/get_trajectory_node_poses_handler.h"
#include "cartographer_grpc/handlers/load_map_handler.h" #include "cartographer_grpc/handlers/load_state_handler.h"
#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_state_handler.h"
#include "cartographer_grpc/sensor/serialization.h" #include "cartographer_grpc/sensor/serialization.h"
#include "glog/logging.h" #include "glog/logging.h"
@ -75,9 +75,9 @@ MapBuilderServer::MapBuilderServer(
server_builder.RegisterHandler<handlers::GetAllSubmapPosesHandler>(); server_builder.RegisterHandler<handlers::GetAllSubmapPosesHandler>();
server_builder.RegisterHandler<handlers::GetLocalToGlobalTransformHandler>(); server_builder.RegisterHandler<handlers::GetLocalToGlobalTransformHandler>();
server_builder.RegisterHandler<handlers::GetConstraintsHandler>(); server_builder.RegisterHandler<handlers::GetConstraintsHandler>();
server_builder.RegisterHandler<handlers::LoadMapHandler>(); server_builder.RegisterHandler<handlers::LoadStateHandler>();
server_builder.RegisterHandler<handlers::RunFinalOptimizationHandler>(); server_builder.RegisterHandler<handlers::RunFinalOptimizationHandler>();
server_builder.RegisterHandler<handlers::WriteMapHandler>(); server_builder.RegisterHandler<handlers::WriteStateHandler>();
grpc_server_ = server_builder.Build(); grpc_server_ = server_builder.Build();
grpc_server_->SetExecutionContext( grpc_server_->SetExecutionContext(
cartographer::common::make_unique<MapBuilderContext>(this)); cartographer::common::make_unique<MapBuilderContext>(this));

32
cartographer_grpc/mapping/map_builder_stub.cc
View File

@ -18,8 +18,8 @@
#include "cartographer_grpc/handlers/add_trajectory_handler.h" #include "cartographer_grpc/handlers/add_trajectory_handler.h"
#include "cartographer_grpc/handlers/finish_trajectory_handler.h" #include "cartographer_grpc/handlers/finish_trajectory_handler.h"
#include "cartographer_grpc/handlers/get_submap_handler.h" #include "cartographer_grpc/handlers/get_submap_handler.h"
#include "cartographer_grpc/handlers/load_map_handler.h" #include "cartographer_grpc/handlers/load_state_handler.h"
#include "cartographer_grpc/handlers/write_map_handler.h" #include "cartographer_grpc/handlers/write_state_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"
@ -93,19 +93,19 @@ std::string MapBuilderStub::SubmapToProto(
void MapBuilderStub::SerializeState( void MapBuilderStub::SerializeState(
cartographer::io::ProtoStreamWriterInterface* writer) { cartographer::io::ProtoStreamWriterInterface* writer) {
google::protobuf::Empty request; google::protobuf::Empty request;
framework::Client<handlers::WriteMapHandler> client(client_channel_); framework::Client<handlers::WriteStateHandler> client(client_channel_);
CHECK(client.Write(request)); CHECK(client.Write(request));
proto::WriteMapResponse response; proto::WriteStateResponse response;
while (client.Read(&response)) { while (client.Read(&response)) {
// writer->WriteProto(response); // writer->WriteProto(response);
switch (response.map_chunk_case()) { switch (response.state_chunk_case()) {
case proto::WriteMapResponse::kPoseGraph: case proto::WriteStateResponse::kPoseGraph:
writer->WriteProto(response.pose_graph()); writer->WriteProto(response.pose_graph());
break; break;
case proto::WriteMapResponse::kAllTrajectoryBuilderOptions: case proto::WriteStateResponse::kAllTrajectoryBuilderOptions:
writer->WriteProto(response.all_trajectory_builder_options()); writer->WriteProto(response.all_trajectory_builder_options());
break; break;
case proto::WriteMapResponse::kSerializedData: case proto::WriteStateResponse::kSerializedData:
writer->WriteProto(response.serialized_data()); writer->WriteProto(response.serialized_data());
break; break;
default: default:
@ -115,23 +115,27 @@ void MapBuilderStub::SerializeState(
CHECK(writer->Close()); CHECK(writer->Close());
} }
void MapBuilderStub::LoadMap( void MapBuilderStub::LoadState(
cartographer::io::ProtoStreamReaderInterface* reader) { cartographer::io::ProtoStreamReaderInterface* reader,
framework::Client<handlers::LoadMapHandler> client(client_channel_); const bool load_frozen_state) {
if (!load_frozen_state) {
LOG(FATAL) << "Not implemented";
}
framework::Client<handlers::LoadStateHandler> client(client_channel_);
// Request with a PoseGraph proto is sent first. // Request with a PoseGraph proto is sent first.
{ {
proto::LoadMapRequest request; proto::LoadStateRequest request;
CHECK(reader->ReadProto(request.mutable_pose_graph())); CHECK(reader->ReadProto(request.mutable_pose_graph()));
CHECK(client.Write(request)); CHECK(client.Write(request));
} }
// Request with an AllTrajectoryBuilderOptions should be second. // Request with an AllTrajectoryBuilderOptions should be second.
{ {
proto::LoadMapRequest request; proto::LoadStateRequest request;
CHECK(reader->ReadProto(request.mutable_all_trajectory_builder_options())); CHECK(reader->ReadProto(request.mutable_all_trajectory_builder_options()));
CHECK(client.Write(request)); CHECK(client.Write(request));
} }
// Multiple requests with SerializedData are sent after. // Multiple requests with SerializedData are sent after.
proto::LoadMapRequest request; proto::LoadStateRequest request;
while (reader->ReadProto(request.mutable_serialized_data())) { while (reader->ReadProto(request.mutable_serialized_data())) {
CHECK(client.Write(request)); CHECK(client.Write(request));
} }

3
cartographer_grpc/mapping/map_builder_stub.h
View File

@ -50,7 +50,8 @@ class MapBuilderStub : public cartographer::mapping::MapBuilderInterface {
cartographer::mapping::proto::SubmapQuery::Response* response) override; cartographer::mapping::proto::SubmapQuery::Response* response) override;
void SerializeState( void SerializeState(
cartographer::io::ProtoStreamWriterInterface* writer) override; cartographer::io::ProtoStreamWriterInterface* writer) override;
void LoadMap(cartographer::io::ProtoStreamReaderInterface* reader) override; void LoadState(cartographer::io::ProtoStreamReaderInterface* reader,
bool load_frozen_state) override;
int num_trajectory_builders() const override; int num_trajectory_builders() const override;
cartographer::mapping::PoseGraphInterface* pose_graph() override; cartographer::mapping::PoseGraphInterface* pose_graph() override;
const std::vector< const std::vector<

17
cartographer_grpc/proto/map_builder_service.proto
View File

@ -102,8 +102,8 @@ message GetSubmapRequest {
cartographer.mapping.proto.SubmapId submap_id = 1; cartographer.mapping.proto.SubmapId submap_id = 1;
} }
message LoadMapRequest { message LoadStateRequest {
oneof map_chunk { oneof state_chunk {
cartographer.mapping.proto.PoseGraph pose_graph = 1; cartographer.mapping.proto.PoseGraph pose_graph = 1;
cartographer.mapping.proto.AllTrajectoryBuilderOptions cartographer.mapping.proto.AllTrajectoryBuilderOptions
all_trajectory_builder_options = 2; all_trajectory_builder_options = 2;
@ -162,8 +162,8 @@ message AddLocalSlamResultDataRequest {
cartographer.mapping.proto.LocalSlamResultData local_slam_result_data = 2; cartographer.mapping.proto.LocalSlamResultData local_slam_result_data = 2;
} }
message WriteMapResponse { message WriteStateResponse {
oneof map_chunk { oneof state_chunk {
cartographer.mapping.proto.PoseGraph pose_graph = 1; cartographer.mapping.proto.PoseGraph pose_graph = 1;
cartographer.mapping.proto.AllTrajectoryBuilderOptions cartographer.mapping.proto.AllTrajectoryBuilderOptions
all_trajectory_builder_options = 2; all_trajectory_builder_options = 2;
@ -234,9 +234,10 @@ service MapBuilderService {
rpc RunFinalOptimization(google.protobuf.Empty) rpc RunFinalOptimization(google.protobuf.Empty)
returns (google.protobuf.Empty); returns (google.protobuf.Empty);
// Adds map data in the order defined by ProtoStreamReader. // Adds serialized SLAM state data in the order defined by ProtoStreamReader.
rpc LoadMap(stream LoadMapRequest) returns (google.protobuf.Empty); rpc LoadState(stream LoadStateRequest) returns (google.protobuf.Empty);
// Receives map data in the order defined by ProtoStreamWriter. // Receives serialized SLAM state data in the order defined by
rpc WriteMap(google.protobuf.Empty) returns (stream WriteMapResponse); // ProtoStreamWriter.
rpc WriteState(google.protobuf.Empty) returns (stream WriteStateResponse);
} }

3
cartographer_grpc/testing/mock_map_builder.h
View File

@ -51,7 +51,8 @@ class MockMapBuilder : public cartographer::mapping::MapBuilderInterface {
cartographer::mapping::proto::SubmapQuery::Response *)); cartographer::mapping::proto::SubmapQuery::Response *));
MOCK_METHOD1(SerializeState, MOCK_METHOD1(SerializeState,
void(cartographer::io::ProtoStreamWriterInterface *)); void(cartographer::io::ProtoStreamWriterInterface *));
MOCK_METHOD1(LoadMap, void(cartographer::io::ProtoStreamReaderInterface *)); MOCK_METHOD2(LoadState,
void(cartographer::io::ProtoStreamReaderInterface *, bool));
MOCK_CONST_METHOD0(num_trajectory_builders, int()); MOCK_CONST_METHOD0(num_trajectory_builders, int());
MOCK_METHOD0(pose_graph, cartographer::mapping::PoseGraphInterface *()); MOCK_METHOD0(pose_graph, cartographer::mapping::PoseGraphInterface *());
MOCK_CONST_METHOD0( MOCK_CONST_METHOD0(