Implement gRPC data handlers and SLAM thread. (#749)

master
Christoph Schütte 2017-12-12 22:36:44 +01:00 committed by Wally B. Feed
parent e596b75113
commit bf77b11645
8 changed files with 232 additions and 9 deletions

View File

@ -30,9 +30,9 @@ namespace framework {
// 'RpcHandler::GetContext<MyContext>()'. // 'RpcHandler::GetContext<MyContext>()'.
class ExecutionContext { class ExecutionContext {
public: public:
// Automatically locks an ExecutionContext for shared use by RPC handlers.
// This non-movable, non-copyable class is used to broker access from various // This non-movable, non-copyable class is used to broker access from various
// RPC handlers to the shared 'ExecutionContext'. Handles automatically lock // RPC handlers to the shared 'ExecutionContext'.
// the context they point to.
template <typename ContextType> template <typename ContextType>
class Synchronized { class Synchronized {
public: public:
@ -49,6 +49,7 @@ class ExecutionContext {
cartographer::common::MutexLocker locker_; cartographer::common::MutexLocker locker_;
ExecutionContext* execution_context_; ExecutionContext* execution_context_;
}; };
virtual ~ExecutionContext() = default;
cartographer::common::Mutex* lock() { return &lock_; } cartographer::common::Mutex* lock() { return &lock_; }
private: private:

View File

@ -53,6 +53,10 @@ class RpcHandler : public RpcHandlerInterface {
ExecutionContext::Synchronized<T> GetContext() { ExecutionContext::Synchronized<T> GetContext() {
return {execution_context_->lock(), execution_context_}; return {execution_context_->lock(), execution_context_};
} }
template <typename T>
T* GetUnsynchronizedContext() {
return dynamic_cast<T*>(execution_context_);
}
private: private:
Rpc* rpc_; Rpc* rpc_;

View File

@ -96,6 +96,11 @@ class Server {
// Sets the server-wide context object shared between RPC handlers. // Sets the server-wide context object shared between RPC handlers.
void SetExecutionContext(std::unique_ptr<ExecutionContext> execution_context); void SetExecutionContext(std::unique_ptr<ExecutionContext> execution_context);
template <typename T>
ExecutionContext::Synchronized<T> GetContext() {
return {execution_context_->lock(), execution_context_.get()};
}
private: private:
Server(const Options& options); Server(const Options& options);
Server(const Server&) = delete; Server(const Server&) = delete;

View File

@ -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<framework::Stream<proto::AddImuDataRequest>,
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<MapBuilderServer::MapBuilderContext>()
->EnqueueSensorData(
request.sensor_metadata().trajectory_id(),
request.sensor_metadata().sensor_id(),
cartographer::sensor::FromProto(request.imu_data()));
}
void OnReadsDone() {
Send(cartographer::common::make_unique<google::protobuf::Empty>());
}
};
} // namespace handlers
} // namespace cartographer_grpc
#endif // CARTOGRAPHER_GRPC_HANDLERS_ADD_IMU_DATA_HANDLER_H

View File

@ -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<proto::AddOdometryDataRequest>,
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<MapBuilderServer::MapBuilderContext>()
->EnqueueSensorData(
request.sensor_metadata().trajectory_id(),
request.sensor_metadata().sensor_id(),
cartographer::sensor::FromProto(request.odometry_data()));
}
void OnReadsDone() {
Send(cartographer::common::make_unique<google::protobuf::Empty>());
}
};
} // namespace handlers
} // namespace cartographer_grpc
#endif // CARTOGRAPHER_GRPC_HANDLERS_ADD_ODOMETRY_DATA_HANDLER_H

View File

@ -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<proto::AddRangefinderDataRequest>,
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<MapBuilderServer::MapBuilderContext>()
->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<google::protobuf::Empty>());
}
};
} // namespace handlers
} // namespace cartographer_grpc
#endif // CARTOGRAPHER_GRPC_HANDLERS_ADD_RANGEFINDER_DATA_HANDLER_H

View File

