diff --git a/cartographer_grpc/handlers/add_fixed_frame_pose_data_handler.cc b/cartographer_grpc/handlers/add_fixed_frame_pose_data_handler.cc new file mode 100644 index 0000000..41c5bcc --- /dev/null +++ b/cartographer_grpc/handlers/add_fixed_frame_pose_data_handler.cc @@ -0,0 +1,64 @@ +/* + * 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_grpc/handlers/add_fixed_frame_pose_data_handler.h" + +#include "cartographer/common/make_unique.h" +#include "cartographer/sensor/dispatchable.h" +#include "cartographer/sensor/fixed_frame_pose_data.h" +#include "cartographer_grpc/framework/rpc_handler.h" +#include "cartographer_grpc/map_builder_context_interface.h" +#include "cartographer_grpc/proto/map_builder_service.pb.h" +#include "cartographer_grpc/sensor/serialization.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer_grpc { +namespace handlers { + +void AddFixedFramePoseDataHandler::OnRequest( + const proto::AddFixedFramePoseDataRequest &request) { + // The 'BlockingQueue' returned by 'sensor_data_queue()' is already + // thread-safe. Therefore it suffices to get an unsynchronized reference to + // the 'MapBuilderContext'. + GetUnsynchronizedContext()->EnqueueSensorData( + request.sensor_metadata().trajectory_id(), + cartographer::sensor::MakeDispatchable( + request.sensor_metadata().sensor_id(), + cartographer::sensor::FromProto(request.fixed_frame_pose_data()))); + + // The 'BlockingQueue' in 'LocalTrajectoryUploader' is thread-safe. + // Therefore it suffices to get an unsynchronized reference to the + // 'MapBuilderContext'. + if (GetUnsynchronizedContext() + ->local_trajectory_uploader()) { + auto data_request = cartographer::common::make_unique< + proto::AddFixedFramePoseDataRequest>(); + sensor::CreateAddFixedFramePoseDataRequest( + request.sensor_metadata().sensor_id(), + request.sensor_metadata().trajectory_id(), + request.fixed_frame_pose_data(), data_request.get()); + GetUnsynchronizedContext() + ->local_trajectory_uploader() + ->EnqueueDataRequest(std::move(data_request)); + } +} + +void AddFixedFramePoseDataHandler::OnReadsDone() { + Send(cartographer::common::make_unique()); +} + +} // namespace handlers +} // namespace cartographer_grpc diff --git a/cartographer_grpc/handlers/add_fixed_frame_pose_data_handler.h b/cartographer_grpc/handlers/add_fixed_frame_pose_data_handler.h index e95a36a..47def8b 100644 --- a/cartographer_grpc/handlers/add_fixed_frame_pose_data_handler.h +++ b/cartographer_grpc/handlers/add_fixed_frame_pose_data_handler.h @@ -17,11 +17,8 @@ #ifndef CARTOGRAPHER_GRPC_HANDLERS_ADD_FIXED_FRAME_POSE_DATA_HANDLER_H #define CARTOGRAPHER_GRPC_HANDLERS_ADD_FIXED_FRAME_POSE_DATA_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 "cartographer_grpc/sensor/serialization.h" #include "google/protobuf/empty.pb.h" namespace cartographer_grpc { @@ -35,36 +32,8 @@ class AddFixedFramePoseDataHandler std::string method_name() const override { return "/cartographer_grpc.proto.MapBuilderService/AddFixedFramePoseData"; } - void OnRequest(const proto::AddFixedFramePoseDataRequest &request) override { - // The 'BlockingQueue' returned by 'sensor_data_queue()' is already - // thread-safe. Therefore it suffices to get an unsynchronized reference to - // the 'MapBuilderContext'. - GetUnsynchronizedContext()->EnqueueSensorData( - request.sensor_metadata().trajectory_id(), - cartographer::sensor::MakeDispatchable( - request.sensor_metadata().sensor_id(), - cartographer::sensor::FromProto(request.fixed_frame_pose_data()))); - - // The 'BlockingQueue' in 'LocalTrajectoryUploader' is thread-safe. - // Therefore it suffices to get an unsynchronized reference to the - // 'MapBuilderContext'. - if (GetUnsynchronizedContext() - ->local_trajectory_uploader()) { - auto data_request = cartographer::common::make_unique< - proto::AddFixedFramePoseDataRequest>(); - sensor::CreateAddFixedFramePoseDataRequest( - request.sensor_metadata().sensor_id(), - request.sensor_metadata().trajectory_id(), - request.fixed_frame_pose_data(), data_request.get()); - GetUnsynchronizedContext() - ->local_trajectory_uploader() - ->EnqueueDataRequest(std::move(data_request)); - } - } - - void OnReadsDone() override { - Send(cartographer::common::make_unique()); - } + void OnRequest(const proto::AddFixedFramePoseDataRequest &request) override; + void OnReadsDone() override; }; } // namespace handlers diff --git a/cartographer_grpc/handlers/add_imu_data_handler.cc b/cartographer_grpc/handlers/add_imu_data_handler.cc new file mode 100644 index 0000000..06231bc --- /dev/null +++ b/cartographer_grpc/handlers/add_imu_data_handler.cc @@ -0,0 +1,62 @@ +/* + * 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_grpc/handlers/add_imu_data_handler.h" + +#include "cartographer/common/make_unique.h" +#include "cartographer/sensor/dispatchable.h" +#include "cartographer/sensor/imu_data.h" +#include "cartographer_grpc/framework/rpc_handler.h" +#include "cartographer_grpc/map_builder_context_interface.h" +#include "cartographer_grpc/proto/map_builder_service.pb.h" +#include "cartographer_grpc/sensor/serialization.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer_grpc { +namespace handlers { + +void AddImuDataHandler::OnRequest(const proto::AddImuDataRequest &request) { + // The 'BlockingQueue' returned by 'sensor_data_queue()' is already + // thread-safe. Therefore it suffices to get an unsynchronized reference to + // the 'MapBuilderContext'. + GetUnsynchronizedContext()->EnqueueSensorData( + request.sensor_metadata().trajectory_id(), + cartographer::sensor::MakeDispatchable( + request.sensor_metadata().sensor_id(), + cartographer::sensor::FromProto(request.imu_data()))); + + // The 'BlockingQueue' in 'LocalTrajectoryUploader' is thread-safe. + // Therefore it suffices to get an unsynchronized reference to the + // 'MapBuilderContext'. + if (GetUnsynchronizedContext() + ->local_trajectory_uploader()) { + auto data_request = + cartographer::common::make_unique(); + sensor::CreateAddImuDataRequest(request.sensor_metadata().sensor_id(), + request.sensor_metadata().trajectory_id(), + request.imu_data(), data_request.get()); + GetUnsynchronizedContext() + ->local_trajectory_uploader() + ->EnqueueDataRequest(std::move(data_request)); + } +} + +void AddImuDataHandler::OnReadsDone() { + Send(cartographer::common::make_unique()); +} + +} // namespace handlers +} // namespace cartographer_grpc diff --git a/cartographer_grpc/handlers/add_imu_data_handler.h b/cartographer_grpc/handlers/add_imu_data_handler.h index 45264ae..4a2f963 100644 --- a/cartographer_grpc/handlers/add_imu_data_handler.h +++ b/cartographer_grpc/handlers/add_imu_data_handler.h @@ -17,11 +17,8 @@ #ifndef CARTOGRAPHER_GRPC_HANDLERS_ADD_IMU_DATA_HANDLER_H #define CARTOGRAPHER_GRPC_HANDLERS_ADD_IMU_DATA_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 "cartographer_grpc/sensor/serialization.h" #include "google/protobuf/empty.pb.h" namespace cartographer_grpc { @@ -34,35 +31,8 @@ class AddImuDataHandler std::string method_name() const override { return "/cartographer_grpc.proto.MapBuilderService/AddImuData"; } - void OnRequest(const proto::AddImuDataRequest &request) override { - // The 'BlockingQueue' returned by 'sensor_data_queue()' is already - // thread-safe. Therefore it suffices to get an unsynchronized reference to - // the 'MapBuilderContext'. - GetUnsynchronizedContext()->EnqueueSensorData( - request.sensor_metadata().trajectory_id(), - cartographer::sensor::MakeDispatchable( - request.sensor_metadata().sensor_id(), - cartographer::sensor::FromProto(request.imu_data()))); - - // The 'BlockingQueue' in 'LocalTrajectoryUploader' is thread-safe. - // Therefore it suffices to get an unsynchronized reference to the - // 'MapBuilderContext'. - if (GetUnsynchronizedContext() - ->local_trajectory_uploader()) { - auto data_request = - cartographer::common::make_unique(); - sensor::CreateAddImuDataRequest(request.sensor_metadata().sensor_id(), - request.sensor_metadata().trajectory_id(), - request.imu_data(), data_request.get()); - GetUnsynchronizedContext() - ->local_trajectory_uploader() - ->EnqueueDataRequest(std::move(data_request)); - } - } - - void OnReadsDone() override { - Send(cartographer::common::make_unique()); - } + void OnRequest(const proto::AddImuDataRequest &request) override; + void OnReadsDone() override; }; } // namespace handlers diff --git a/cartographer_grpc/handlers/add_imu_data_handler_test.cc b/cartographer_grpc/handlers/add_imu_data_handler_test.cc index 8f13804..4794750 100644 --- a/cartographer_grpc/handlers/add_imu_data_handler_test.cc +++ b/cartographer_grpc/handlers/add_imu_data_handler_test.cc @@ -16,6 +16,7 @@ #include "cartographer_grpc/handlers/add_imu_data_handler.h" #include "cartographer/common/make_unique.h" +#include "cartographer/sensor/dispatchable.h" #include "cartographer_grpc/framework/testing/rpc_handler_test_server.h" #include "cartographer_grpc/testing/mock_local_trajectory_uploader.h" #include "cartographer_grpc/testing/mock_map_builder_context.h" diff --git a/cartographer_grpc/handlers/add_landmark_data_handler.cc b/cartographer_grpc/handlers/add_landmark_data_handler.cc new file mode 100644 index 0000000..e24bd0f --- /dev/null +++ b/cartographer_grpc/handlers/add_landmark_data_handler.cc @@ -0,0 +1,64 @@ +/* + * 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_grpc/handlers/add_landmark_data_handler.h" + +#include "cartographer/common/make_unique.h" +#include "cartographer/sensor/dispatchable.h" +#include "cartographer/sensor/landmark_data.h" +#include "cartographer_grpc/framework/rpc_handler.h" +#include "cartographer_grpc/map_builder_context_interface.h" +#include "cartographer_grpc/proto/map_builder_service.pb.h" +#include "cartographer_grpc/sensor/serialization.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer_grpc { +namespace handlers { + +void AddLandmarkDataHandler::OnRequest( + const proto::AddLandmarkDataRequest &request) { + // The 'BlockingQueue' returned by 'sensor_data_queue()' is already + // thread-safe. Therefore it suffices to get an unsynchronized reference to + // the 'MapBuilderContext'. + GetUnsynchronizedContext()->EnqueueSensorData( + request.sensor_metadata().trajectory_id(), + cartographer::sensor::MakeDispatchable( + request.sensor_metadata().sensor_id(), + cartographer::sensor::FromProto(request.landmark_data()))); + + // The 'BlockingQueue' in 'LocalTrajectoryUploader' is thread-safe. + // Therefore it suffices to get an unsynchronized reference to the + // 'MapBuilderContext'. + if (GetUnsynchronizedContext() + ->local_trajectory_uploader()) { + auto data_request = + cartographer::common::make_unique(); + sensor::CreateAddLandmarkDataRequest( + request.sensor_metadata().sensor_id(), + request.sensor_metadata().trajectory_id(), request.landmark_data(), + data_request.get()); + GetUnsynchronizedContext() + ->local_trajectory_uploader() + ->EnqueueDataRequest(std::move(data_request)); + } +} + +void AddLandmarkDataHandler::OnReadsDone() { + Send(cartographer::common::make_unique()); +} + +} // namespace handlers +} // namespace cartographer_grpc diff --git a/cartographer_grpc/handlers/add_landmark_data_handler.h b/cartographer_grpc/handlers/add_landmark_data_handler.h index f736984..1690c1c 100644 --- a/cartographer_grpc/handlers/add_landmark_data_handler.h +++ b/cartographer_grpc/handlers/add_landmark_data_handler.h @@ -17,9 +17,7 @@ #ifndef CARTOGRAPHER_GRPC_HANDLERS_ADD_LANDMARK_DATA_HANDLER_H #define CARTOGRAPHER_GRPC_HANDLERS_ADD_LANDMARK_DATA_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" @@ -34,36 +32,8 @@ class AddLandmarkDataHandler std::string method_name() const override { return "/cartographer_grpc.proto.MapBuilderService/AddLandmarkData"; } - void OnRequest(const proto::AddLandmarkDataRequest &request) override { - // The 'BlockingQueue' returned by 'sensor_data_queue()' is already - // thread-safe. Therefore it suffices to get an unsynchronized reference to - // the 'MapBuilderContext'. - GetUnsynchronizedContext()->EnqueueSensorData( - request.sensor_metadata().trajectory_id(), - cartographer::sensor::MakeDispatchable( - request.sensor_metadata().sensor_id(), - cartographer::sensor::FromProto(request.landmark_data()))); - - // The 'BlockingQueue' in 'LocalTrajectoryUploader' is thread-safe. - // Therefore it suffices to get an unsynchronized reference to the - // 'MapBuilderContext'. - if (GetUnsynchronizedContext() - ->local_trajectory_uploader()) { - auto data_request = - cartographer::common::make_unique(); - sensor::CreateAddLandmarkDataRequest( - request.sensor_metadata().sensor_id(), - request.sensor_metadata().trajectory_id(), request.landmark_data(), - data_request.get()); - GetUnsynchronizedContext() - ->local_trajectory_uploader() - ->EnqueueDataRequest(std::move(data_request)); - } - } - - void OnReadsDone() override { - Send(cartographer::common::make_unique()); - } + void OnRequest(const proto::AddLandmarkDataRequest &request) override; + void OnReadsDone() override; }; } // namespace handlers diff --git a/cartographer_grpc/handlers/add_local_slam_result_data_handler.cc b/cartographer_grpc/handlers/add_local_slam_result_data_handler.cc new file mode 100644 index 0000000..8df6c45 --- /dev/null +++ b/cartographer_grpc/handlers/add_local_slam_result_data_handler.cc @@ -0,0 +1,49 @@ +/* + * 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_grpc/handlers/add_local_slam_result_data_handler.h" + +#include "cartographer/common/make_unique.h" +#include "cartographer/mapping/local_slam_result_data.h" +#include "cartographer/mapping/trajectory_node.h" +#include "cartographer/sensor/dispatchable.h" +#include "cartographer_grpc/framework/rpc_handler.h" +#include "cartographer_grpc/map_builder_context_interface.h" +#include "cartographer_grpc/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer_grpc { +namespace handlers { + +void AddLocalSlamResultDataHandler::OnRequest( + const proto::AddLocalSlamResultDataRequest& request) { + auto local_slam_result_data = + GetContext()->ProcessLocalSlamResultData( + request.sensor_metadata().sensor_id(), + cartographer::common::FromUniversal( + request.local_slam_result_data().timestamp()), + request.local_slam_result_data()); + GetContext()->EnqueueLocalSlamResultData( + request.sensor_metadata().trajectory_id(), + request.sensor_metadata().sensor_id(), std::move(local_slam_result_data)); +} + +void AddLocalSlamResultDataHandler::OnReadsDone() { + Send(cartographer::common::make_unique()); +} + +} // namespace handlers +} // namespace cartographer_grpc diff --git a/cartographer_grpc/handlers/add_local_slam_result_data_handler.h b/cartographer_grpc/handlers/add_local_slam_result_data_handler.h index bc532cf..62e7267 100644 --- a/cartographer_grpc/handlers/add_local_slam_result_data_handler.h +++ b/cartographer_grpc/handlers/add_local_slam_result_data_handler.h @@ -17,10 +17,7 @@ #ifndef CARTOGRAPHER_GRPC_HANDLERS_ADD_LOCAL_SLAM_RESULT_DATA_HANDLER_H #define CARTOGRAPHER_GRPC_HANDLERS_ADD_LOCAL_SLAM_RESULT_DATA_HANDLER_H -#include "cartographer/common/make_unique.h" -#include "cartographer/mapping/trajectory_node.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" @@ -35,22 +32,8 @@ class AddLocalSlamResultDataHandler std::string method_name() const override { return "/cartographer_grpc.proto.MapBuilderService/AddLocalSlamResultData"; } - void OnRequest(const proto::AddLocalSlamResultDataRequest& request) override { - auto local_slam_result_data = - GetContext()->ProcessLocalSlamResultData( - request.sensor_metadata().sensor_id(), - cartographer::common::FromUniversal( - request.local_slam_result_data().timestamp()), - request.local_slam_result_data()); - GetContext()->EnqueueLocalSlamResultData( - request.sensor_metadata().trajectory_id(), - request.sensor_metadata().sensor_id(), - std::move(local_slam_result_data)); - } - - void OnReadsDone() override { - Send(cartographer::common::make_unique()); - } + void OnRequest(const proto::AddLocalSlamResultDataRequest& request) override; + void OnReadsDone() override; }; } // namespace handlers diff --git a/cartographer_grpc/handlers/add_odometry_data_handler.cc b/cartographer_grpc/handlers/add_odometry_data_handler.cc new file mode 100644 index 0000000..edfe094 --- /dev/null +++ b/cartographer_grpc/handlers/add_odometry_data_handler.cc @@ -0,0 +1,64 @@ +/* + * 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_grpc/handlers/add_odometry_data_handler.h" + +#include "cartographer/common/make_unique.h" +#include "cartographer/sensor/dispatchable.h" +#include "cartographer/sensor/odometry_data.h" +#include "cartographer_grpc/framework/rpc_handler.h" +#include "cartographer_grpc/map_builder_context_interface.h" +#include "cartographer_grpc/proto/map_builder_service.pb.h" +#include "cartographer_grpc/sensor/serialization.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer_grpc { +namespace handlers { + +void AddOdometryDataHandler::OnRequest( + const proto::AddOdometryDataRequest &request) { + // The 'BlockingQueue' returned by 'sensor_data_queue()' is already + // thread-safe. Therefore it suffices to get an unsynchronized reference to + // the 'MapBuilderContext'. + GetUnsynchronizedContext()->EnqueueSensorData( + request.sensor_metadata().trajectory_id(), + cartographer::sensor::MakeDispatchable( + request.sensor_metadata().sensor_id(), + cartographer::sensor::FromProto(request.odometry_data()))); + + // The 'BlockingQueue' in 'LocalTrajectoryUploader' is thread-safe. + // Therefore it suffices to get an unsynchronized reference to the + // 'MapBuilderContext'. + if (GetUnsynchronizedContext() + ->local_trajectory_uploader()) { + auto data_request = + cartographer::common::make_unique(); + sensor::CreateAddOdometryDataRequest( + request.sensor_metadata().sensor_id(), + request.sensor_metadata().trajectory_id(), request.odometry_data(), + data_request.get()); + GetUnsynchronizedContext() + ->local_trajectory_uploader() + ->EnqueueDataRequest(std::move(data_request)); + } +} + +void AddOdometryDataHandler::OnReadsDone() { + Send(cartographer::common::make_unique()); +} + +} // namespace handlers +} // namespace cartographer_grpc diff --git a/cartographer_grpc/handlers/add_odometry_data_handler.h b/cartographer_grpc/handlers/add_odometry_data_handler.h index ad32577..ffe1192 100644 --- a/cartographer_grpc/handlers/add_odometry_data_handler.h +++ b/cartographer_grpc/handlers/add_odometry_data_handler.h @@ -17,9 +17,7 @@ #ifndef CARTOGRAPHER_GRPC_HANDLERS_ADD_ODOMETRY_DATA_HANDLER_H #define CARTOGRAPHER_GRPC_HANDLERS_ADD_ODOMETRY_DATA_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" @@ -34,36 +32,8 @@ class AddOdometryDataHandler std::string method_name() const override { return "/cartographer_grpc.proto.MapBuilderService/AddOdometryData"; } - void OnRequest(const proto::AddOdometryDataRequest &request) override { - // The 'BlockingQueue' returned by 'sensor_data_queue()' is already - // thread-safe. Therefore it suffices to get an unsynchronized reference to - // the 'MapBuilderContext'. - GetUnsynchronizedContext()->EnqueueSensorData( - request.sensor_metadata().trajectory_id(), - cartographer::sensor::MakeDispatchable( - request.sensor_metadata().sensor_id(), - cartographer::sensor::FromProto(request.odometry_data()))); - - // The 'BlockingQueue' in 'LocalTrajectoryUploader' is thread-safe. - // Therefore it suffices to get an unsynchronized reference to the - // 'MapBuilderContext'. - if (GetUnsynchronizedContext() - ->local_trajectory_uploader()) { - auto data_request = - cartographer::common::make_unique(); - sensor::CreateAddOdometryDataRequest( - request.sensor_metadata().sensor_id(), - request.sensor_metadata().trajectory_id(), request.odometry_data(), - data_request.get()); - GetUnsynchronizedContext() - ->local_trajectory_uploader() - ->EnqueueDataRequest(std::move(data_request)); - } - } - - void OnReadsDone() override { - Send(cartographer::common::make_unique()); - } + void OnRequest(const proto::AddOdometryDataRequest &request) override; + void OnReadsDone() override; }; } // namespace handlers diff --git a/cartographer_grpc/handlers/add_rangefinder_data_handler.cc b/cartographer_grpc/handlers/add_rangefinder_data_handler.cc new file mode 100644 index 0000000..471bc88 --- /dev/null +++ b/cartographer_grpc/handlers/add_rangefinder_data_handler.cc @@ -0,0 +1,47 @@ +/* + * 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_grpc/handlers/add_rangefinder_data_handler.h" + +#include "cartographer/common/make_unique.h" +#include "cartographer/sensor/dispatchable.h" +#include "cartographer/sensor/timed_point_cloud_data.h" +#include "cartographer_grpc/framework/rpc_handler.h" +#include "cartographer_grpc/map_builder_context_interface.h" +#include "cartographer_grpc/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer_grpc { +namespace handlers { + +void AddRangefinderDataHandler::OnRequest( + const proto::AddRangefinderDataRequest &request) { + // The 'BlockingQueue' returned by 'sensor_data_queue()' is already + // thread-safe. Therefore it suffices to get an unsynchronized reference to + // the 'MapBuilderContext'. + GetUnsynchronizedContext()->EnqueueSensorData( + request.sensor_metadata().trajectory_id(), + cartographer::sensor::MakeDispatchable( + request.sensor_metadata().sensor_id(), + cartographer::sensor::FromProto(request.timed_point_cloud_data()))); +} + +void AddRangefinderDataHandler::OnReadsDone() { + Send(cartographer::common::make_unique()); +} + +} // namespace handlers +} // namespace cartographer_grpc diff --git a/cartographer_grpc/handlers/add_rangefinder_data_handler.h b/cartographer_grpc/handlers/add_rangefinder_data_handler.h index bba566e..2664481 100644 --- a/cartographer_grpc/handlers/add_rangefinder_data_handler.h +++ b/cartographer_grpc/handlers/add_rangefinder_data_handler.h @@ -17,9 +17,7 @@ #ifndef CARTOGRAPHER_GRPC_HANDLERS_ADD_RANGEFINDER_DATA_HANDLER_H #define CARTOGRAPHER_GRPC_HANDLERS_ADD_RANGEFINDER_DATA_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" @@ -34,20 +32,8 @@ class AddRangefinderDataHandler std::string method_name() const override { return "/cartographer_grpc.proto.MapBuilderService/AddRangefinderData"; } - void OnRequest(const proto::AddRangefinderDataRequest &request) override { - // The 'BlockingQueue' returned by 'sensor_data_queue()' is already - // thread-safe. Therefore it suffices to get an unsynchronized reference to - // the 'MapBuilderContext'. - GetUnsynchronizedContext()->EnqueueSensorData( - request.sensor_metadata().trajectory_id(), - cartographer::sensor::MakeDispatchable( - request.sensor_metadata().sensor_id(), - cartographer::sensor::FromProto(request.timed_point_cloud_data()))); - } - - void OnReadsDone() override { - Send(cartographer::common::make_unique()); - } + void OnRequest(const proto::AddRangefinderDataRequest &request) override; + void OnReadsDone() override; }; } // namespace handlers diff --git a/cartographer_grpc/handlers/add_trajectory_handler.cc b/cartographer_grpc/handlers/add_trajectory_handler.cc new file mode 100644 index 0000000..927af24 --- /dev/null +++ b/cartographer_grpc/handlers/add_trajectory_handler.cc @@ -0,0 +1,67 @@ +/* + * 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_grpc/handlers/add_trajectory_handler.h" + +#include "cartographer/common/make_unique.h" +#include "cartographer_grpc/framework/rpc_handler.h" +#include "cartographer_grpc/map_builder_context_interface.h" +#include "cartographer_grpc/proto/map_builder_service.pb.h" +#include "cartographer_grpc/sensor/serialization.h" + +namespace cartographer_grpc { +namespace handlers { + +void AddTrajectoryHandler::OnRequest( + const proto::AddTrajectoryRequest& request) { + auto local_slam_result_callback = + GetUnsynchronizedContext() + ->GetLocalSlamResultCallbackForSubscriptions(); + std::set + expected_sensor_ids; + for (const auto& sensor_id : request.expected_sensor_ids()) { + expected_sensor_ids.insert(sensor::FromProto(sensor_id)); + } + const int trajectory_id = + GetContext() + ->map_builder() + .AddTrajectoryBuilder(expected_sensor_ids, + request.trajectory_builder_options(), + local_slam_result_callback); + if (GetUnsynchronizedContext() + ->local_trajectory_uploader()) { + auto trajectory_builder_options = request.trajectory_builder_options(); + + // Clear the trajectory builder options to convey to the cloud + // Cartographer instance that does not need to instantiate a + // 'LocalTrajectoryBuilder'. + trajectory_builder_options.clear_trajectory_builder_2d_options(); + trajectory_builder_options.clear_trajectory_builder_3d_options(); + + GetContext() + ->local_trajectory_uploader() + ->AddTrajectory(trajectory_id, expected_sensor_ids, + trajectory_builder_options); + } + + auto response = + cartographer::common::make_unique(); + response->set_trajectory_id(trajectory_id); + Send(std::move(response)); +} + +} // namespace handlers +} // namespace cartographer_grpc diff --git a/cartographer_grpc/handlers/add_trajectory_handler.h b/cartographer_grpc/handlers/add_trajectory_handler.h index 6d21837..49055e1 100644 --- a/cartographer_grpc/handlers/add_trajectory_handler.h +++ b/cartographer_grpc/handlers/add_trajectory_handler.h @@ -17,11 +17,8 @@ #ifndef CARTOGRAPHER_GRPC_HANDLERS_ADD_TRAJECTORY_HANDLER_H #define CARTOGRAPHER_GRPC_HANDLERS_ADD_TRAJECTORY_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 "cartographer_grpc/sensor/serialization.h" namespace cartographer_grpc { namespace handlers { @@ -33,40 +30,7 @@ class AddTrajectoryHandler std::string method_name() const override { return "/cartographer_grpc.proto.MapBuilderService/AddTrajectory"; } - void OnRequest(const proto::AddTrajectoryRequest& request) override { - auto local_slam_result_callback = - GetUnsynchronizedContext() - ->GetLocalSlamResultCallbackForSubscriptions(); - std::set - expected_sensor_ids; - for (const auto& sensor_id : request.expected_sensor_ids()) { - expected_sensor_ids.insert(sensor::FromProto(sensor_id)); - } - const int trajectory_id = - GetContext()->map_builder().AddTrajectoryBuilder( - expected_sensor_ids, request.trajectory_builder_options(), - local_slam_result_callback); - if (GetUnsynchronizedContext() - ->local_trajectory_uploader()) { - auto trajectory_builder_options = request.trajectory_builder_options(); - - // Clear the trajectory builder options to convey to the cloud - // Cartographer instance that does not need to instantiate a - // 'LocalTrajectoryBuilder'. - trajectory_builder_options.clear_trajectory_builder_2d_options(); - trajectory_builder_options.clear_trajectory_builder_3d_options(); - - GetContext() - ->local_trajectory_uploader() - ->AddTrajectory(trajectory_id, expected_sensor_ids, - trajectory_builder_options); - } - - auto response = - cartographer::common::make_unique(); - response->set_trajectory_id(trajectory_id); - Send(std::move(response)); - } + void OnRequest(const proto::AddTrajectoryRequest& request) override; }; } // namespace handlers diff --git a/cartographer_grpc/handlers/finish_trajectory_handler.cc b/cartographer_grpc/handlers/finish_trajectory_handler.cc new file mode 100644 index 0000000..71aacb2 --- /dev/null +++ b/cartographer_grpc/handlers/finish_trajectory_handler.cc @@ -0,0 +1,44 @@ +/* + * 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_grpc/handlers/finish_trajectory_handler.h" + +#include "cartographer/common/make_unique.h" +#include "cartographer_grpc/framework/rpc_handler.h" +#include "cartographer_grpc/map_builder_context_interface.h" +#include "cartographer_grpc/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer_grpc { +namespace handlers { + +void FinishTrajectoryHandler::OnRequest( + const proto::FinishTrajectoryRequest& request) { + GetContext()->map_builder().FinishTrajectory( + request.trajectory_id()); + GetUnsynchronizedContext() + ->NotifyFinishTrajectory(request.trajectory_id()); + if (GetUnsynchronizedContext() + ->local_trajectory_uploader()) { + GetContext() + ->local_trajectory_uploader() + ->FinishTrajectory(request.trajectory_id()); + } + Send(cartographer::common::make_unique()); +} + +} // namespace handlers +} // namespace cartographer_grpc diff --git a/cartographer_grpc/handlers/finish_trajectory_handler.h b/cartographer_grpc/handlers/finish_trajectory_handler.h index d72a60d..75c0579 100644 --- a/cartographer_grpc/handlers/finish_trajectory_handler.h +++ b/cartographer_grpc/handlers/finish_trajectory_handler.h @@ -17,9 +17,7 @@ #ifndef CARTOGRAPHER_GRPC_HANDLERS_FINISH_TRAJECTORY_HANDLER_H #define CARTOGRAPHER_GRPC_HANDLERS_FINISH_TRAJECTORY_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" @@ -33,19 +31,7 @@ class FinishTrajectoryHandler std::string method_name() const override { return "/cartographer_grpc.proto.MapBuilderService/FinishTrajectory"; } - void OnRequest(const proto::FinishTrajectoryRequest& request) override { - GetContext()->map_builder().FinishTrajectory( - request.trajectory_id()); - GetUnsynchronizedContext()->NotifyFinishTrajectory( - request.trajectory_id()); - if (GetUnsynchronizedContext() - ->local_trajectory_uploader()) { - GetContext() - ->local_trajectory_uploader() - ->FinishTrajectory(request.trajectory_id()); - } - Send(cartographer::common::make_unique()); - } + void OnRequest(const proto::FinishTrajectoryRequest& request) override; }; } // namespace handlers diff --git a/cartographer_grpc/handlers/get_all_submap_poses.cc b/cartographer_grpc/handlers/get_all_submap_poses.cc new file mode 100644 index 0000000..b2b9f22 --- /dev/null +++ b/cartographer_grpc/handlers/get_all_submap_poses.cc @@ -0,0 +1,47 @@ +/* + * 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_grpc/handlers/get_all_submap_poses.h" + +#include "cartographer/common/make_unique.h" +#include "cartographer_grpc/framework/rpc_handler.h" +#include "cartographer_grpc/map_builder_context_interface.h" +#include "cartographer_grpc/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer_grpc { +namespace handlers { + +void GetAllSubmapPosesHandler::OnRequest( + const google::protobuf::Empty& request) { + auto submap_poses = GetContext() + ->map_builder() + .pose_graph() + ->GetAllSubmapPoses(); + auto response = + cartographer::common::make_unique(); + for (const auto& submap_id_pose : submap_poses) { + auto* submap_pose = response->add_submap_poses(); + submap_id_pose.id.ToProto(submap_pose->mutable_submap_id()); + submap_pose->set_submap_version(submap_id_pose.data.version); + *submap_pose->mutable_global_pose() = + cartographer::transform::ToProto(submap_id_pose.data.pose); + } + Send(std::move(response)); +} + +} // namespace handlers +} // namespace cartographer_grpc diff --git a/cartographer_grpc/handlers/get_all_submap_poses.h b/cartographer_grpc/handlers/get_all_submap_poses.h index 448331d..8c65d35 100644 --- a/cartographer_grpc/handlers/get_all_submap_poses.h +++ b/cartographer_grpc/handlers/get_all_submap_poses.h @@ -17,9 +17,7 @@ #ifndef CARTOGRAPHER_GRPC_HANDLERS_GET_ALL_SUBMAP_POSES_H #define CARTOGRAPHER_GRPC_HANDLERS_GET_ALL_SUBMAP_POSES_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" @@ -33,22 +31,7 @@ class GetAllSubmapPosesHandler std::string method_name() const override { return "/cartographer_grpc.proto.MapBuilderService/GetAllSubmapPoses"; } - void OnRequest(const google::protobuf::Empty& request) override { - auto submap_poses = GetContext() - ->map_builder() - .pose_graph() - ->GetAllSubmapPoses(); - auto response = - cartographer::common::make_unique(); - for (const auto& submap_id_pose : submap_poses) { - auto* submap_pose = response->add_submap_poses(); - submap_id_pose.id.ToProto(submap_pose->mutable_submap_id()); - submap_pose->set_submap_version(submap_id_pose.data.version); - *submap_pose->mutable_global_pose() = - cartographer::transform::ToProto(submap_id_pose.data.pose); - } - Send(std::move(response)); - } + void OnRequest(const google::protobuf::Empty& request) override; }; } // namespace handlers diff --git a/cartographer_grpc/handlers/get_constraints_handler.cc b/cartographer_grpc/handlers/get_constraints_handler.cc new file mode 100644 index 0000000..89624d3 --- /dev/null +++ b/cartographer_grpc/handlers/get_constraints_handler.cc @@ -0,0 +1,44 @@ +/* + * 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_grpc/handlers/get_constraints_handler.h" + +#include "cartographer/common/make_unique.h" +#include "cartographer/mapping/pose_graph.h" +#include "cartographer_grpc/framework/rpc_handler.h" +#include "cartographer_grpc/map_builder_context_interface.h" +#include "cartographer_grpc/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer_grpc { +namespace handlers { + +void GetConstraintsHandler::OnRequest(const google::protobuf::Empty& request) { + auto constraints = GetContext() + ->map_builder() + .pose_graph() + ->constraints(); + auto response = + cartographer::common::make_unique(); + response->mutable_constraints()->Reserve(constraints.size()); + for (const auto& constraint : constraints) { + *response->add_constraints() = cartographer::mapping::ToProto(constraint); + } + Send(std::move(response)); +} + +} // namespace handlers +} // namespace cartographer_grpc diff --git a/cartographer_grpc/handlers/get_constraints_handler.h b/cartographer_grpc/handlers/get_constraints_handler.h index 353c76a..a972175 100644 --- a/cartographer_grpc/handlers/get_constraints_handler.h +++ b/cartographer_grpc/handlers/get_constraints_handler.h @@ -17,10 +17,7 @@ #ifndef CARTOGRAPHER_GRPC_HANDLERS_GET_CONSTRAINTS_HANDLER_H #define CARTOGRAPHER_GRPC_HANDLERS_GET_CONSTRAINTS_HANDLER_H -#include "cartographer/common/make_unique.h" -#include "cartographer/mapping/pose_graph.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" @@ -34,19 +31,7 @@ class GetConstraintsHandler std::string method_name() const override { return "/cartographer_grpc.proto.MapBuilderService/GetConstraints"; } - void OnRequest(const google::protobuf::Empty& request) override { - auto constraints = GetContext() - ->map_builder() - .pose_graph() - ->constraints(); - auto response = - cartographer::common::make_unique(); - response->mutable_constraints()->Reserve(constraints.size()); - for (const auto& constraint : constraints) { - *response->add_constraints() = cartographer::mapping::ToProto(constraint); - } - Send(std::move(response)); - } + void OnRequest(const google::protobuf::Empty& request) override; }; } // namespace handlers diff --git a/cartographer_grpc/handlers/get_local_to_global_transform_handler.cc b/cartographer_grpc/handlers/get_local_to_global_transform_handler.cc new file mode 100644 index 0000000..dc170f3 --- /dev/null +++ b/cartographer_grpc/handlers/get_local_to_global_transform_handler.cc @@ -0,0 +1,42 @@ +/* + * 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_grpc/handlers/get_local_to_global_transform_handler.h" + +#include "cartographer_grpc/framework/rpc_handler.h" +#include "cartographer_grpc/map_builder_context_interface.h" +#include "cartographer_grpc/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer_grpc { +namespace handlers { + +void GetLocalToGlobalTransformHandler::OnRequest( + const proto::GetLocalToGlobalTransformRequest& request) { + auto response = cartographer::common::make_unique< + proto::GetLocalToGlobalTransformResponse>(); + auto local_to_global = + GetContext() + ->map_builder() + .pose_graph() + ->GetLocalToGlobalTransform(request.trajectory_id()); + *response->mutable_local_to_global() = + cartographer::transform::ToProto(local_to_global); + Send(std::move(response)); +} + +} // namespace handlers +} // namespace cartographer_grpc diff --git a/cartographer_grpc/handlers/get_local_to_global_transform_handler.h b/cartographer_grpc/handlers/get_local_to_global_transform_handler.h index 613660c..5a86ae2 100644 --- a/cartographer_grpc/handlers/get_local_to_global_transform_handler.h +++ b/cartographer_grpc/handlers/get_local_to_global_transform_handler.h @@ -17,6 +17,10 @@ #ifndef CARTOGRAPHER_GRPC_HANDLERS_GET_LOCAL_TO_GLOBAL_TRANSFORM_HANDLER_H #define CARTOGRAPHER_GRPC_HANDLERS_GET_LOCAL_TO_GLOBAL_TRANSFORM_HANDLER_H +#include "cartographer_grpc/framework/rpc_handler.h" +#include "cartographer_grpc/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + namespace cartographer_grpc { namespace handlers { @@ -29,18 +33,7 @@ class GetLocalToGlobalTransformHandler "GetLocalToGlobalTransform"; } void OnRequest( - const proto::GetLocalToGlobalTransformRequest& request) override { - auto response = cartographer::common::make_unique< - proto::GetLocalToGlobalTransformResponse>(); - auto local_to_global = - GetContext() - ->map_builder() - .pose_graph() - ->GetLocalToGlobalTransform(request.trajectory_id()); - *response->mutable_local_to_global() = - cartographer::transform::ToProto(local_to_global); - Send(std::move(response)); - } + const proto::GetLocalToGlobalTransformRequest& request) override; }; } // namespace handlers diff --git a/cartographer_grpc/handlers/get_submap_handler.cc b/cartographer_grpc/handlers/get_submap_handler.cc new file mode 100644 index 0000000..1f97a95 --- /dev/null +++ b/cartographer_grpc/handlers/get_submap_handler.cc @@ -0,0 +1,39 @@ +/* + * 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_grpc/handlers/get_submap_handler.h" + +#include "cartographer/common/make_unique.h" +#include "cartographer_grpc/framework/rpc_handler.h" +#include "cartographer_grpc/map_builder_context_interface.h" +#include "cartographer_grpc/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer_grpc { +namespace handlers { + +void GetSubmapHandler::OnRequest(const proto::GetSubmapRequest &request) { + auto response = cartographer::common::make_unique(); + response->set_error_msg( + GetContext()->map_builder().SubmapToProto( + cartographer::mapping::SubmapId{request.submap_id().trajectory_id(), + request.submap_id().submap_index()}, + response->mutable_submap_query_response())); + Send(std::move(response)); +} + +} // namespace handlers +} // namespace cartographer_grpc diff --git a/cartographer_grpc/handlers/get_submap_handler.h b/cartographer_grpc/handlers/get_submap_handler.h index 9587a9a..bdc056f 100644 --- a/cartographer_grpc/handlers/get_submap_handler.h +++ b/cartographer_grpc/handlers/get_submap_handler.h @@ -17,9 +17,7 @@ #ifndef CARTOGRAPHER_GRPC_HANDLERS_GET_SUBMAP_HANDLER_H #define CARTOGRAPHER_GRPC_HANDLERS_GET_SUBMAP_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" @@ -33,16 +31,7 @@ class GetSubmapHandler std::string method_name() const override { return "/cartographer_grpc.proto.MapBuilderService/GetSubmap"; } - void OnRequest(const proto::GetSubmapRequest &request) override { - auto response = - cartographer::common::make_unique(); - response->set_error_msg( - GetContext()->map_builder().SubmapToProto( - cartographer::mapping::SubmapId{request.submap_id().trajectory_id(), - request.submap_id().submap_index()}, - response->mutable_submap_query_response())); - Send(std::move(response)); - } + void OnRequest(const proto::GetSubmapRequest &request) override; }; } // namespace handlers diff --git a/cartographer_grpc/handlers/get_trajectory_node_poses_handler.cc b/cartographer_grpc/handlers/get_trajectory_node_poses_handler.cc new file mode 100644 index 0000000..9206226 --- /dev/null +++ b/cartographer_grpc/handlers/get_trajectory_node_poses_handler.cc @@ -0,0 +1,47 @@ +/* + * 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_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_context_interface.h" +#include "cartographer_grpc/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer_grpc { +namespace handlers { + +void GetTrajectoryNodePosesHandler::OnRequest( + const google::protobuf::Empty& request) { + auto node_poses = GetContext() + ->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 diff --git a/cartographer_grpc/handlers/get_trajectory_node_poses_handler.h b/cartographer_grpc/handlers/get_trajectory_node_poses_handler.h index f7014a7..a3cfa74 100644 --- a/cartographer_grpc/handlers/get_trajectory_node_poses_handler.h +++ b/cartographer_grpc/handlers/get_trajectory_node_poses_handler.h @@ -17,9 +17,7 @@ #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" @@ -33,22 +31,7 @@ class GetTrajectoryNodePosesHandler std::string method_name() const override { return "/cartographer_grpc.proto.MapBuilderService/GetTrajectoryNodePoses"; } - void OnRequest(const google::protobuf::Empty& request) override { - auto node_poses = GetContext() - ->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)); - } + void OnRequest(const google::protobuf::Empty& request) override; }; } // namespace handlers diff --git a/cartographer_grpc/handlers/load_map_handler.cc b/cartographer_grpc/handlers/load_map_handler.cc new file mode 100644 index 0000000..462fcdb --- /dev/null +++ b/cartographer_grpc/handlers/load_map_handler.cc @@ -0,0 +1,48 @@ +/* + * 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_grpc/handlers/load_map_handler.h" + +#include "cartographer/common/make_unique.h" +#include "cartographer/io/in_memory_proto_stream.h" +#include "cartographer_grpc/framework/rpc_handler.h" +#include "cartographer_grpc/map_builder_context_interface.h" +#include "cartographer_grpc/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer_grpc { +namespace handlers { + +void LoadMapHandler::OnRequest(const proto::LoadMapRequest& request) { + switch (request.map_chunk_case()) { + case proto::LoadMapRequest::kPoseGraph: + reader_.AddProto(request.pose_graph()); + break; + case proto::LoadMapRequest::kSerializedData: + reader_.AddProto(request.serialized_data()); + break; + default: + LOG(FATAL) << "Unhandled proto::LoadMapRequest case."; + } +} + +void LoadMapHandler::OnReadsDone() { + GetContext()->map_builder().LoadMap(&reader_); + Send(cartographer::common::make_unique()); +} + +} // namespace handlers +} // namespace cartographer_grpc diff --git a/cartographer_grpc/handlers/load_map_handler.h b/cartographer_grpc/handlers/load_map_handler.h index f80f7e8..46cb376 100644 --- a/cartographer_grpc/handlers/load_map_handler.h +++ b/cartographer_grpc/handlers/load_map_handler.h @@ -17,10 +17,8 @@ #ifndef CARTOGRAPHER_GRPC_HANDLERS_LOAD_MAP_HANDLER_H #define CARTOGRAPHER_GRPC_HANDLERS_LOAD_MAP_HANDLER_H -#include "cartographer/common/make_unique.h" #include "cartographer/io/in_memory_proto_stream.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" @@ -34,23 +32,8 @@ class LoadMapHandler std::string method_name() const override { return "/cartographer_grpc.proto.MapBuilderService/LoadMap"; } - void OnRequest(const proto::LoadMapRequest& request) override { - switch (request.map_chunk_case()) { - case proto::LoadMapRequest::kPoseGraph: - reader_.AddProto(request.pose_graph()); - break; - case proto::LoadMapRequest::kSerializedData: - reader_.AddProto(request.serialized_data()); - break; - default: - LOG(FATAL) << "Unhandled proto::LoadMapRequest case."; - } - } - - void OnReadsDone() override { - GetContext()->map_builder().LoadMap(&reader_); - Send(cartographer::common::make_unique()); - } + void OnRequest(const proto::LoadMapRequest& request) override; + void OnReadsDone() override; private: cartographer::io::InMemoryProtoStreamReader reader_; diff --git a/cartographer_grpc/handlers/receive_local_slam_results_handler.cc b/cartographer_grpc/handlers/receive_local_slam_results_handler.cc new file mode 100644 index 0000000..cd82aac --- /dev/null +++ b/cartographer_grpc/handlers/receive_local_slam_results_handler.cc @@ -0,0 +1,82 @@ +/* + * 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_grpc/handlers/receive_local_slam_results_handler.h" + +#include "cartographer/common/make_unique.h" +#include "cartographer_grpc/framework/rpc_handler.h" +#include "cartographer_grpc/map_builder_context_interface.h" +#include "cartographer_grpc/proto/map_builder_service.pb.h" + +namespace cartographer_grpc { +namespace handlers { +namespace { + +std::unique_ptr GenerateResponse( + std::unique_ptr + local_slam_result) { + auto response = cartographer::common::make_unique< + proto::ReceiveLocalSlamResultsResponse>(); + response->set_trajectory_id(local_slam_result->trajectory_id); + response->set_timestamp( + cartographer::common::ToUniversal(local_slam_result->time)); + *response->mutable_local_pose() = + cartographer::transform::ToProto(local_slam_result->local_pose); + if (local_slam_result->range_data) { + *response->mutable_range_data() = + cartographer::sensor::ToProto(*local_slam_result->range_data); + } + if (local_slam_result->insertion_result) { + local_slam_result->insertion_result->node_id.ToProto( + response->mutable_insertion_result()->mutable_node_id()); + } + return response; +} + +} // namespace + +void ReceiveLocalSlamResultsHandler::OnRequest( + const proto::ReceiveLocalSlamResultsRequest& request) { + auto writer = GetWriter(); + MapBuilderContextInterface::SubscriptionId subscription_id = + GetUnsynchronizedContext() + ->SubscribeLocalSlamResults( + request.trajectory_id(), + [writer]( + std::unique_ptr + local_slam_result) { + if (local_slam_result) { + writer.Write(GenerateResponse(std::move(local_slam_result))); + } else { + // Callback with 'nullptr' signals that the trajectory + // finished. + writer.WritesDone(); + } + }); + + subscription_id_ = cartographer::common::make_unique< + MapBuilderContextInterface::SubscriptionId>(subscription_id); +} + +void ReceiveLocalSlamResultsHandler::OnFinish() { + if (subscription_id_) { + GetUnsynchronizedContext() + ->UnsubscribeLocalSlamResults(*subscription_id_); + } +} + +} // namespace handlers +} // namespace cartographer_grpc diff --git a/cartographer_grpc/handlers/receive_local_slam_results_handler.h b/cartographer_grpc/handlers/receive_local_slam_results_handler.h index cc342c8..de5a678 100644 --- a/cartographer_grpc/handlers/receive_local_slam_results_handler.h +++ b/cartographer_grpc/handlers/receive_local_slam_results_handler.h @@ -17,9 +17,10 @@ #ifndef CARTOGRAPHER_GRPC_HANDLERS_RECEIVE_LOCAL_SLAM_RESULTS_HANDLER_H #define CARTOGRAPHER_GRPC_HANDLERS_RECEIVE_LOCAL_SLAM_RESULTS_HANDLER_H -#include "cartographer/common/make_unique.h" +#include + #include "cartographer_grpc/framework/rpc_handler.h" -#include "cartographer_grpc/map_builder_server.h" +#include "cartographer_grpc/map_builder_context_interface.h" #include "cartographer_grpc/proto/map_builder_service.pb.h" namespace cartographer_grpc { @@ -33,57 +34,8 @@ class ReceiveLocalSlamResultsHandler std::string method_name() const override { return "/cartographer_grpc.proto.MapBuilderService/ReceiveLocalSlamResults"; } - void OnRequest( - const proto::ReceiveLocalSlamResultsRequest& request) override { - auto writer = GetWriter(); - MapBuilderContextInterface::SubscriptionId subscription_id = - GetUnsynchronizedContext() - ->SubscribeLocalSlamResults( - request.trajectory_id(), - [writer]( - std::unique_ptr - local_slam_result) { - if (local_slam_result) { - writer.Write( - GenerateResponse(std::move(local_slam_result))); - } else { - // Callback with 'nullptr' signals that the trajectory - // finished. - writer.WritesDone(); - } - }); - - subscription_id_ = cartographer::common::make_unique< - MapBuilderContextInterface::SubscriptionId>(subscription_id); - } - - static std::unique_ptr - GenerateResponse(std::unique_ptr - local_slam_result) { - auto response = cartographer::common::make_unique< - proto::ReceiveLocalSlamResultsResponse>(); - response->set_trajectory_id(local_slam_result->trajectory_id); - response->set_timestamp( - cartographer::common::ToUniversal(local_slam_result->time)); - *response->mutable_local_pose() = - cartographer::transform::ToProto(local_slam_result->local_pose); - if (local_slam_result->range_data) { - *response->mutable_range_data() = - cartographer::sensor::ToProto(*local_slam_result->range_data); - } - if (local_slam_result->insertion_result) { - local_slam_result->insertion_result->node_id.ToProto( - response->mutable_insertion_result()->mutable_node_id()); - } - return response; - } - - void OnFinish() override { - if (subscription_id_) { - GetUnsynchronizedContext() - ->UnsubscribeLocalSlamResults(*subscription_id_); - } - } + void OnRequest(const proto::ReceiveLocalSlamResultsRequest& request) override; + void OnFinish() override; private: std::unique_ptr subscription_id_; diff --git a/cartographer_grpc/handlers/run_final_optimization_handler.cc b/cartographer_grpc/handlers/run_final_optimization_handler.cc new file mode 100644 index 0000000..ebbc3a3 --- /dev/null +++ b/cartographer_grpc/handlers/run_final_optimization_handler.cc @@ -0,0 +1,40 @@ +/* + * 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_grpc/handlers/run_final_optimization_handler.h" + +#include "cartographer/common/make_unique.h" +#include "cartographer/mapping/map_builder_interface.h" +#include "cartographer/mapping/pose_graph.h" +#include "cartographer_grpc/framework/rpc_handler.h" +#include "cartographer_grpc/map_builder_context_interface.h" +#include "cartographer_grpc/proto/map_builder_service.pb.h" +#include "google/protobuf/empty.pb.h" + +namespace cartographer_grpc { +namespace handlers { + +void RunFinalOptimizationHandler::OnRequest( + const google::protobuf::Empty& request) { + GetContext() + ->map_builder() + .pose_graph() + ->RunFinalOptimization(); + Send(cartographer::common::make_unique()); +} + +} // namespace handlers +} // namespace cartographer_grpc diff --git a/cartographer_grpc/handlers/run_final_optimization_handler.h b/cartographer_grpc/handlers/run_final_optimization_handler.h index fbb72e3..34af101 100644 --- a/cartographer_grpc/handlers/run_final_optimization_handler.h +++ b/cartographer_grpc/handlers/run_final_optimization_handler.h @@ -17,10 +17,7 @@ #ifndef CARTOGRAPHER_GRPC_HANDLERS_RUN_FINAL_OPTIMIZATION_H #define CARTOGRAPHER_GRPC_HANDLERS_RUN_FINAL_OPTIMIZATION_H -#include "cartographer/common/make_unique.h" -#include "cartographer/mapping/pose_graph.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" @@ -34,13 +31,7 @@ class RunFinalOptimizationHandler std::string method_name() const override { return "/cartographer_grpc.proto.MapBuilderService/RunFinalOptimization"; } - void OnRequest(const google::protobuf::Empty& request) override { - GetContext() - ->map_builder() - .pose_graph() - ->RunFinalOptimization(); - Send(cartographer::common::make_unique()); - } + void OnRequest(const google::protobuf::Empty& request) override; }; } // namespace handlers diff --git a/cartographer_grpc/handlers/write_map_handler.cc b/cartographer_grpc/handlers/write_map_handler.cc new file mode 100644 index 0000000..d9a4d89 --- /dev/null +++ b/cartographer_grpc/handlers/write_map_handler.cc @@ -0,0 +1,56 @@ +/* + * 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_grpc/handlers/write_map_handler.h" + +#include "cartographer/common/make_unique.h" +#include "cartographer/io/in_memory_proto_stream.h" +#include "cartographer_grpc/framework/rpc_handler.h" +#include "cartographer_grpc/map_builder_context_interface.h" +#include "cartographer_grpc/map_builder_server.h" +#include "cartographer_grpc/proto/map_builder_service.pb.h" + +namespace cartographer_grpc { +namespace handlers { + +void WriteMapHandler::OnRequest(const google::protobuf::Empty& request) { + auto writer = GetWriter(); + cartographer::io::ForwardingProtoStreamWriter proto_stream_writer( + [writer](const google::protobuf::Message* proto) { + if (!proto) { + writer.WritesDone(); + return true; + } + + auto response = + cartographer::common::make_unique(); + if (proto->GetTypeName() == "cartographer.mapping.proto.PoseGraph") { + response->mutable_pose_graph()->CopyFrom(*proto); + } else if (proto->GetTypeName() == + "cartographer.mapping.proto.SerializedData") { + response->mutable_serialized_data()->CopyFrom(*proto); + } else { + LOG(FATAL) << "Unsupported message type: " << proto->GetTypeName(); + } + writer.Write(std::move(response)); + return true; + }); + GetContext()->map_builder().SerializeState( + &proto_stream_writer); +} + +} // namespace handlers +} // namespace cartographer_grpc diff --git a/cartographer_grpc/handlers/write_map_handler.h b/cartographer_grpc/handlers/write_map_handler.h index 882eb4a..f45527c 100644 --- a/cartographer_grpc/handlers/write_map_handler.h +++ b/cartographer_grpc/handlers/write_map_handler.h @@ -17,12 +17,9 @@ #ifndef CARTOGRAPHER_GRPC_HANDLERS_WRITE_MAP_HANDLER_H #define CARTOGRAPHER_GRPC_HANDLERS_WRITE_MAP_HANDLER_H -#include "cartographer/common/make_unique.h" -#include "cartographer/io/in_memory_proto_stream.h" #include "cartographer_grpc/framework/rpc_handler.h" -#include "cartographer_grpc/map_builder_context_interface.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 { @@ -34,31 +31,7 @@ class WriteMapHandler std::string method_name() const override { return "/cartographer_grpc.proto.MapBuilderService/WriteMap"; } - void OnRequest(const google::protobuf::Empty& request) override { - auto writer = GetWriter(); - cartographer::io::ForwardingProtoStreamWriter proto_stream_writer( - [writer](const google::protobuf::Message* proto) { - if (!proto) { - writer.WritesDone(); - return true; - } - - auto response = - cartographer::common::make_unique(); - if (proto->GetTypeName() == "cartographer.mapping.proto.PoseGraph") { - response->mutable_pose_graph()->CopyFrom(*proto); - } else if (proto->GetTypeName() == - "cartographer.mapping.proto.SerializedData") { - response->mutable_serialized_data()->CopyFrom(*proto); - } else { - LOG(FATAL) << "Unsupported message type: " << proto->GetTypeName(); - } - writer.Write(std::move(response)); - return true; - }); - GetContext()->map_builder().SerializeState( - &proto_stream_writer); - } + void OnRequest(const google::protobuf::Empty& request) override; }; } // namespace handlers