Implement gRPC data handlers and SLAM thread. (#749)
parent
e596b75113
commit
bf77b11645
|
@ -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:
|
||||||
|
|
|
@ -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_;
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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
|
||||||
|
|
Loading…
Reference in New Issue