diff --git a/cartographer/cloud/internal/client/pose_graph_stub.cc b/cartographer/cloud/internal/client/pose_graph_stub.cc index 78ebd2a..c8bc622 100644 --- a/cartographer/cloud/internal/client/pose_graph_stub.cc +++ b/cartographer/cloud/internal/client/pose_graph_stub.cc @@ -22,10 +22,12 @@ #include "cartographer/cloud/internal/handlers/get_landmark_poses_handler.h" #include "cartographer/cloud/internal/handlers/get_local_to_global_transform_handler.h" #include "cartographer/cloud/internal/handlers/get_trajectory_node_poses_handler.h" +#include "cartographer/cloud/internal/handlers/get_trajectory_states_handler.h" #include "cartographer/cloud/internal/handlers/is_trajectory_finished_handler.h" #include "cartographer/cloud/internal/handlers/is_trajectory_frozen_handler.h" #include "cartographer/cloud/internal/handlers/run_final_optimization_handler.h" #include "cartographer/cloud/internal/handlers/set_landmark_pose_handler.h" +#include "cartographer/cloud/internal/mapping/serialization.h" #include "cartographer/mapping/pose_graph.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" @@ -108,7 +110,16 @@ PoseGraphStub::GetTrajectoryNodePoses() const { std::map PoseGraphStub::GetTrajectoryStates() const { - LOG(FATAL) << "not implemented"; + google::protobuf::Empty request; + async_grpc::Client client( + client_channel_); + CHECK(client.Write(request)); + std::map + trajectories_state; + for (const auto& entry : client.response().trajectories_state()) { + trajectories_state[entry.first] = FromProto(entry.second); + } + return trajectories_state; } std::map PoseGraphStub::GetLandmarkPoses() diff --git a/cartographer/cloud/internal/client_server_test.cc b/cartographer/cloud/internal/client_server_test.cc index c4aaa97..697398b 100644 --- a/cartographer/cloud/internal/client_server_test.cc +++ b/cartographer/cloud/internal/client_server_test.cc @@ -297,6 +297,7 @@ TEST_F(ClientServerTest, LocalSlam2D) { InitializeRealServer(); server_->Start(); InitializeStub(); + EXPECT_TRUE(stub_->pose_graph()->GetTrajectoryStates().empty()); int trajectory_id = stub_->AddTrajectoryBuilder({kRangeSensorId}, trajectory_builder_options_, local_slam_result_callback_); @@ -308,7 +309,12 @@ TEST_F(ClientServerTest, LocalSlam2D) { trajectory_stub->AddSensorData(kRangeSensorId.id, measurement); } WaitForLocalSlamResults(measurements.size()); + EXPECT_EQ(stub_->pose_graph()->GetTrajectoryStates().at(trajectory_id), + PoseGraphInterface::TrajectoryState::ACTIVE); stub_->FinishTrajectory(trajectory_id); + stub_->pose_graph()->RunFinalOptimization(); + EXPECT_EQ(stub_->pose_graph()->GetTrajectoryStates().at(trajectory_id), + PoseGraphInterface::TrajectoryState::FINISHED); EXPECT_EQ(local_slam_result_poses_.size(), measurements.size()); EXPECT_NEAR(kTravelDistance, (local_slam_result_poses_.back().translation() - @@ -334,16 +340,15 @@ TEST_F(ClientServerTest, LocalSlamAndDelete2D) { } WaitForLocalSlamResults(measurements.size()); stub_->pose_graph()->RunFinalOptimization(); - // TODO(gaschler): Enable after pending PR has merged. - // EXPECT_EQ(stub_->pose_graph()->GetTrajectoryStates().at(trajectory_id), - // PoseGraphInterface::TrajectoryState::ACTIVE); + EXPECT_EQ(stub_->pose_graph()->GetTrajectoryStates().at(trajectory_id), + PoseGraphInterface::TrajectoryState::ACTIVE); EXPECT_GT(stub_->pose_graph()->GetAllSubmapPoses().size(), 0); EXPECT_GT(stub_->pose_graph()->GetTrajectoryNodePoses().size(), 0); stub_->FinishTrajectory(trajectory_id); stub_->pose_graph()->DeleteTrajectory(trajectory_id); stub_->pose_graph()->RunFinalOptimization(); - // EXPECT_EQ(stub_->pose_graph()->GetTrajectoryStates().at(trajectory_id), - // PoseGraphInterface::TrajectoryState::DELETED); + EXPECT_EQ(stub_->pose_graph()->GetTrajectoryStates().at(trajectory_id), + PoseGraphInterface::TrajectoryState::DELETED); EXPECT_EQ(stub_->pose_graph()->GetAllSubmapPoses().size(), 0); EXPECT_EQ(stub_->pose_graph()->GetTrajectoryNodePoses().size(), 0); server_->Shutdown(); diff --git a/cartographer/cloud/internal/handlers/get_trajectory_states_handler.cc b/cartographer/cloud/internal/handlers/get_trajectory_states_handler.cc new file mode 100644 index 0000000..8b29388 --- /dev/null +++ b/cartographer/cloud/internal/handlers/get_trajectory_states_handler.cc @@ -0,0 +1,46 @@ +/* + * 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. + */ + +#include "cartographer/cloud/internal/handlers/get_trajectory_states_handler.h" + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/internal/mapping/serialization.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "cartographer/common/make_unique.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +void GetTrajectoryStatesHandler::OnRequest( + const google::protobuf::Empty& request) { + auto trajectories_state = GetContext() + ->map_builder() + .pose_graph() + ->GetTrajectoryStates(); + auto response = common::make_unique(); + for (const auto& entry : trajectories_state) { + (*response->mutable_trajectories_state())[entry.first] = + ToProto(entry.second); + } + Send(std::move(response)); +} + +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/cartographer/cloud/internal/handlers/get_trajectory_states_handler.h b/cartographer/cloud/internal/handlers/get_trajectory_states_handler.h new file mode 100644 index 0000000..0a311fd --- /dev/null +++ b/cartographer/cloud/internal/handlers/get_trajectory_states_handler.h @@ -0,0 +1,43 @@ +/* + * 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_CLOUD_INTERNAL_HANDLERS_GET_TRAJECTORY_STATES_HANDLER_H +#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_GET_TRAJECTORY_STATES_HANDLER_H + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +DEFINE_HANDLER_SIGNATURE( + GetTrajectoryStatesSignature, google::protobuf::Empty, + proto::GetTrajectoryStatesResponse, + "/cartographer.cloud.proto.MapBuilderService/GetTrajectoryStates") + +class GetTrajectoryStatesHandler + : public async_grpc::RpcHandler { + public: + void OnRequest(const google::protobuf::Empty& request) override; +}; + +} // namespace handlers +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_GET_TRAJECTORY_STATES_HANDLER_H diff --git a/cartographer/cloud/internal/map_builder_server.cc b/cartographer/cloud/internal/map_builder_server.cc index 16be45e..427489f 100644 --- a/cartographer/cloud/internal/map_builder_server.cc +++ b/cartographer/cloud/internal/map_builder_server.cc @@ -31,6 +31,7 @@ #include "cartographer/cloud/internal/handlers/get_local_to_global_transform_handler.h" #include "cartographer/cloud/internal/handlers/get_submap_handler.h" #include "cartographer/cloud/internal/handlers/get_trajectory_node_poses_handler.h" +#include "cartographer/cloud/internal/handlers/get_trajectory_states_handler.h" #include "cartographer/cloud/internal/handlers/is_trajectory_finished_handler.h" #include "cartographer/cloud/internal/handlers/is_trajectory_frozen_handler.h" #include "cartographer/cloud/internal/handlers/load_state_from_file_handler.h" @@ -82,6 +83,7 @@ MapBuilderServer::MapBuilderServer( server_builder.RegisterHandler(); server_builder.RegisterHandler(); server_builder.RegisterHandler(); + server_builder.RegisterHandler(); server_builder.RegisterHandler(); server_builder.RegisterHandler(); server_builder.RegisterHandler(); diff --git a/cartographer/cloud/internal/mapping/serialization.cc b/cartographer/cloud/internal/mapping/serialization.cc index d5a6dcf..b6a56e9 100644 --- a/cartographer/cloud/internal/mapping/serialization.cc +++ b/cartographer/cloud/internal/mapping/serialization.cc @@ -20,6 +20,42 @@ namespace cartographer { namespace cloud { +namespace { + +using TrajectoryState = + ::cartographer::mapping::PoseGraphInterface::TrajectoryState; + +} // namespace + +proto::TrajectoryState ToProto(const TrajectoryState& trajectory_state) { + switch (trajectory_state) { + case TrajectoryState::ACTIVE: + return proto::TrajectoryState::ACTIVE; + case TrajectoryState::FINISHED: + return proto::TrajectoryState::FINISHED; + case TrajectoryState::FROZEN: + return proto::TrajectoryState::FROZEN; + case TrajectoryState::DELETED: + return proto::TrajectoryState::DELETED; + default: + LOG(FATAL) << "Unknown TrajectoryState"; + } +} + +TrajectoryState FromProto(const proto::TrajectoryState& proto) { + switch (proto) { + case proto::TrajectoryState::ACTIVE: + return TrajectoryState::ACTIVE; + case proto::TrajectoryState::FINISHED: + return TrajectoryState::FINISHED; + case proto::TrajectoryState::FROZEN: + return TrajectoryState::FROZEN; + case proto::TrajectoryState::DELETED: + return TrajectoryState::DELETED; + default: + LOG(FATAL) << "Unknown proto::TrajectoryState"; + } +} proto::TrajectoryRemapping ToProto( const std::map& trajectory_remapping) { diff --git a/cartographer/cloud/internal/mapping/serialization.h b/cartographer/cloud/internal/mapping/serialization.h index 4b21bf7..2dd9ac4 100644 --- a/cartographer/cloud/internal/mapping/serialization.h +++ b/cartographer/cloud/internal/mapping/serialization.h @@ -18,10 +18,16 @@ #define CARTOGRAPHER_CLOUD_INTERNAL_MAPPING_SERIALIZATION_H #include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "cartographer/mapping/pose_graph_interface.h" namespace cartographer { namespace cloud { +proto::TrajectoryState ToProto( + const mapping::PoseGraphInterface::TrajectoryState& trajectory_state); +mapping::PoseGraphInterface::TrajectoryState FromProto( + const proto::TrajectoryState& proto); + proto::TrajectoryRemapping ToProto( const std::map& trajectory_remapping); std::map FromProto(const proto::TrajectoryRemapping& proto); diff --git a/cartographer/cloud/proto/map_builder_service.proto b/cartographer/cloud/proto/map_builder_service.proto index 465fa69..ac68114 100644 --- a/cartographer/cloud/proto/map_builder_service.proto +++ b/cartographer/cloud/proto/map_builder_service.proto @@ -33,6 +33,13 @@ enum SensorType { LOCAL_SLAM_RESULT = 5; } +enum TrajectoryState { + ACTIVE = 0; + FINISHED = 1; + FROZEN = 2; + DELETED = 3; +} + message SensorId { string id = 1; SensorType type = 2; @@ -172,6 +179,10 @@ message GetTrajectoryNodePosesResponse { repeated TrajectoryNodePose node_poses = 1; } +message GetTrajectoryStatesResponse { + map trajectories_state = 1; +} + message GetLandmarkPosesResponse { repeated cartographer.mapping.proto.PoseGraph.LandmarkPose landmark_poses = 1; } @@ -276,6 +287,10 @@ service MapBuilderService { rpc GetTrajectoryNodePoses(google.protobuf.Empty) returns (GetTrajectoryNodePosesResponse); + // Returns the states of trajectories. + rpc GetTrajectoryStates(google.protobuf.Empty) + returns (GetTrajectoryStatesResponse); + // Returns the current optimized landmark poses. rpc GetLandmarkPoses(google.protobuf.Empty) returns (GetLandmarkPosesResponse);