Add GetTrajectoryNodePoses() to gRPC service (#796)

master
Christoph Schütte 2018-01-08 14:19:39 +01:00 committed by Wally B. Feed
parent d57c2441b8
commit 78d05bf745
5 changed files with 93 additions and 1 deletions

View File

@ -53,6 +53,11 @@ struct NodeId {
return std::forward_as_tuple(trajectory_id, node_index) <
std::forward_as_tuple(other.trajectory_id, other.node_index);
}
void ToProto(proto::NodeId* proto) const {
proto->set_trajectory_id(trajectory_id);
proto->set_node_index(node_index);
}
};
inline std::ostream& operator<<(std::ostream& os, const NodeId& v) {

View File

@ -0,0 +1,54 @@
/*
* 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_HANDLERS_GET_TRAJECTORY_NODE_POSES_HANDLER_H
#define CARTOGRAPHER_GRPC_HANDLERS_GET_TRAJECTORY_NODE_POSES_HANDLER_H
#include "cartographer/common/make_unique.h"
#include "cartographer_grpc/framework/rpc_handler.h"
#include "cartographer_grpc/map_builder_server.h"
#include "cartographer_grpc/proto/map_builder_service.pb.h"
#include "google/protobuf/empty.pb.h"
namespace cartographer_grpc {
namespace handlers {
class GetTrajectoryNodePosesHandler
: public framework::RpcHandler<google::protobuf::Empty,
proto::GetTrajectoryNodePosesResponse> {
public:
void OnRequest(const google::protobuf::Empty& request) override {
auto node_poses = GetContext<MapBuilderServer::MapBuilderContext>()
->map_builder()
.pose_graph()
->GetTrajectoryNodePoses();
auto response = cartographer::common::make_unique<
proto::GetTrajectoryNodePosesResponse>();
for (const auto& node_id_pose : node_poses) {
auto* node_pose = response->add_node_poses();
node_id_pose.id.ToProto(node_pose->mutable_node_id());
*node_pose->mutable_global_pose() =
cartographer::transform::ToProto(node_id_pose.data.global_pose);
node_pose->set_has_constant_data(node_id_pose.data.has_constant_data);
}
Send(std::move(response));
}
};
} // namespace handlers
} // namespace cartographer_grpc
#endif // CARTOGRAPHER_GRPC_HANDLERS_GET_TRAJECTORY_NODE_POSES_HANDLER_H

View File

@ -23,6 +23,7 @@
#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/get_trajectory_node_poses_handler.h"
#include "cartographer_grpc/handlers/receive_local_slam_results_handler.h"
#include "cartographer_grpc/proto/map_builder_service.grpc.pb.h"
#include "glog/logging.h"
@ -120,6 +121,9 @@ MapBuilderServer::MapBuilderServer(
server_builder
.RegisterHandler<handlers::GetSubmapHandler, proto::MapBuilderService>(
"GetSubmap");
server_builder.RegisterHandler<handlers::GetTrajectoryNodePosesHandler,
proto::MapBuilderService>(
"GetTrajectoryNodePoses");
grpc_server_ = server_builder.Build();
grpc_server_->SetExecutionContext(
cartographer::common::make_unique<MapBuilderContext>(this));

View File

@ -55,7 +55,22 @@ PoseGraphStub::GetTrajectoryNodes() {
cartographer::mapping::MapById<cartographer::mapping::NodeId,
cartographer::mapping::TrajectoryNodePose>
PoseGraphStub::GetTrajectoryNodePoses() {
LOG(FATAL) << "Not implemented";
grpc::ClientContext client_context;
google::protobuf::Empty request;
proto::GetTrajectoryNodePosesResponse response;
stub_->GetTrajectoryNodePoses(&client_context, request, &response);
cartographer::mapping::MapById<cartographer::mapping::NodeId,
cartographer::mapping::TrajectoryNodePose>
node_poses;
for (const auto& node_pose : response.node_poses()) {
node_poses.Insert(
cartographer::mapping::NodeId{node_pose.node_id().trajectory_id(),
node_pose.node_id().node_index()},
cartographer::mapping::TrajectoryNodePose{
node_pose.has_constant_data(),
cartographer::transform::ToRigid3(node_pose.global_pose())});
}
return node_poses;
}
bool PoseGraphStub::IsTrajectoryFinished(int trajectory_id) {

View File

@ -83,6 +83,16 @@ message GetSubmapResponse {
string error_msg = 2;
}
message TrajectoryNodePose {
cartographer.mapping.proto.NodeId node_id = 1;
cartographer.transform.proto.Rigid3d global_pose = 2;
bool has_constant_data = 3;
}
message GetTrajectoryNodePosesResponse {
repeated TrajectoryNodePose node_poses = 1;
}
service MapBuilderService {
// Starts a new trajectory and returns its index.
rpc AddTrajectory(AddTrajectoryRequest) returns (AddTrajectoryResponse);
@ -114,4 +124,8 @@ service MapBuilderService {
// Retrieves a single submap.
rpc GetSubmap(GetSubmapRequest) returns (GetSubmapResponse);
// Returns the current optimized trajectory poses.
rpc GetTrajectoryNodePoses(google.protobuf.Empty)
returns (GetTrajectoryNodePosesResponse);
}