diff --git a/cartographer_grpc/mapping/map_builder_stub.cc b/cartographer_grpc/mapping/map_builder_stub.cc new file mode 100644 index 0000000..adf7576 --- /dev/null +++ b/cartographer_grpc/mapping/map_builder_stub.cc @@ -0,0 +1,98 @@ +/* + * 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" + +#include "cartographer_grpc/proto/map_builder_service.pb.h" +#include "glog/logging.h" + +namespace cartographer_grpc { +namespace mapping { + +MapBuilderStub::MapBuilderStub(const std::string& server_address) + : client_channel_(grpc::CreateChannel(server_address, + grpc::InsecureChannelCredentials())), + service_stub_(proto::MapBuilderService::NewStub(client_channel_)), + pose_graph_stub_(client_channel_, service_stub_.get()) {} + +int MapBuilderStub::AddTrajectoryBuilder( + const std::unordered_set& expected_sensor_ids, + const cartographer::mapping::proto::TrajectoryBuilderOptions& + trajectory_options, + LocalSlamResultCallback local_slam_result_callback) { + proto::AddTrajectoryRequest request; + proto::AddTrajectoryResponse result; + *request.mutable_trajectory_builder_options() = trajectory_options; + for (const auto& sensor_id : expected_sensor_ids) { + *request.add_expected_sensor_ids() = sensor_id; + } + grpc::Status status = + service_stub_->AddTrajectory(&client_context_, request, &result); + CHECK(status.ok()); + + // Construct trajectory builder stub. + trajectory_builder_stubs_.emplace( + std::piecewise_construct, std::forward_as_tuple(result.trajectory_id()), + std::forward_as_tuple(cartographer::common::make_unique< + cartographer_grpc::mapping::TrajectoryBuilderStub>( + client_channel_, service_stub_.get()))); + return result.trajectory_id(); +} + +int MapBuilderStub::AddTrajectoryForDeserialization() { + 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; + google::protobuf::Empty response; + request.set_trajectory_id(trajectory_id); + grpc::Status status = + service_stub_->FinishTrajectory(&client_context_, request, &response); + CHECK(status.ok()); + trajectory_builder_stubs_.erase(trajectory_id); +} + +std::string MapBuilderStub::SubmapToProto( + const cartographer::mapping::SubmapId& submap_id, + cartographer::mapping::proto::SubmapQuery::Response* response) { + LOG(FATAL) << "Not implemented"; +} + +void MapBuilderStub::SerializeState( + cartographer::io::ProtoStreamWriter* writer) { + LOG(FATAL) << "Not implemented"; +} + +void MapBuilderStub::LoadMap(cartographer::io::ProtoStreamReader* reader) { + LOG(FATAL) << "Not implemented"; +} + +int MapBuilderStub::num_trajectory_builders() const { + LOG(FATAL) << "Not implemented"; +} + +cartographer::mapping::PoseGraphInterface* MapBuilderStub::pose_graph() { + return &pose_graph_stub_; +} + +} // namespace mapping +} // namespace cartographer_grpc diff --git a/cartographer_grpc/mapping/map_builder_stub.h b/cartographer_grpc/mapping/map_builder_stub.h new file mode 100644 index 0000000..80c7bb8 --- /dev/null +++ b/cartographer_grpc/mapping/map_builder_stub.h @@ -0,0 +1,67 @@ +/* + * 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. + */ + +#ifndef CARTOGRAPHER_GRPC_MAPPING_MAP_BUILDER_STUB_H_ +#define CARTOGRAPHER_GRPC_MAPPING_MAP_BUILDER_STUB_H_ + +#include + +#include "cartographer/mapping/map_builder_interface.h" +#include "cartographer_grpc/mapping/pose_graph_stub.h" +#include "cartographer_grpc/mapping/trajectory_builder_stub.h" +#include "cartographer_grpc/proto/map_builder_service.grpc.pb.h" +#include "grpc++/grpc++.h" + +namespace cartographer_grpc { +namespace mapping { + +class MapBuilderStub : public cartographer::mapping::MapBuilderInterface { + public: + MapBuilderStub(const std::string& server_address); + + MapBuilderStub(const MapBuilderStub&) = delete; + MapBuilderStub& operator=(const MapBuilderStub&) = delete; + + int AddTrajectoryBuilder( + const std::unordered_set& expected_sensor_ids, + const cartographer::mapping::proto::TrajectoryBuilderOptions& + trajectory_options, + LocalSlamResultCallback local_slam_result_callback) override; + int AddTrajectoryForDeserialization() override; + cartographer::mapping::TrajectoryBuilderInterface* GetTrajectoryBuilder( + int trajectory_id) const override; + void FinishTrajectory(int trajectory_id) override; + std::string SubmapToProto( + const cartographer::mapping::SubmapId& submap_id, + cartographer::mapping::proto::SubmapQuery::Response* response) override; + void SerializeState(cartographer::io::ProtoStreamWriter* writer) override; + void LoadMap(cartographer::io::ProtoStreamReader* reader) override; + int num_trajectory_builders() const override; + cartographer::mapping::PoseGraphInterface* pose_graph() override; + + private: + grpc::ClientContext client_context_; + std::shared_ptr client_channel_; + std::unique_ptr service_stub_; + PoseGraphStub pose_graph_stub_; + std::map> + trajectory_builder_stubs_; +}; + +} // namespace mapping +} // namespace cartographer_grpc + +#endif // CARTOGRAPHER_GRPC_MAPPING_MAP_BUILDER_STUB_H_ diff --git a/cartographer_grpc/mapping/pose_graph_stub.cc b/cartographer_grpc/mapping/pose_graph_stub.cc new file mode 100644 index 0000000..bf18b3d --- /dev/null +++ b/cartographer_grpc/mapping/pose_graph_stub.cc @@ -0,0 +1,58 @@ +/* + * 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/pose_graph_stub.h" + +#include "glog/logging.h" + +namespace cartographer_grpc { +namespace mapping { + +PoseGraphStub::PoseGraphStub(std::shared_ptr client_channel, + proto::MapBuilderService::Stub* stub) + : client_channel_(client_channel), stub_(stub) {} + +void PoseGraphStub::RunFinalOptimization() { LOG(FATAL) << "Not implemented"; } + +cartographer::mapping::MapById< + cartographer::mapping::SubmapId, + cartographer::mapping::PoseGraphInterface::SubmapData> +GetAllSubmapData() { + LOG(FATAL) << "Not implemented"; +} + +cartographer::transform::Rigid3d PoseGraphStub::GetLocalToGlobalTransform( + int trajectory_id) { + LOG(FATAL) << "Not implemented"; +} + +cartographer::mapping::MapById +GetTrajectoryNodes() { + LOG(FATAL) << "Not implemented"; +} + +bool PoseGraphStub::IsTrajectoryFinished(int trajectory_id) { + LOG(FATAL) << "Not implemented"; +} + +std::vector +PoseGraphStub::constraints() { + LOG(FATAL) << "Not implemented"; +} + +} // namespace mapping +} // namespace cartographer_grpc diff --git a/cartographer_grpc/mapping/pose_graph_stub.h b/cartographer_grpc/mapping/pose_graph_stub.h new file mode 100644 index 0000000..a8ab1b2 --- /dev/null +++ b/cartographer_grpc/mapping/pose_graph_stub.h @@ -0,0 +1,54 @@ +/* + * 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. + */ + +#ifndef CARTOGRAPHER_GRPC_MAPPING_POSE_GRAPH_STUB_H_ +#define CARTOGRAPHER_GRPC_MAPPING_POSE_GRAPH_STUB_H_ + +#include "cartographer/mapping/pose_graph_interface.h" +#include "cartographer_grpc/proto/map_builder_service.grpc.pb.h" +#include "grpc++/grpc++.h" + +namespace cartographer_grpc { +namespace mapping { + +class PoseGraphStub : public cartographer::mapping::PoseGraphInterface { + public: + PoseGraphStub(std::shared_ptr client_channel, + proto::MapBuilderService::Stub* stub); + + PoseGraphStub(const PoseGraphStub&) = delete; + PoseGraphStub& operator=(const PoseGraphStub&) = delete; + + void RunFinalOptimization() override; + cartographer::mapping::MapById + GetAllSubmapData() override; + cartographer::transform::Rigid3d GetLocalToGlobalTransform( + int trajectory_id) override; + cartographer::mapping::MapById + GetTrajectoryNodes() override; + bool IsTrajectoryFinished(int trajectory_id) override; + std::vector constraints() override; + + private: + std::shared_ptr client_channel_; + proto::MapBuilderService::Stub* stub_; +}; + +} // namespace mapping +} // namespace cartographer_grpc + +#endif // CARTOGRAPHER_GRPC_MAPPING_POSE_GRAPH_STUB_H_ diff --git a/cartographer_grpc/mapping/trajectory_builder_stub.cc b/cartographer_grpc/mapping/trajectory_builder_stub.cc new file mode 100644 index 0000000..1b55a2d --- /dev/null +++ b/cartographer_grpc/mapping/trajectory_builder_stub.cc @@ -0,0 +1,54 @@ +/* + * 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/trajectory_builder_stub.h" + +#include "glog/logging.h" + +namespace cartographer_grpc { +namespace mapping { + +TrajectoryBuilderStub::TrajectoryBuilderStub( + std::shared_ptr client_channel, + proto::MapBuilderService::Stub* stub) + : client_channel_(client_channel), stub_(stub) {} + +void TrajectoryBuilderStub::AddSensorData( + const std::string& sensor_id, + const cartographer::sensor::TimedPointCloudData& timed_point_cloud_data) { + LOG(FATAL) << "Not implemented"; +} + +void TrajectoryBuilderStub::AddSensorData( + const std::string& sensor_id, + const cartographer::sensor::ImuData& imu_data) { + LOG(FATAL) << "Not implemented"; +} + +void TrajectoryBuilderStub::AddSensorData( + const std::string& sensor_id, + const cartographer::sensor::OdometryData& odometry_data) { + LOG(FATAL) << "Not implemented"; +} + +void TrajectoryBuilderStub::AddSensorData( + const std::string& sensor_id, + const cartographer::sensor::FixedFramePoseData& fixed_frame_pose) { + LOG(FATAL) << "Not implemented"; +} + +} // namespace mapping +} // namespace cartographer_grpc diff --git a/cartographer_grpc/mapping/trajectory_builder_stub.h b/cartographer_grpc/mapping/trajectory_builder_stub.h new file mode 100644 index 0000000..fc50942 --- /dev/null +++ b/cartographer_grpc/mapping/trajectory_builder_stub.h @@ -0,0 +1,56 @@ +/* + * 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. + */ + +#ifndef CARTOGRAPHER_GRPC_MAPPING_TRAJECTORY_BUILDER_STUB_H_ +#define CARTOGRAPHER_GRPC_MAPPING_TRAJECTORY_BUILDER_STUB_H_ + +#include "cartographer/mapping/trajectory_builder_interface.h" +#include "cartographer_grpc/proto/map_builder_service.grpc.pb.h" +#include "grpc++/grpc++.h" + +namespace cartographer_grpc { +namespace mapping { + +class TrajectoryBuilderStub + : public cartographer::mapping::TrajectoryBuilderInterface { + public: + TrajectoryBuilderStub(std::shared_ptr client_channel, + proto::MapBuilderService::Stub* stub); + + TrajectoryBuilderStub(const TrajectoryBuilderStub&) = delete; + TrajectoryBuilderStub& operator=(const TrajectoryBuilderStub&) = delete; + + void AddSensorData(const std::string& sensor_id, + const cartographer::sensor::TimedPointCloudData& + timed_point_cloud_data) override; + void AddSensorData(const std::string& sensor_id, + const cartographer::sensor::ImuData& imu_data) override; + void AddSensorData( + const std::string& sensor_id, + const cartographer::sensor::OdometryData& odometry_data) override; + void AddSensorData(const std::string& sensor_id, + const cartographer::sensor::FixedFramePoseData& + fixed_frame_pose) override; + + private: + std::shared_ptr client_channel_; + proto::MapBuilderService::Stub* stub_; +}; + +} // namespace mapping +} // namespace cartographer_grpc + +#endif // CARTOGRAPHER_GRPC_MAPPING_TRAJECTORY_BUILDER_STUB_H_