2017-12-13 22:53:47 +08:00
|
|
|
/*
|
|
|
|
* Copyright 2017 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.
|
|
|
|
*/
|
|
|
|
|
|
|
|
#include "cartographer_grpc/mapping/map_builder_stub.h"
|
2018-02-02 00:19:15 +08:00
|
|
|
#include "cartographer_grpc/handlers/add_trajectory_handler.h"
|
|
|
|
#include "cartographer_grpc/handlers/finish_trajectory_handler.h"
|
|
|
|
#include "cartographer_grpc/handlers/get_submap_handler.h"
|
2018-02-28 03:38:57 +08:00
|
|
|
#include "cartographer_grpc/handlers/load_state_handler.h"
|
|
|
|
#include "cartographer_grpc/handlers/write_state_handler.h"
|
2017-12-13 22:53:47 +08:00
|
|
|
#include "cartographer_grpc/proto/map_builder_service.pb.h"
|
2018-01-26 22:07:49 +08:00
|
|
|
#include "cartographer_grpc/sensor/serialization.h"
|
2017-12-13 22:53:47 +08:00
|
|
|
#include "glog/logging.h"
|
|
|
|
|
|
|
|
namespace cartographer_grpc {
|
|
|
|
namespace mapping {
|
|
|
|
|
|
|
|
MapBuilderStub::MapBuilderStub(const std::string& server_address)
|
|
|
|
: client_channel_(grpc::CreateChannel(server_address,
|
|
|
|
grpc::InsecureChannelCredentials())),
|
2018-02-02 00:19:15 +08:00
|
|
|
pose_graph_stub_(client_channel_) {}
|
2017-12-13 22:53:47 +08:00
|
|
|
|
|
|
|
int MapBuilderStub::AddTrajectoryBuilder(
|
2018-01-26 22:07:49 +08:00
|
|
|
const std::set<SensorId>& expected_sensor_ids,
|
2017-12-13 22:53:47 +08:00
|
|
|
const cartographer::mapping::proto::TrajectoryBuilderOptions&
|
|
|
|
trajectory_options,
|
|
|
|
LocalSlamResultCallback local_slam_result_callback) {
|
|
|
|
proto::AddTrajectoryRequest request;
|
|
|
|
*request.mutable_trajectory_builder_options() = trajectory_options;
|
|
|
|
for (const auto& sensor_id : expected_sensor_ids) {
|
2018-01-26 22:07:49 +08:00
|
|
|
*request.add_expected_sensor_ids() = sensor::ToProto(sensor_id);
|
2017-12-13 22:53:47 +08:00
|
|
|
}
|
2018-02-02 05:31:33 +08:00
|
|
|
framework::Client<handlers::AddTrajectoryHandler> client(
|
|
|
|
client_channel_,
|
|
|
|
framework::CreateLimitedBackoffStrategy(
|
|
|
|
cartographer::common::FromMilliseconds(100), 2.f, 5));
|
2018-02-02 00:19:15 +08:00
|
|
|
CHECK(client.Write(request));
|
2017-12-13 22:53:47 +08:00
|
|
|
|
|
|
|
// Construct trajectory builder stub.
|
|
|
|
trajectory_builder_stubs_.emplace(
|
2018-02-02 00:19:15 +08:00
|
|
|
std::piecewise_construct,
|
|
|
|
std::forward_as_tuple(client.response().trajectory_id()),
|
2017-12-13 22:53:47 +08:00
|
|
|
std::forward_as_tuple(cartographer::common::make_unique<
|
|
|
|
cartographer_grpc::mapping::TrajectoryBuilderStub>(
|
2018-02-02 00:19:15 +08:00
|
|
|
client_channel_, client.response().trajectory_id(),
|
2017-12-20 19:42:27 +08:00
|
|
|
local_slam_result_callback)));
|
2018-02-02 00:19:15 +08:00
|
|
|
return client.response().trajectory_id();
|
2017-12-13 22:53:47 +08:00
|
|
|
}
|
|
|
|
|
2018-02-05 16:45:43 +08:00
|
|
|
int MapBuilderStub::AddTrajectoryForDeserialization(
|
|
|
|
const cartographer::mapping::proto::TrajectoryBuilderOptionsWithSensorIds&
|
|
|
|
options_with_sensor_ids_proto) {
|
2017-12-13 22:53:47 +08:00
|
|
|
LOG(FATAL) << "Not implemented";
|
|
|
|
}
|
|
|
|
|
|
|
|
cartographer::mapping::TrajectoryBuilderInterface*
|
|
|
|
MapBuilderStub::GetTrajectoryBuilder(int trajectory_id) const {
|
|
|
|
return trajectory_builder_stubs_.at(trajectory_id).get();
|
|
|
|
}
|
|
|
|
|
|
|
|
void MapBuilderStub::FinishTrajectory(int trajectory_id) {
|
|
|
|
proto::FinishTrajectoryRequest request;
|
|
|
|
request.set_trajectory_id(trajectory_id);
|
2018-02-02 00:19:15 +08:00
|
|
|
framework::Client<handlers::FinishTrajectoryHandler> client(client_channel_);
|
|
|
|
CHECK(client.Write(request));
|
2017-12-13 22:53:47 +08:00
|
|
|
trajectory_builder_stubs_.erase(trajectory_id);
|
|
|
|
}
|
|
|
|
|
|
|
|
std::string MapBuilderStub::SubmapToProto(
|
|
|
|
const cartographer::mapping::SubmapId& submap_id,
|
2018-01-08 20:52:14 +08:00
|
|
|
cartographer::mapping::proto::SubmapQuery::Response*
|
|
|
|
submap_query_response) {
|
|
|
|
proto::GetSubmapRequest request;
|
|
|
|
submap_id.ToProto(request.mutable_submap_id());
|
2018-02-02 00:19:15 +08:00
|
|
|
framework::Client<handlers::GetSubmapHandler> client(client_channel_);
|
|
|
|
CHECK(client.Write(request));
|
|
|
|
submap_query_response->CopyFrom(client.response().submap_query_response());
|
|
|
|
return client.response().error_msg();
|
2017-12-13 22:53:47 +08:00
|
|
|
}
|
|
|
|
|
|
|
|
void MapBuilderStub::SerializeState(
|
2018-01-25 22:27:50 +08:00
|
|
|
cartographer::io::ProtoStreamWriterInterface* writer) {
|
2018-02-01 18:21:50 +08:00
|
|
|
google::protobuf::Empty request;
|
2018-02-28 03:38:57 +08:00
|
|
|
framework::Client<handlers::WriteStateHandler> client(client_channel_);
|
2018-02-02 00:19:15 +08:00
|
|
|
CHECK(client.Write(request));
|
2018-02-28 03:38:57 +08:00
|
|
|
proto::WriteStateResponse response;
|
2018-02-02 00:19:15 +08:00
|
|
|
while (client.Read(&response)) {
|
2018-02-07 23:35:44 +08:00
|
|
|
// writer->WriteProto(response);
|
2018-02-28 03:38:57 +08:00
|
|
|
switch (response.state_chunk_case()) {
|
|
|
|
case proto::WriteStateResponse::kPoseGraph:
|
2018-02-07 23:35:44 +08:00
|
|
|
writer->WriteProto(response.pose_graph());
|
|
|
|
break;
|
2018-02-28 03:38:57 +08:00
|
|
|
case proto::WriteStateResponse::kAllTrajectoryBuilderOptions:
|
2018-02-07 23:35:44 +08:00
|
|
|
writer->WriteProto(response.all_trajectory_builder_options());
|
|
|
|
break;
|
2018-02-28 03:38:57 +08:00
|
|
|
case proto::WriteStateResponse::kSerializedData:
|
2018-02-07 23:35:44 +08:00
|
|
|
writer->WriteProto(response.serialized_data());
|
|
|
|
break;
|
|
|
|
default:
|
|
|
|
LOG(FATAL) << "Unhandled message type";
|
|
|
|
}
|
2018-02-01 18:21:50 +08:00
|
|
|
}
|
|
|
|
CHECK(writer->Close());
|
2017-12-13 22:53:47 +08:00
|
|
|
}
|
|
|
|
|
2018-02-28 03:38:57 +08:00
|
|
|
void MapBuilderStub::LoadState(
|
|
|
|
cartographer::io::ProtoStreamReaderInterface* reader,
|
|
|
|
const bool load_frozen_state) {
|
|
|
|
if (!load_frozen_state) {
|
|
|
|
LOG(FATAL) << "Not implemented";
|
|
|
|
}
|
|
|
|
framework::Client<handlers::LoadStateHandler> client(client_channel_);
|
2018-01-25 16:27:42 +08:00
|
|
|
// Request with a PoseGraph proto is sent first.
|
|
|
|
{
|
2018-02-28 03:38:57 +08:00
|
|
|
proto::LoadStateRequest request;
|
2018-01-25 16:27:42 +08:00
|
|
|
CHECK(reader->ReadProto(request.mutable_pose_graph()));
|
2018-02-20 23:00:01 +08:00
|
|
|
CHECK(client.Write(request));
|
|
|
|
}
|
|
|
|
// Request with an AllTrajectoryBuilderOptions should be second.
|
|
|
|
{
|
2018-02-28 03:38:57 +08:00
|
|
|
proto::LoadStateRequest request;
|
2018-02-07 23:35:44 +08:00
|
|
|
CHECK(reader->ReadProto(request.mutable_all_trajectory_builder_options()));
|
2018-02-02 00:19:15 +08:00
|
|
|
CHECK(client.Write(request));
|
2018-01-25 16:27:42 +08:00
|
|
|
}
|
|
|
|
// Multiple requests with SerializedData are sent after.
|
2018-02-28 03:38:57 +08:00
|
|
|
proto::LoadStateRequest request;
|
2018-01-25 16:27:42 +08:00
|
|
|
while (reader->ReadProto(request.mutable_serialized_data())) {
|
2018-02-02 00:19:15 +08:00
|
|
|
CHECK(client.Write(request));
|
2018-01-25 16:27:42 +08:00
|
|
|
}
|
|
|
|
|
|
|
|
CHECK(reader->eof());
|
2018-02-02 00:19:15 +08:00
|
|
|
CHECK(client.WritesDone());
|
|
|
|
CHECK(client.Finish().ok());
|
2017-12-13 22:53:47 +08:00
|
|
|
}
|
|
|
|
|
|
|
|
int MapBuilderStub::num_trajectory_builders() const {
|
2018-01-25 15:59:52 +08:00
|
|
|
return trajectory_builder_stubs_.size();
|
2017-12-13 22:53:47 +08:00
|
|
|
}
|
|
|
|
|
|
|
|
cartographer::mapping::PoseGraphInterface* MapBuilderStub::pose_graph() {
|
|
|
|
return &pose_graph_stub_;
|
|
|
|
}
|
|
|
|
|
2018-02-05 16:45:43 +08:00
|
|
|
const std::vector<
|
|
|
|
cartographer::mapping::proto::TrajectoryBuilderOptionsWithSensorIds>&
|
|
|
|
MapBuilderStub::GetAllTrajectoryBuilderOptions() const {
|
|
|
|
LOG(FATAL) << "Not implemented";
|
|
|
|
}
|
|
|
|
|
2017-12-13 22:53:47 +08:00
|
|
|
} // namespace mapping
|
|
|
|
} // namespace cartographer_grpc
|