From bf77b11645b9d98094da133e9d9ca729ea35da5c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= Date: Tue, 12 Dec 2017 22:36:44 +0100 Subject: [PATCH] Implement gRPC data handlers and SLAM thread. (#749) --- .../framework/execution_context.h | 5 +- cartographer_grpc/framework/rpc_handler.h | 4 ++ cartographer_grpc/framework/server.h | 5 ++ .../handlers/add_imu_data_handler.h | 52 ++++++++++++++++++ .../handlers/add_odometry_data_handler.h | 53 +++++++++++++++++++ .../handlers/add_rangefinder_data_handler.h | 53 +++++++++++++++++++ cartographer_grpc/map_builder_server.cc | 41 +++++++++++++- cartographer_grpc/map_builder_server.h | 28 ++++++++-- 8 files changed, 232 insertions(+), 9 deletions(-) create mode 100644 cartographer_grpc/handlers/add_imu_data_handler.h create mode 100644 cartographer_grpc/handlers/add_odometry_data_handler.h create mode 100644 cartographer_grpc/handlers/add_rangefinder_data_handler.h diff --git a/cartographer_grpc/framework/execution_context.h b/cartographer_grpc/framework/execution_context.h index 060c30c..00c97e1 100644 --- a/cartographer_grpc/framework/execution_context.h +++ b/cartographer_grpc/framework/execution_context.h @@ -30,9 +30,9 @@ namespace framework { // 'RpcHandler::GetContext()'. class ExecutionContext { public: + // Automatically locks an ExecutionContext for shared use by RPC handlers. // This non-movable, non-copyable class is used to broker access from various - // RPC handlers to the shared 'ExecutionContext'. Handles automatically lock - // the context they point to. + // RPC handlers to the shared 'ExecutionContext'. template class Synchronized { public: @@ -49,6 +49,7 @@ class ExecutionContext { cartographer::common::MutexLocker locker_; ExecutionContext* execution_context_; }; + virtual ~ExecutionContext() = default; cartographer::common::Mutex* lock() { return &lock_; } private: diff --git a/cartographer_grpc/framework/rpc_handler.h b/cartographer_grpc/framework/rpc_handler.h index 733f66d..ead2e7b 100644 --- a/cartographer_grpc/framework/rpc_handler.h +++ b/cartographer_grpc/framework/rpc_handler.h @@ -53,6 +53,10 @@ class RpcHandler : public RpcHandlerInterface { ExecutionContext::Synchronized GetContext() { return {execution_context_->lock(), execution_context_}; } + template + T* GetUnsynchronizedContext() { + return dynamic_cast(execution_context_); + } private: Rpc* rpc_; diff --git a/cartographer_grpc/framework/server.h b/cartographer_grpc/framework/server.h index d08ab98..e76bd09 100644 --- a/cartographer_grpc/framework/server.h +++ b/cartographer_grpc/framework/server.h @@ -96,6 +96,11 @@ class Server { // Sets the server-wide context object shared between RPC handlers. void SetExecutionContext(std::unique_ptr execution_context); + template + ExecutionContext::Synchronized GetContext() { + return {execution_context_->lock(), execution_context_.get()}; + } + private: Server(const Options& options); Server(const Server&) = delete; diff --git a/cartographer_grpc/handlers/add_imu_data_handler.h b/cartographer_grpc/handlers/add_imu_data_handler.h new file mode 100644 index 0000000..6851e9e --- /dev/null +++ b/cartographer_grpc/handlers/add_imu_data_handler.h @@ -0,0 +1,52 @@ +/* + * 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_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 "google/protobuf/empty.pb.h" + +namespace cartographer_grpc { +namespace handlers { + +class AddImuDataHandler + : public framework::RpcHandler, + google::protobuf::Empty> { + public: + 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(), + request.sensor_metadata().sensor_id(), + cartographer::sensor::FromProto(request.imu_data())); + } + + void OnReadsDone() { + Send(cartographer::common::make_unique()); + } +}; + +} // namespace handlers +} // namespace cartographer_grpc + +#endif // CARTOGRAPHER_GRPC_HANDLERS_ADD_IMU_DATA_HANDLER_H diff --git a/cartographer_grpc/handlers/add_odometry_data_handler.h b/cartographer_grpc/handlers/add_odometry_data_handler.h new file mode 100644 index 0000000..36582eb --- /dev/null +++ b/cartographer_grpc/handlers/add_odometry_data_handler.h @@ -0,0 +1,53 @@ +/* + * 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_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" + +namespace cartographer_grpc { +namespace handlers { + +class AddOdometryDataHandler + : public framework::RpcHandler< + framework::Stream, + google::protobuf::Empty> { + public: + 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(), + request.sensor_metadata().sensor_id(), + cartographer::sensor::FromProto(request.odometry_data())); + } + + void OnReadsDone() { + Send(cartographer::common::make_unique()); + } +}; + +} // namespace handlers +} // namespace cartographer_grpc + +#endif // CARTOGRAPHER_GRPC_HANDLERS_ADD_ODOMETRY_DATA_HANDLER_H diff --git a/cartographer_grpc/handlers/add_rangefinder_data_handler.h b/cartographer_grpc/handlers/add_rangefinder_data_handler.h new file mode 100644 index 0000000..682cf5b --- /dev/null +++ b/cartographer_grpc/handlers/add_rangefinder_data_handler.h @@ -0,0 +1,53 @@ +/* + * 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_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" + +namespace cartographer_grpc { +namespace handlers { + +class AddRangefinderDataHandler + : public framework::RpcHandler< + framework::Stream, + google::protobuf::Empty> { + public: + 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(), + request.sensor_metadata().sensor_id(), + cartographer::sensor::FromProto(request.timed_point_cloud_data())); + } + + void OnReadsDone() { + Send(cartographer::common::make_unique()); + } +}; + +} // namespace handlers +} // namespace cartographer_grpc + +#endif // CARTOGRAPHER_GRPC_HANDLERS_ADD_RANGEFINDER_DATA_HANDLER_H diff --git a/cartographer_grpc/map_builder_server.cc b/cartographer_grpc/map_builder_server.cc index da445ba..a65740c 100644 --- a/cartographer_grpc/map_builder_server.cc +++ b/cartographer_grpc/map_builder_server.cc @@ -13,8 +13,12 @@ * See the License for the specific language governing permissions and * limitations under the License. */ + #include "cartographer_grpc/map_builder_server.h" +#include "cartographer_grpc/handlers/add_imu_data_handler.h" +#include "cartographer_grpc/handlers/add_odometry_data_handler.h" +#include "cartographer_grpc/handlers/add_rangefinder_data_handler.h" #include "cartographer_grpc/handlers/add_trajectory_handler.h" #include "cartographer_grpc/handlers/finish_trajectory_handler.h" #include "cartographer_grpc/proto/map_builder_service.grpc.pb.h" @@ -22,6 +26,27 @@ namespace cartographer_grpc { +MapBuilderServer::MapBuilderContext::MapBuilderContext( + cartographer::mapping::MapBuilder* map_builder, + cartographer::common::BlockingQueue* sensor_data_queue) + : map_builder_(map_builder), sensor_data_queue_(sensor_data_queue) {} + +cartographer::mapping::MapBuilder& +MapBuilderServer::MapBuilderContext::map_builder() { + return *map_builder_; +} + +cartographer::common::BlockingQueue& +MapBuilderServer::MapBuilderContext::sensor_data_queue() { + return *sensor_data_queue_; +} + +void MapBuilderServer::MapBuilderContext::AddSensorDataToTrajectory( + const SensorData& sensor_data) { + sensor_data.sensor_data->AddToTrajectoryBuilder( + map_builder_->GetTrajectoryBuilder(sensor_data.trajectory_id)); +} + MapBuilderServer::MapBuilderServer( const proto::MapBuilderServerOptions& map_builder_server_options) : map_builder_(map_builder_server_options.map_builder_options()) { @@ -31,11 +56,20 @@ MapBuilderServer::MapBuilderServer( map_builder_server_options.num_grpc_threads()); server_builder.RegisterHandler("AddTrajectory"); + server_builder.RegisterHandler("AddOdometryData"); + server_builder + .RegisterHandler( + "AddImuData"); + server_builder.RegisterHandler( + "AddRangefinderData"); server_builder.RegisterHandler("FinishTrajectory"); grpc_server_ = server_builder.Build(); grpc_server_->SetExecutionContext( - cartographer::common::make_unique(&map_builder_)); + cartographer::common::make_unique( + &map_builder_, &sensor_data_queue_)); } void MapBuilderServer::Start() { @@ -60,8 +94,11 @@ void MapBuilderServer::Shutdown() { } void MapBuilderServer::ProcessSensorDataQueue() { + LOG(INFO) << "Starting SLAM thread."; while (!shutting_down_) { - // TODO(cschuet): Implement this. + SensorData sensor_data = sensor_data_queue_.Pop(); + grpc_server_->GetContext()->AddSensorDataToTrajectory( + sensor_data); } } diff --git a/cartographer_grpc/map_builder_server.h b/cartographer_grpc/map_builder_server.h index 36ca158..5872c83 100644 --- a/cartographer_grpc/map_builder_server.h +++ b/cartographer_grpc/map_builder_server.h @@ -28,18 +28,35 @@ namespace cartographer_grpc { class MapBuilderServer { public: + struct SensorData { + int trajectory_id; + std::unique_ptr sensor_data; + }; + class MapBuilderContext : public framework::ExecutionContext { public: - MapBuilderContext(cartographer::mapping::MapBuilder* map_builder) - : map_builder_(map_builder) {} - cartographer::mapping::MapBuilder& map_builder() { return *map_builder_; } + MapBuilderContext( + cartographer::mapping::MapBuilder *map_builder, + cartographer::common::BlockingQueue *sensor_data_queue); + cartographer::mapping::MapBuilder &map_builder(); + cartographer::common::BlockingQueue &sensor_data_queue(); + void AddSensorDataToTrajectory(const SensorData &sensor_data); + + template + void EnqueueSensorData(int trajectory_id, const std::string &sensor_id, + const SensorDataType &sensor_data) { + sensor_data_queue_->Push(MapBuilderServer::SensorData{ + trajectory_id, + cartographer::sensor::MakeDispatchable(sensor_id, sensor_data)}); + } private: - cartographer::mapping::MapBuilder* map_builder_; + cartographer::mapping::MapBuilder *map_builder_; + cartographer::common::BlockingQueue *sensor_data_queue_; }; MapBuilderServer( - const proto::MapBuilderServerOptions& map_builder_server_options); + const proto::MapBuilderServerOptions &map_builder_server_options); // Starts the gRPC server and the SLAM thread. void Start(); @@ -60,6 +77,7 @@ class MapBuilderServer { std::unique_ptr slam_thread_; std::unique_ptr grpc_server_; cartographer::mapping::MapBuilder map_builder_; + cartographer::common::BlockingQueue sensor_data_queue_; }; } // namespace cartographer_grpc