2017-12-05 21:46:25 +08:00
|
|
|
/*
|
|
|
|
* 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_MAP_BUILDER_SERVER_H
|
|
|
|
#define CARTOGRAPHER_GRPC_MAP_BUILDER_SERVER_H
|
|
|
|
|
|
|
|
#include "cartographer/common/blocking_queue.h"
|
2017-12-18 23:32:34 +08:00
|
|
|
#include "cartographer/common/time.h"
|
2018-01-12 20:00:25 +08:00
|
|
|
#include "cartographer/mapping/local_slam_result_data.h"
|
2017-12-05 21:46:25 +08:00
|
|
|
#include "cartographer/mapping/map_builder.h"
|
2017-12-18 23:32:34 +08:00
|
|
|
#include "cartographer/mapping/trajectory_builder_interface.h"
|
2018-01-11 17:19:37 +08:00
|
|
|
#include "cartographer/sensor/dispatchable.h"
|
2017-12-05 21:46:25 +08:00
|
|
|
#include "cartographer_grpc/framework/execution_context.h"
|
|
|
|
#include "cartographer_grpc/framework/server.h"
|
|
|
|
#include "cartographer_grpc/proto/map_builder_server_options.pb.h"
|
|
|
|
|
|
|
|
namespace cartographer_grpc {
|
|
|
|
|
|
|
|
class MapBuilderServer {
|
|
|
|
public:
|
2017-12-20 16:46:54 +08:00
|
|
|
struct LocalSlamResult {
|
|
|
|
int trajectory_id;
|
|
|
|
cartographer::common::Time time;
|
|
|
|
cartographer::transform::Rigid3d local_pose;
|
|
|
|
std::shared_ptr<const cartographer::sensor::RangeData> range_data;
|
2018-01-11 00:26:04 +08:00
|
|
|
std::unique_ptr<const cartographer::mapping::TrajectoryBuilderInterface::
|
|
|
|
InsertionResult>
|
|
|
|
insertion_result;
|
2017-12-20 16:46:54 +08:00
|
|
|
};
|
2017-12-20 18:42:01 +08:00
|
|
|
// Calling with 'nullptr' signals subscribers that the subscription has ended.
|
2017-12-20 16:46:54 +08:00
|
|
|
using LocalSlamSubscriptionCallback =
|
|
|
|
std::function<void(std::unique_ptr<LocalSlamResult>)>;
|
2018-01-12 20:00:25 +08:00
|
|
|
struct Data {
|
2017-12-13 05:36:44 +08:00
|
|
|
int trajectory_id;
|
2018-01-12 20:00:25 +08:00
|
|
|
std::unique_ptr<cartographer::sensor::Data> data;
|
2017-12-13 05:36:44 +08:00
|
|
|
};
|
2017-12-18 23:32:34 +08:00
|
|
|
struct SubscriptionId {
|
|
|
|
const int trajectory_id;
|
|
|
|
const int subscription_index;
|
|
|
|
};
|
2017-12-13 05:36:44 +08:00
|
|
|
|
2017-12-05 21:46:25 +08:00
|
|
|
class MapBuilderContext : public framework::ExecutionContext {
|
|
|
|
public:
|
2017-12-18 23:32:34 +08:00
|
|
|
MapBuilderContext(MapBuilderServer* map_builder_server);
|
2017-12-19 21:36:25 +08:00
|
|
|
cartographer::mapping::MapBuilderInterface& map_builder();
|
2018-01-12 20:00:25 +08:00
|
|
|
cartographer::common::BlockingQueue<std::unique_ptr<Data>>&
|
2017-12-18 17:02:57 +08:00
|
|
|
sensor_data_queue();
|
2017-12-18 23:32:34 +08:00
|
|
|
cartographer::mapping::TrajectoryBuilderInterface::LocalSlamResultCallback
|
|
|
|
GetLocalSlamResultCallbackForSubscriptions();
|
2018-01-12 20:00:25 +08:00
|
|
|
void AddSensorDataToTrajectory(const Data& sensor_data);
|
2017-12-18 23:32:34 +08:00
|
|
|
SubscriptionId SubscribeLocalSlamResults(
|
|
|
|
int trajectory_id, LocalSlamSubscriptionCallback callback);
|
|
|
|
void UnsubscribeLocalSlamResults(const SubscriptionId& subscription_id);
|
2017-12-20 18:42:01 +08:00
|
|
|
void NotifyFinishTrajectory(int trajectory_id);
|
2018-01-12 20:00:25 +08:00
|
|
|
std::unique_ptr<cartographer::mapping::LocalSlamResultData>
|
|
|
|
ProcessLocalSlamResultData(
|
|
|
|
const std::string& sensor_id, cartographer::common::Time time,
|
|
|
|
const cartographer::mapping::proto::LocalSlamResultData& proto);
|
2017-12-13 05:36:44 +08:00
|
|
|
|
2018-01-12 20:00:25 +08:00
|
|
|
template <typename DataType>
|
2017-12-18 17:02:57 +08:00
|
|
|
void EnqueueSensorData(int trajectory_id, const std::string& sensor_id,
|
2018-01-12 20:00:25 +08:00
|
|
|
const DataType& data) {
|
|
|
|
map_builder_server_->incoming_data_queue_.Push(
|
|
|
|
cartographer::common::make_unique<MapBuilderServer::Data>(
|
|
|
|
MapBuilderServer::Data{
|
|
|
|
trajectory_id,
|
|
|
|
cartographer::sensor::MakeDispatchable(sensor_id, data)}));
|
|
|
|
}
|
|
|
|
|
|
|
|
void EnqueueLocalSlamResultData(
|
|
|
|
int trajectory_id, const std::string& sensor_id,
|
|
|
|
std::unique_ptr<cartographer::mapping::LocalSlamResultData>
|
|
|
|
local_slam_result_data) {
|
|
|
|
map_builder_server_->incoming_data_queue_.Push(
|
|
|
|
cartographer::common::make_unique<MapBuilderServer::Data>(
|
|
|
|
MapBuilderServer::Data{trajectory_id,
|
|
|
|
std::move(local_slam_result_data)}));
|
2017-12-13 05:36:44 +08:00
|
|
|
}
|
2017-12-05 21:46:25 +08:00
|
|
|
|
|
|
|
private:
|
2018-01-12 20:00:25 +08:00
|
|
|
std::shared_ptr<cartographer::mapping_2d::Submap> UpdateSubmap2D(
|
|
|
|
const cartographer::mapping::proto::Submap& proto);
|
|
|
|
std::shared_ptr<cartographer::mapping_3d::Submap> UpdateSubmap3D(
|
|
|
|
const cartographer::mapping::proto::Submap& proto);
|
|
|
|
|
2017-12-18 23:32:34 +08:00
|
|
|
MapBuilderServer* map_builder_server_;
|
2018-01-12 20:00:25 +08:00
|
|
|
cartographer::mapping::MapById<
|
|
|
|
cartographer::mapping::SubmapId,
|
|
|
|
std::shared_ptr<cartographer::mapping::Submap>>
|
|
|
|
unfinished_submaps_;
|
2017-12-05 21:46:25 +08:00
|
|
|
};
|
2017-12-18 23:32:34 +08:00
|
|
|
friend MapBuilderContext;
|
2017-12-05 21:46:25 +08:00
|
|
|
|
|
|
|
MapBuilderServer(
|
2017-12-19 03:47:00 +08:00
|
|
|
const proto::MapBuilderServerOptions& map_builder_server_options,
|
2017-12-19 21:36:25 +08:00
|
|
|
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder);
|
2017-12-05 21:46:25 +08:00
|
|
|
|
|
|
|
// Starts the gRPC server and the SLAM thread.
|
|
|
|
void Start();
|
|
|
|
|
|
|
|
// Waits for the 'MapBuilderServer' to shut down. Note: The server must be
|
|
|
|
// either shutting down or some other thread must call 'Shutdown()' for this
|
|
|
|
// function to ever return.
|
|
|
|
void WaitForShutdown();
|
|
|
|
|
2018-01-03 21:56:56 +08:00
|
|
|
// Waits until all computation is finished (for testing).
|
|
|
|
void WaitUntilIdle();
|
|
|
|
|
2017-12-05 21:46:25 +08:00
|
|
|
// Shuts down the gRPC server and the SLAM thread.
|
|
|
|
void Shutdown();
|
|
|
|
|
|
|
|
private:
|
2017-12-18 23:32:34 +08:00
|
|
|
using LocalSlamResultHandlerSubscriptions =
|
|
|
|
std::map<int /* subscription_index */, LocalSlamSubscriptionCallback>;
|
|
|
|
|
2017-12-05 21:46:25 +08:00
|
|
|
void ProcessSensorDataQueue();
|
|
|
|
void StartSlamThread();
|
2017-12-18 23:32:34 +08:00
|
|
|
void OnLocalSlamResult(
|
|
|
|
int trajectory_id, cartographer::common::Time time,
|
|
|
|
cartographer::transform::Rigid3d local_pose,
|
|
|
|
cartographer::sensor::RangeData range_data,
|
2018-01-11 00:26:04 +08:00
|
|
|
std::unique_ptr<const cartographer::mapping::TrajectoryBuilderInterface::
|
|
|
|
InsertionResult>
|
|
|
|
insertion_result);
|
2017-12-18 23:32:34 +08:00
|
|
|
SubscriptionId SubscribeLocalSlamResults(
|
|
|
|
int trajectory_id, LocalSlamSubscriptionCallback callback);
|
|
|
|
void UnsubscribeLocalSlamResults(const SubscriptionId& subscription_id);
|
2017-12-20 18:42:01 +08:00
|
|
|
void NotifyFinishTrajectory(int trajectory_id);
|
2017-12-05 21:46:25 +08:00
|
|
|
|
|
|
|
bool shutting_down_ = false;
|
|
|
|
std::unique_ptr<std::thread> slam_thread_;
|
|
|
|
std::unique_ptr<framework::Server> grpc_server_;
|
2017-12-19 21:36:25 +08:00
|
|
|
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder_;
|
2018-01-12 20:00:25 +08:00
|
|
|
cartographer::common::BlockingQueue<std::unique_ptr<Data>>
|
|
|
|
incoming_data_queue_;
|
2017-12-18 23:32:34 +08:00
|
|
|
cartographer::common::Mutex local_slam_subscriptions_lock_;
|
|
|
|
int current_subscription_index_ = 0;
|
|
|
|
std::map<int /* trajectory ID */, LocalSlamResultHandlerSubscriptions>
|
|
|
|
local_slam_subscriptions_ GUARDED_BY(local_slam_subscriptions_lock_);
|
2017-12-05 21:46:25 +08:00
|
|
|
};
|
|
|
|
|
|
|
|
} // namespace cartographer_grpc
|
|
|
|
|
|
|
|
#endif // CARTOGRAPHER_GRPC_MAP_BUILDER_SERVER_H
|