@ -13,8 +13,12 @@
* See the License for the specific language governing permissions and * See the License for the specific language governing permissions and
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer_grpc/map_builder_server.h" #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/add_trajectory_handler.h"
#include "cartographer_grpc/handlers/finish_trajectory_handler.h" #include "cartographer_grpc/handlers/finish_trajectory_handler.h"
#include "cartographer_grpc/proto/map_builder_service.grpc.pb.h" #include "cartographer_grpc/proto/map_builder_service.grpc.pb.h"
@ -22,6 +26,27 @@
namespace cartographer_grpc { namespace cartographer_grpc {
MapBuilderServer::MapBuilderContext::MapBuilderContext(
cartographer::mapping::MapBuilder* map_builder,
cartographer::common::BlockingQueue<SensorData>* 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::SensorData>&
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( MapBuilderServer::MapBuilderServer(
const proto::MapBuilderServerOptions& map_builder_server_options) const proto::MapBuilderServerOptions& map_builder_server_options)
: map_builder_(map_builder_server_options.map_builder_options()) { : map_builder_(map_builder_server_options.map_builder_options()) {
@ -31,11 +56,20 @@ MapBuilderServer::MapBuilderServer(
map_builder_server_options.num_grpc_threads()); map_builder_server_options.num_grpc_threads());
server_builder.RegisterHandler<handlers::AddTrajectoryHandler, server_builder.RegisterHandler<handlers::AddTrajectoryHandler,
proto::MapBuilderService>("AddTrajectory"); proto::MapBuilderService>("AddTrajectory");
server_builder.RegisterHandler<handlers::AddOdometryDataHandler,
proto::MapBuilderService>("AddOdometryData");
server_builder
.RegisterHandler<handlers::AddImuDataHandler, proto::MapBuilderService>(
"AddImuData");
server_builder.RegisterHandler<handlers::AddRangefinderDataHandler,
proto::MapBuilderService>(
"AddRangefinderData");
server_builder.RegisterHandler<handlers::FinishTrajectoryHandler, server_builder.RegisterHandler<handlers::FinishTrajectoryHandler,
proto::MapBuilderService>("FinishTrajectory"); proto::MapBuilderService>("FinishTrajectory");
grpc_server_ = server_builder.Build(); grpc_server_ = server_builder.Build();
grpc_server_->SetExecutionContext( grpc_server_->SetExecutionContext(
cartographer::common::make_unique<MapBuilderContext>(&map_builder_)); cartographer::common::make_unique<MapBuilderContext>(
&map_builder_, &sensor_data_queue_));
} }
void MapBuilderServer::Start() { void MapBuilderServer::Start() {
@ -60,8 +94,11 @@ void MapBuilderServer::Shutdown() {
} }
void MapBuilderServer::ProcessSensorDataQueue() { void MapBuilderServer::ProcessSensorDataQueue() {
LOG(INFO) << "Starting SLAM thread.";
while (!shutting_down_) { while (!shutting_down_) {
// TODO(cschuet): Implement this. SensorData sensor_data = sensor_data_queue_.Pop();
grpc_server_->GetContext<MapBuilderContext>()->AddSensorDataToTrajectory(
sensor_data);
} }
} }

View File

@ -28,18 +28,35 @@ namespace cartographer_grpc {
class MapBuilderServer { class MapBuilderServer {
public: public:
struct SensorData {
int trajectory_id;
std::unique_ptr<cartographer::sensor::Data> sensor_data;
};
class MapBuilderContext : public framework::ExecutionContext { class MapBuilderContext : public framework::ExecutionContext {
public: public:
MapBuilderContext(cartographer::mapping::MapBuilder* map_builder) MapBuilderContext(
: map_builder_(map_builder) {} cartographer::mapping::MapBuilder *map_builder,
cartographer::mapping::MapBuilder& map_builder() { return *map_builder_; } cartographer::common::BlockingQueue<SensorData> *sensor_data_queue);
cartographer::mapping::MapBuilder &map_builder();
cartographer::common::BlockingQueue<SensorData> &sensor_data_queue();
void AddSensorDataToTrajectory(const SensorData &sensor_data);
template <typename SensorDataType>
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: private:
cartographer::mapping::MapBuilder* map_builder_; cartographer::mapping::MapBuilder *map_builder_;
cartographer::common::BlockingQueue<SensorData> *sensor_data_queue_;
}; };
MapBuilderServer( MapBuilderServer(
const proto::MapBuilderServerOptions& map_builder_server_options); const proto::MapBuilderServerOptions &map_builder_server_options);
// Starts the gRPC server and the SLAM thread. // Starts the gRPC server and the SLAM thread.
void Start(); void Start();
@ -60,6 +77,7 @@ class MapBuilderServer {
std::unique_ptr<std::thread> slam_thread_; std::unique_ptr<std::thread> slam_thread_;
std::unique_ptr<framework::Server> grpc_server_; std::unique_ptr<framework::Server> grpc_server_;
cartographer::mapping::MapBuilder map_builder_; cartographer::mapping::MapBuilder map_builder_;
cartographer::common::BlockingQueue<SensorData> sensor_data_queue_;
}; };
} // namespace cartographer_grpc } // namespace cartographer_grpc