12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Introduce MapBuilderContextInterface (#869)

master
Christoph Schütte 2018-01-31 19:22:08 +01:00 committed by Wally B. Feed
parent ab890a8e15
commit 27e8c84f2e
22 changed files with 451 additions and 324 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
cartographer_grpc/framework/execution_context.h
View File

@ -49,7 +49,10 @@ class ExecutionContext {
cartographer::common::MutexLocker locker_;
ExecutionContext* execution_context_;
};
ExecutionContext() = default;
virtual ~ExecutionContext() = default;
ExecutionContext(const ExecutionContext&) = delete;
ExecutionContext& operator=(const ExecutionContext&) = delete;
cartographer::common::Mutex* lock() { return &lock_; }
private:

12
cartographer_grpc/handlers/add_fixed_frame_pose_data_handler.h
View File

@ -39,16 +39,16 @@ class AddFixedFramePoseDataHandler
// 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(),
GetUnsynchronizedContext<MapBuilderContext>()->EnqueueSensorData(
request.sensor_metadata().trajectory_id(),
cartographer::sensor::MakeDispatchable(
request.sensor_metadata().sensor_id(),
cartographer::sensor::FromProto(request.fixed_frame_pose_data()));
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<MapBuilderServer::MapBuilderContext>()
if (GetUnsynchronizedContext<MapBuilderContext>()
->local_trajectory_uploader()) {
auto data_request = cartographer::common::make_unique<
proto::AddFixedFramePoseDataRequest>();
@ -56,7 +56,7 @@ class AddFixedFramePoseDataHandler
request.sensor_metadata().sensor_id(),
request.sensor_metadata().trajectory_id(),
request.fixed_frame_pose_data(), data_request.get());
GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
GetUnsynchronizedContext<MapBuilderContext>()
->local_trajectory_uploader()
->EnqueueDataRequest(std::move(data_request));
}

12
cartographer_grpc/handlers/add_imu_data_handler.h
View File

@ -38,23 +38,23 @@ class AddImuDataHandler
// 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(),
GetUnsynchronizedContext<MapBuilderContext>()->EnqueueSensorData(
request.sensor_metadata().trajectory_id(),
cartographer::sensor::MakeDispatchable(
request.sensor_metadata().sensor_id(),
cartographer::sensor::FromProto(request.imu_data()));
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<MapBuilderServer::MapBuilderContext>()
if (GetUnsynchronizedContext<MapBuilderContext>()
->local_trajectory_uploader()) {
auto data_request =
cartographer::common::make_unique<proto::AddImuDataRequest>();
sensor::CreateAddImuDataRequest(request.sensor_metadata().sensor_id(),
request.sensor_metadata().trajectory_id(),
request.imu_data(), data_request.get());
GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
GetUnsynchronizedContext<MapBuilderContext>()
->local_trajectory_uploader()
->EnqueueDataRequest(std::move(data_request));
}

12
cartographer_grpc/handlers/add_landmark_data_handler.h
View File

@ -38,16 +38,16 @@ class AddLandmarkDataHandler
// 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(),
GetUnsynchronizedContext<MapBuilderContext>()->EnqueueSensorData(
request.sensor_metadata().trajectory_id(),
cartographer::sensor::MakeDispatchable(
request.sensor_metadata().sensor_id(),
cartographer::sensor::FromProto(request.landmark_data()));
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<MapBuilderServer::MapBuilderContext>()
if (GetUnsynchronizedContext<MapBuilderContext>()
->local_trajectory_uploader()) {
auto data_request =
cartographer::common::make_unique<proto::AddLandmarkDataRequest>();
@ -55,7 +55,7 @@ class AddLandmarkDataHandler
request.sensor_metadata().sensor_id(),
request.sensor_metadata().trajectory_id(), request.landmark_data(),
data_request.get());
GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
GetUnsynchronizedContext<MapBuilderContext>()
->local_trajectory_uploader()
->EnqueueDataRequest(std::move(data_request));
}

19
cartographer_grpc/handlers/add_local_slam_result_data_handler.h
View File

@ -37,16 +37,15 @@ class AddLocalSlamResultDataHandler
}
void OnRequest(const proto::AddLocalSlamResultDataRequest& request) override {
auto local_slam_result_data =
GetContext<MapBuilderServer::MapBuilderContext>()
->ProcessLocalSlamResultData(
request.sensor_metadata().sensor_id(),
cartographer::common::FromUniversal(
request.local_slam_result_data().timestamp()),
request.local_slam_result_data());
GetContext<MapBuilderServer::MapBuilderContext>()
->EnqueueLocalSlamResultData(request.sensor_metadata().trajectory_id(),
request.sensor_metadata().sensor_id(),
std::move(local_slam_result_data));
GetContext<MapBuilderContext>()->ProcessLocalSlamResultData(
request.sensor_metadata().sensor_id(),
cartographer::common::FromUniversal(
request.local_slam_result_data().timestamp()),
request.local_slam_result_data());
GetContext<MapBuilderContext>()->EnqueueLocalSlamResultData(
request.sensor_metadata().trajectory_id(),
request.sensor_metadata().sensor_id(),
std::move(local_slam_result_data));
}
void OnReadsDone() override {

12
cartographer_grpc/handlers/add_odometry_data_handler.h
View File

@ -38,16 +38,16 @@ class AddOdometryDataHandler
// 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(),
GetUnsynchronizedContext<MapBuilderContext>()->EnqueueSensorData(
request.sensor_metadata().trajectory_id(),
cartographer::sensor::MakeDispatchable(
request.sensor_metadata().sensor_id(),
cartographer::sensor::FromProto(request.odometry_data()));
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<MapBuilderServer::MapBuilderContext>()
if (GetUnsynchronizedContext<MapBuilderContext>()
->local_trajectory_uploader()) {
auto data_request =
cartographer::common::make_unique<proto::AddOdometryDataRequest>();
@ -55,7 +55,7 @@ class AddOdometryDataHandler
request.sensor_metadata().sensor_id(),
request.sensor_metadata().trajectory_id(), request.odometry_data(),
data_request.get());
GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
GetUnsynchronizedContext<MapBuilderContext>()
->local_trajectory_uploader()
->EnqueueDataRequest(std::move(data_request));
}

8
cartographer_grpc/handlers/add_rangefinder_data_handler.h
View File

@ -38,11 +38,11 @@ class AddRangefinderDataHandler
// 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(),
GetUnsynchronizedContext<MapBuilderContext>()->EnqueueSensorData(
request.sensor_metadata().trajectory_id(),
cartographer::sensor::MakeDispatchable(
request.sensor_metadata().sensor_id(),
cartographer::sensor::FromProto(request.timed_point_cloud_data()));
cartographer::sensor::FromProto(request.timed_point_cloud_data())));
}
void OnReadsDone() override {

14
cartographer_grpc/handlers/add_trajectory_handler.h
View File

@ -35,7 +35,7 @@ class AddTrajectoryHandler
}
void OnRequest(const proto::AddTrajectoryRequest& request) override {
auto local_slam_result_callback =
GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
GetUnsynchronizedContext<MapBuilderContext>()
->GetLocalSlamResultCallbackForSubscriptions();
std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
expected_sensor_ids;
@ -43,12 +43,10 @@ class AddTrajectoryHandler
expected_sensor_ids.insert(sensor::FromProto(sensor_id));
}
const int trajectory_id =
GetContext<MapBuilderServer::MapBuilderContext>()
->map_builder()
.AddTrajectoryBuilder(expected_sensor_ids,
request.trajectory_builder_options(),
local_slam_result_callback);
if (GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
GetContext<MapBuilderContext>()->map_builder().AddTrajectoryBuilder(
expected_sensor_ids, request.trajectory_builder_options(),
local_slam_result_callback);
if (GetUnsynchronizedContext<MapBuilderContext>()
->local_trajectory_uploader()) {
auto trajectory_builder_options = request.trajectory_builder_options();
@ -58,7 +56,7 @@ class AddTrajectoryHandler
trajectory_builder_options.clear_trajectory_builder_2d_options();
trajectory_builder_options.clear_trajectory_builder_3d_options();
GetContext<MapBuilderServer::MapBuilderContext>()
GetContext<MapBuilderContext>()
->local_trajectory_uploader()
->AddTrajectory(trajectory_id, expected_sensor_ids,
trajectory_builder_options);

13
cartographer_grpc/handlers/finish_trajectory_handler.h
View File

@ -34,14 +34,13 @@ class FinishTrajectoryHandler
return "/cartographer_grpc.proto.MapBuilderService/FinishTrajectory";
}
void OnRequest(const proto::FinishTrajectoryRequest& request) override {
GetContext<MapBuilderServer::MapBuilderContext>()
->map_builder()
.FinishTrajectory(request.trajectory_id());
GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
->NotifyFinishTrajectory(request.trajectory_id());
if (GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
GetContext<MapBuilderContext>()->map_builder().FinishTrajectory(
request.trajectory_id());
GetUnsynchronizedContext<MapBuilderContext>()->NotifyFinishTrajectory(
request.trajectory_id());
if (GetUnsynchronizedContext<MapBuilderContext>()
->local_trajectory_uploader()) {
GetContext<MapBuilderServer::MapBuilderContext>()
GetContext<MapBuilderContext>()
->local_trajectory_uploader()
->FinishTrajectory(request.trajectory_id());
}

2
cartographer_grpc/handlers/get_all_submap_poses.h
View File

@ -34,7 +34,7 @@ class GetAllSubmapPosesHandler
return "/cartographer_grpc.proto.MapBuilderService/GetAllSubmapPoses";
}
void OnRequest(const google::protobuf::Empty& request) override {
auto submap_poses = GetContext<MapBuilderServer::MapBuilderContext>()
auto submap_poses = GetContext<MapBuilderContext>()
->map_builder()
.pose_graph()
->GetAllSubmapPoses();

2
cartographer_grpc/handlers/get_constraints_handler.h
View File

@ -35,7 +35,7 @@ class GetConstraintsHandler
return "/cartographer_grpc.proto.MapBuilderService/GetConstraints";
}
void OnRequest(const google::protobuf::Empty& request) override {
auto constraints = GetContext<MapBuilderServer::MapBuilderContext>()
auto constraints = GetContext<MapBuilderContext>()
->map_builder()
.pose_graph()
->constraints();

2
cartographer_grpc/handlers/get_local_to_global_transform_handler.h
View File

@ -33,7 +33,7 @@ class GetLocalToGlobalTransformHandler
auto response = cartographer::common::make_unique<
proto::GetLocalToGlobalTransformResponse>();
auto local_to_global =
GetContext<MapBuilderServer::MapBuilderContext>()
GetContext<MapBuilderContext>()
->map_builder()
.pose_graph()
->GetLocalToGlobalTransform(request.trajectory_id());

12
cartographer_grpc/handlers/get_submap_handler.h
View File

@ -36,13 +36,11 @@ class GetSubmapHandler
void OnRequest(const proto::GetSubmapRequest &request) override {
auto response =
cartographer::common::make_unique<proto::GetSubmapResponse>();
response->set_error_msg(GetContext<MapBuilderServer::MapBuilderContext>()
->map_builder()
.SubmapToProto(
cartographer::mapping::SubmapId{
request.submap_id().trajectory_id(),
request.submap_id().submap_index()},
response->mutable_submap_query_response()));
response->set_error_msg(
GetContext<MapBuilderContext>()->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));
}
};

2
cartographer_grpc/handlers/get_trajectory_node_poses_handler.h
View File

@ -34,7 +34,7 @@ class GetTrajectoryNodePosesHandler
return "/cartographer_grpc.proto.MapBuilderService/GetTrajectoryNodePoses";
}
void OnRequest(const google::protobuf::Empty& request) override {
auto node_poses = GetContext<MapBuilderServer::MapBuilderContext>()
auto node_poses = GetContext<MapBuilderContext>()
->map_builder()
.pose_graph()
->GetTrajectoryNodePoses();

3
cartographer_grpc/handlers/load_map_handler.h
View File

@ -48,8 +48,7 @@ class LoadMapHandler
}
void OnReadsDone() override {
GetContext<MapBuilderServer::MapBuilderContext>()->map_builder().LoadMap(
&reader_);
GetContext<MapBuilderContext>()->map_builder().LoadMap(&reader_);
Send(cartographer::common::make_unique<google::protobuf::Empty>());
}

22
cartographer_grpc/handlers/receive_local_slam_results_handler.h
View File

@ -36,12 +36,13 @@ class ReceiveLocalSlamResultsHandler
void OnRequest(
const proto::ReceiveLocalSlamResultsRequest& request) override {
auto writer = GetWriter();
MapBuilderServer::SubscriptionId subscription_id =
GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
MapBuilderContextInterface::SubscriptionId subscription_id =
GetUnsynchronizedContext<MapBuilderContext>()
->SubscribeLocalSlamResults(
request.trajectory_id(),
[writer](std::unique_ptr<MapBuilderServer::LocalSlamResult>
local_slam_result) {
[writer](
std::unique_ptr<MapBuilderContextInterface::LocalSlamResult>
local_slam_result) {
if (local_slam_result) {
writer.Write(
GenerateResponse(std::move(local_slam_result)));
@ -52,14 +53,13 @@ class ReceiveLocalSlamResultsHandler
}
});
subscription_id_ =
cartographer::common::make_unique<MapBuilderServer::SubscriptionId>(
subscription_id);
subscription_id_ = cartographer::common::make_unique<
MapBuilderContextInterface::SubscriptionId>(subscription_id);
}
static std::unique_ptr<proto::ReceiveLocalSlamResultsResponse>
GenerateResponse(
std::unique_ptr<MapBuilderServer::LocalSlamResult> local_slam_result) {
GenerateResponse(std::unique_ptr<MapBuilderContextInterface::LocalSlamResult>
local_slam_result) {
auto response = cartographer::common::make_unique<
proto::ReceiveLocalSlamResultsResponse>();
response->set_trajectory_id(local_slam_result->trajectory_id);
@ -80,13 +80,13 @@ class ReceiveLocalSlamResultsHandler
void OnFinish() override {
if (subscription_id_) {
GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
GetUnsynchronizedContext<MapBuilderContext>()
->UnsubscribeLocalSlamResults(*subscription_id_);
}
}
private:
std::unique_ptr<MapBuilderServer::SubscriptionId> subscription_id_;
std::unique_ptr<MapBuilderContextInterface::SubscriptionId> subscription_id_;
};
} // namespace handlers

2
cartographer_grpc/handlers/run_final_optimization_handler.h
View File

@ -35,7 +35,7 @@ class RunFinalOptimizationHandler
return "/cartographer_grpc.proto.MapBuilderService/RunFinalOptimization";
}
void OnRequest(const google::protobuf::Empty& request) override {
GetContext<MapBuilderServer::MapBuilderContext>()
GetContext<MapBuilderContext>()
->map_builder()
.pose_graph()
->RunFinalOptimization();

195
cartographer_grpc/map_builder_context.cc Normal file
View File

@ -0,0 +1,195 @@
/*
* 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/map_builder_context.h"
#include "cartographer_grpc/map_builder_server.h"
namespace cartographer_grpc {
MapBuilderContext::MapBuilderContext(MapBuilderServer* map_builder_server)
: map_builder_server_(map_builder_server) {}
cartographer::mapping::MapBuilderInterface& MapBuilderContext::map_builder() {
return *map_builder_server_->map_builder_;
}
cartographer::common::BlockingQueue<
std::unique_ptr<MapBuilderContextInterface::Data>>&
MapBuilderContext::sensor_data_queue() {
return map_builder_server_->incoming_data_queue_;
}
cartographer::mapping::TrajectoryBuilderInterface::LocalSlamResultCallback
MapBuilderContext::GetLocalSlamResultCallbackForSubscriptions() {
MapBuilderServer* map_builder_server = map_builder_server_;
return [map_builder_server](
int trajectory_id, cartographer::common::Time time,
cartographer::transform::Rigid3d local_pose,
cartographer::sensor::RangeData range_data,
std::unique_ptr<const cartographer::mapping::
TrajectoryBuilderInterface::InsertionResult>
insertion_result) {
map_builder_server->OnLocalSlamResult(trajectory_id, time, local_pose,
std::move(range_data),
std::move(insertion_result));
};
}
void MapBuilderContext::AddSensorDataToTrajectory(const Data& sensor_data) {
sensor_data.data->AddToTrajectoryBuilder(
map_builder_server_->map_builder_->GetTrajectoryBuilder(
sensor_data.trajectory_id));
}
MapBuilderContextInterface::SubscriptionId
MapBuilderContext::SubscribeLocalSlamResults(
int trajectory_id, LocalSlamSubscriptionCallback callback) {
return map_builder_server_->SubscribeLocalSlamResults(trajectory_id,
callback);
}
void MapBuilderContext::UnsubscribeLocalSlamResults(
const SubscriptionId& subscription_id) {
map_builder_server_->UnsubscribeLocalSlamResults(subscription_id);
}
void MapBuilderContext::NotifyFinishTrajectory(int trajectory_id) {
map_builder_server_->NotifyFinishTrajectory(trajectory_id);
}
std::shared_ptr<cartographer::mapping_2d::Submap>
MapBuilderContext::UpdateSubmap2D(
const cartographer::mapping::proto::Submap& proto) {
CHECK(proto.has_submap_2d());
cartographer::mapping::SubmapId submap_id{proto.submap_id().trajectory_id(),
proto.submap_id().submap_index()};
std::shared_ptr<cartographer::mapping_2d::Submap> submap_2d_ptr;
auto submap_it = unfinished_submaps_.find(submap_id);
if (submap_it == unfinished_submaps_.end()) {
// Seeing a submap for the first time it should never be finished.
CHECK(!proto.submap_2d().finished());
submap_2d_ptr =
std::make_shared<cartographer::mapping_2d::Submap>(proto.submap_2d());
unfinished_submaps_.Insert(submap_id, submap_2d_ptr);
} else {
submap_2d_ptr = std::dynamic_pointer_cast<cartographer::mapping_2d::Submap>(
submap_it->data);
CHECK(submap_2d_ptr);
submap_2d_ptr->UpdateFromProto(proto);
// If the submap was just finished by the recent update, remove it from the
// list of unfinished submaps.
if (submap_2d_ptr->finished()) {
unfinished_submaps_.Trim(submap_id);
} else {
// If the submap is unfinished set the 'num_range_data' to 0 since we
// haven't changed the HybridGrid.
submap_2d_ptr->SetNumRangeData(0);
}
}
return submap_2d_ptr;
}
std::shared_ptr<cartographer::mapping_3d::Submap>
MapBuilderContext::UpdateSubmap3D(
const cartographer::mapping::proto::Submap& proto) {
CHECK(proto.has_submap_3d());
cartographer::mapping::SubmapId submap_id{proto.submap_id().trajectory_id(),
proto.submap_id().submap_index()};
std::shared_ptr<cartographer::mapping_3d::Submap> submap_3d_ptr;
auto submap_it = unfinished_submaps_.find(submap_id);
if (submap_it == unfinished_submaps_.end()) {
// Seeing a submap for the first time it should never be finished.
CHECK(!proto.submap_3d().finished());
submap_3d_ptr =
std::make_shared<cartographer::mapping_3d::Submap>(proto.submap_3d());
unfinished_submaps_.Insert(submap_id, submap_3d_ptr);
submap_it = unfinished_submaps_.find(submap_id);
} else {
submap_3d_ptr = std::dynamic_pointer_cast<cartographer::mapping_3d::Submap>(
submap_it->data);
CHECK(submap_3d_ptr);
// Update submap with information in incoming request.
submap_3d_ptr->UpdateFromProto(proto);
// If the submap was just finished by the recent update, remove it from the
// list of unfinished submaps.
if (submap_3d_ptr->finished()) {
unfinished_submaps_.Trim(submap_id);
} else {
// If the submap is unfinished set the 'num_range_data' to 0 since we
// haven't changed the HybridGrid.
submap_3d_ptr->SetNumRangeData(0);
}
}
return submap_3d_ptr;
}
std::unique_ptr<cartographer::mapping::LocalSlamResultData>
MapBuilderContext::ProcessLocalSlamResultData(
const std::string& sensor_id, cartographer::common::Time time,
const cartographer::mapping::proto::LocalSlamResultData& proto) {
CHECK_GE(proto.submaps().size(), 1);
CHECK(proto.submaps(0).has_submap_2d() || proto.submaps(0).has_submap_3d());
if (proto.submaps(0).has_submap_2d()) {
std::vector<std::shared_ptr<const cartographer::mapping_2d::Submap>>
submaps;
for (const auto& submap_proto : proto.submaps()) {
submaps.push_back(UpdateSubmap2D(submap_proto));
}
return cartographer::common::make_unique<
cartographer::mapping::LocalSlamResult2D>(
sensor_id, time,
std::make_shared<const cartographer::mapping::TrajectoryNode::Data>(
cartographer::mapping::FromProto(proto.node_data())),
submaps);
} else {
std::vector<std::shared_ptr<const cartographer::mapping_3d::Submap>>
submaps;
for (const auto& submap_proto : proto.submaps()) {
submaps.push_back(UpdateSubmap3D(submap_proto));
}
return cartographer::common::make_unique<
cartographer::mapping::LocalSlamResult3D>(
sensor_id, time,
std::make_shared<const cartographer::mapping::TrajectoryNode::Data>(
cartographer::mapping::FromProto(proto.node_data())),
std::move(submaps));
}
}
LocalTrajectoryUploader* MapBuilderContext::local_trajectory_uploader() {
return map_builder_server_->local_trajectory_uploader_.get();
}
void MapBuilderContext::EnqueueSensorData(
int trajectory_id, std::unique_ptr<cartographer::sensor::Data> data) {
map_builder_server_->incoming_data_queue_.Push(
cartographer::common::make_unique<Data>(
Data{trajectory_id, std::move(data)}));
}
void MapBuilderContext::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<Data>(
Data{trajectory_id, std::move(local_slam_result_data)}));
}
} // namespace cartographer_grpc

67
cartographer_grpc/map_builder_context.h Normal file
View File

@ -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.
*/
#ifndef CARTOGRAPHER_GRPC_MAP_BUILDER_CONTEXT_H
#define CARTOGRAPHER_GRPC_MAP_BUILDER_CONTEXT_H
#include "cartographer/mapping_2d/submaps.h"
#include "cartographer/mapping_3d/submaps.h"
#include "cartographer_grpc/map_builder_context_interface.h"
namespace cartographer_grpc {
class MapBuilderContext : public MapBuilderContextInterface {
public:
MapBuilderContext(MapBuilderServer* map_builder_server);
cartographer::mapping::MapBuilderInterface& map_builder() override;
cartographer::common::BlockingQueue<std::unique_ptr<Data>>&
sensor_data_queue() override;
cartographer::mapping::TrajectoryBuilderInterface::LocalSlamResultCallback
GetLocalSlamResultCallbackForSubscriptions() override;
void AddSensorDataToTrajectory(const Data& sensor_data) override;
SubscriptionId SubscribeLocalSlamResults(
int trajectory_id, LocalSlamSubscriptionCallback callback) override;
void UnsubscribeLocalSlamResults(
const SubscriptionId& subscription_id) override;
void NotifyFinishTrajectory(int trajectory_id) override;
std::unique_ptr<cartographer::mapping::LocalSlamResultData>
ProcessLocalSlamResultData(
const std::string& sensor_id, cartographer::common::Time time,
const cartographer::mapping::proto::LocalSlamResultData& proto) override;
LocalTrajectoryUploader* local_trajectory_uploader() override;
void EnqueueSensorData(
int trajectory_id,
std::unique_ptr<cartographer::sensor::Data> data) override;
void EnqueueLocalSlamResultData(
int trajectory_id, const std::string& sensor_id,
std::unique_ptr<cartographer::mapping::LocalSlamResultData>
local_slam_result_data) override;
private:
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);
MapBuilderServer* map_builder_server_;
cartographer::mapping::MapById<cartographer::mapping::SubmapId,
std::shared_ptr<cartographer::mapping::Submap>>
unfinished_submaps_;
};
} // namespace cartographer_grpc
#endif // CARTOGRAPHER_GRPC_MAP_BUILDER_CONTEXT_H

88
cartographer_grpc/map_builder_context_interface.h Normal file
View File

@ -0,0 +1,88 @@
/*
* 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.
*/
#ifndef CARTOGRAPHER_GRPC_MAP_BUILDER_CONTEXT_INTERFACE_H
#define CARTOGRAPHER_GRPC_MAP_BUILDER_CONTEXT_INTERFACE_H
#include "cartographer/common/blocking_queue.h"
#include "cartographer/mapping/map_builder_interface.h"
#include "cartographer/sensor/data.h"
#include "cartographer/sensor/range_data.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer_grpc/framework/execution_context.h"
#include "cartographer_grpc/local_trajectory_uploader.h"
namespace cartographer_grpc {
class MapBuilderServer;
class MapBuilderContextInterface : public framework::ExecutionContext {
public:
struct LocalSlamResult {
int trajectory_id;
cartographer::common::Time time;
cartographer::transform::Rigid3d local_pose;
std::shared_ptr<const cartographer::sensor::RangeData> range_data;
std::unique_ptr<const cartographer::mapping::TrajectoryBuilderInterface::
InsertionResult>
insertion_result;
};
// Calling with 'nullptr' signals subscribers that the subscription has ended.
using LocalSlamSubscriptionCallback =
std::function<void(std::unique_ptr<LocalSlamResult>)>;
struct Data {
int trajectory_id;
std::unique_ptr<cartographer::sensor::Data> data;
};
struct SubscriptionId {
const int trajectory_id;
const int subscription_index;
};
MapBuilderContextInterface() = default;
~MapBuilderContextInterface() = default;
MapBuilderContextInterface(const MapBuilderContextInterface&) = delete;
MapBuilderContextInterface& operator=(const MapBuilderContextInterface&) =
delete;
virtual cartographer::mapping::MapBuilderInterface& map_builder() = 0;
virtual cartographer::common::BlockingQueue<std::unique_ptr<Data>>&
sensor_data_queue() = 0;
virtual cartographer::mapping::TrajectoryBuilderInterface::
LocalSlamResultCallback
GetLocalSlamResultCallbackForSubscriptions() = 0;
virtual void AddSensorDataToTrajectory(const Data& sensor_data) = 0;
virtual SubscriptionId SubscribeLocalSlamResults(
int trajectory_id, LocalSlamSubscriptionCallback callback) = 0;
virtual void UnsubscribeLocalSlamResults(
const SubscriptionId& subscription_id) = 0;
virtual void NotifyFinishTrajectory(int trajectory_id) = 0;
virtual std::unique_ptr<cartographer::mapping::LocalSlamResultData>
ProcessLocalSlamResultData(
const std::string& sensor_id, cartographer::common::Time time,
const cartographer::mapping::proto::LocalSlamResultData& proto) = 0;
virtual LocalTrajectoryUploader* local_trajectory_uploader() = 0;
virtual void EnqueueSensorData(
int trajectory_id, std::unique_ptr<cartographer::sensor::Data> data) = 0;
virtual void EnqueueLocalSlamResultData(
int trajectory_id, const std::string& sensor_id,
std::unique_ptr<cartographer::mapping::LocalSlamResultData>
local_slam_result_data) = 0;
};
} // namespace cartographer_grpc
#endif // CARTOGRAPHER_GRPC_MAP_BUILDER_CONTEXT_INTERFACE_H

184
cartographer_grpc/map_builder_server.cc
View File

@ -44,163 +44,6 @@ const cartographer::common::Duration kPopTimeout =
} // namespace
MapBuilderServer::MapBuilderContext::MapBuilderContext(
MapBuilderServer* map_builder_server)
: map_builder_server_(map_builder_server) {}
cartographer::mapping::MapBuilderInterface&
MapBuilderServer::MapBuilderContext::map_builder() {
return *map_builder_server_->map_builder_;
}
cartographer::common::BlockingQueue<std::unique_ptr<MapBuilderServer::Data>>&
MapBuilderServer::MapBuilderContext::sensor_data_queue() {
return map_builder_server_->incoming_data_queue_;
}
cartographer::mapping::TrajectoryBuilderInterface::LocalSlamResultCallback
MapBuilderServer::MapBuilderContext::
GetLocalSlamResultCallbackForSubscriptions() {
MapBuilderServer* map_builder_server = map_builder_server_;
return [map_builder_server](
int trajectory_id, cartographer::common::Time time,
cartographer::transform::Rigid3d local_pose,
cartographer::sensor::RangeData range_data,
std::unique_ptr<const cartographer::mapping::
TrajectoryBuilderInterface::InsertionResult>
insertion_result) {
map_builder_server->OnLocalSlamResult(trajectory_id, time, local_pose,
std::move(range_data),
std::move(insertion_result));
};
}
void MapBuilderServer::MapBuilderContext::AddSensorDataToTrajectory(
const Data& sensor_data) {
sensor_data.data->AddToTrajectoryBuilder(
map_builder_server_->map_builder_->GetTrajectoryBuilder(
sensor_data.trajectory_id));
}
MapBuilderServer::SubscriptionId
MapBuilderServer::MapBuilderContext::SubscribeLocalSlamResults(
int trajectory_id, LocalSlamSubscriptionCallback callback) {
return map_builder_server_->SubscribeLocalSlamResults(trajectory_id,
callback);
}
void MapBuilderServer::MapBuilderContext::UnsubscribeLocalSlamResults(
const SubscriptionId& subscription_id) {
map_builder_server_->UnsubscribeLocalSlamResults(subscription_id);
}
void MapBuilderServer::MapBuilderContext::NotifyFinishTrajectory(
int trajectory_id) {
map_builder_server_->NotifyFinishTrajectory(trajectory_id);
}
std::shared_ptr<cartographer::mapping_2d::Submap>
MapBuilderServer::MapBuilderContext::UpdateSubmap2D(
const cartographer::mapping::proto::Submap& proto) {
CHECK(proto.has_submap_2d());
cartographer::mapping::SubmapId submap_id{proto.submap_id().trajectory_id(),
proto.submap_id().submap_index()};
std::shared_ptr<cartographer::mapping_2d::Submap> submap_2d_ptr;
auto submap_it = unfinished_submaps_.find(submap_id);
if (submap_it == unfinished_submaps_.end()) {
// Seeing a submap for the first time it should never be finished.
CHECK(!proto.submap_2d().finished());
submap_2d_ptr =
std::make_shared<cartographer::mapping_2d::Submap>(proto.submap_2d());
unfinished_submaps_.Insert(submap_id, submap_2d_ptr);
} else {
submap_2d_ptr = std::dynamic_pointer_cast<cartographer::mapping_2d::Submap>(
submap_it->data);
CHECK(submap_2d_ptr);
submap_2d_ptr->UpdateFromProto(proto);
// If the submap was just finished by the recent update, remove it from the
// list of unfinished submaps.
if (submap_2d_ptr->finished()) {
unfinished_submaps_.Trim(submap_id);
} else {
// If the submap is unfinished set the 'num_range_data' to 0 since we
// haven't changed the HybridGrid.
submap_2d_ptr->SetNumRangeData(0);
}
}
return submap_2d_ptr;
}
std::shared_ptr<cartographer::mapping_3d::Submap>
MapBuilderServer::MapBuilderContext::UpdateSubmap3D(
const cartographer::mapping::proto::Submap& proto) {
CHECK(proto.has_submap_3d());
cartographer::mapping::SubmapId submap_id{proto.submap_id().trajectory_id(),
proto.submap_id().submap_index()};
std::shared_ptr<cartographer::mapping_3d::Submap> submap_3d_ptr;
auto submap_it = unfinished_submaps_.find(submap_id);
if (submap_it == unfinished_submaps_.end()) {
// Seeing a submap for the first time it should never be finished.
CHECK(!proto.submap_3d().finished());
submap_3d_ptr =
std::make_shared<cartographer::mapping_3d::Submap>(proto.submap_3d());
unfinished_submaps_.Insert(submap_id, submap_3d_ptr);
submap_it = unfinished_submaps_.find(submap_id);
} else {
submap_3d_ptr = std::dynamic_pointer_cast<cartographer::mapping_3d::Submap>(
submap_it->data);
CHECK(submap_3d_ptr);
// Update submap with information in incoming request.
submap_3d_ptr->UpdateFromProto(proto);
// If the submap was just finished by the recent update, remove it from the
// list of unfinished submaps.
if (submap_3d_ptr->finished()) {
unfinished_submaps_.Trim(submap_id);
} else {
// If the submap is unfinished set the 'num_range_data' to 0 since we
// haven't changed the HybridGrid.
submap_3d_ptr->SetNumRangeData(0);
}
}
return submap_3d_ptr;
}
std::unique_ptr<cartographer::mapping::LocalSlamResultData>
MapBuilderServer::MapBuilderContext::ProcessLocalSlamResultData(
const std::string& sensor_id, cartographer::common::Time time,
const cartographer::mapping::proto::LocalSlamResultData& proto) {
CHECK_GE(proto.submaps().size(), 1);
CHECK(proto.submaps(0).has_submap_2d() || proto.submaps(0).has_submap_3d());
if (proto.submaps(0).has_submap_2d()) {
std::vector<std::shared_ptr<const cartographer::mapping_2d::Submap>>
submaps;
for (const auto& submap_proto : proto.submaps()) {
submaps.push_back(UpdateSubmap2D(submap_proto));
}
return cartographer::common::make_unique<
cartographer::mapping::LocalSlamResult2D>(
sensor_id, time,
std::make_shared<const cartographer::mapping::TrajectoryNode::Data>(
cartographer::mapping::FromProto(proto.node_data())),
submaps);
} else {
std::vector<std::shared_ptr<const cartographer::mapping_3d::Submap>>
submaps;
for (const auto& submap_proto : proto.submaps()) {
submaps.push_back(UpdateSubmap3D(submap_proto));
}
return cartographer::common::make_unique<
cartographer::mapping::LocalSlamResult3D>(
sensor_id, time,
std::make_shared<const cartographer::mapping::TrajectoryNode::Data>(
cartographer::mapping::FromProto(proto.node_data())),
std::move(submaps));
}
}
MapBuilderServer::MapBuilderServer(
const proto::MapBuilderServerOptions& map_builder_server_options,
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder)
@ -267,7 +110,7 @@ void MapBuilderServer::Shutdown() {
void MapBuilderServer::ProcessSensorDataQueue() {
LOG(INFO) << "Starting SLAM thread.";
while (!shutting_down_) {
std::unique_ptr<Data> sensor_data =
std::unique_ptr<MapBuilderContextInterface::Data> sensor_data =
incoming_data_queue_.PopWithTimeout(kPopTimeout);
if (sensor_data) {
grpc_server_->GetContext<MapBuilderContext>()->AddSensorDataToTrajectory(
@ -324,23 +167,29 @@ void MapBuilderServer::OnLocalSlamResult(
const cartographer::mapping::TrajectoryBuilderInterface::
InsertionResult>(*insertion_result)
: nullptr;
LocalSlamSubscriptionCallback callback = entry.second;
callback(cartographer::common::make_unique<LocalSlamResult>(
LocalSlamResult{trajectory_id, time, local_pose, shared_range_data,
std::move(copy_of_insertion_result)}));
MapBuilderContextInterface::LocalSlamSubscriptionCallback callback =
entry.second;
callback(cartographer::common::make_unique<
MapBuilderContextInterface::LocalSlamResult>(
MapBuilderContextInterface::LocalSlamResult{
trajectory_id, time, local_pose, shared_range_data,
std::move(copy_of_insertion_result)}));
}
}
MapBuilderServer::SubscriptionId MapBuilderServer::SubscribeLocalSlamResults(
int trajectory_id, LocalSlamSubscriptionCallback callback) {
MapBuilderContextInterface::SubscriptionId
MapBuilderServer::SubscribeLocalSlamResults(
int trajectory_id,
MapBuilderContextInterface::LocalSlamSubscriptionCallback callback) {
cartographer::common::MutexLocker locker(&local_slam_subscriptions_lock_);
local_slam_subscriptions_[trajectory_id].emplace(current_subscription_index_,
callback);
return SubscriptionId{trajectory_id, current_subscription_index_++};
return MapBuilderContextInterface::SubscriptionId{
trajectory_id, current_subscription_index_++};
}
void MapBuilderServer::UnsubscribeLocalSlamResults(
const SubscriptionId& subscription_id) {
const MapBuilderContextInterface::SubscriptionId& subscription_id) {
cartographer::common::MutexLocker locker(&local_slam_subscriptions_lock_);
CHECK_EQ(local_slam_subscriptions_[subscription_id.trajectory_id].erase(
subscription_id.subscription_index),
@ -350,7 +199,8 @@ void MapBuilderServer::UnsubscribeLocalSlamResults(
void MapBuilderServer::NotifyFinishTrajectory(int trajectory_id) {
cartographer::common::MutexLocker locker(&local_slam_subscriptions_lock_);
for (auto& entry : local_slam_subscriptions_[trajectory_id]) {
LocalSlamSubscriptionCallback callback = entry.second;
MapBuilderContextInterface::LocalSlamSubscriptionCallback callback =
entry.second;
// 'nullptr' signals subscribers that the trajectory finished.
callback(nullptr);
}

89
cartographer_grpc/map_builder_server.h
View File

@ -26,86 +26,13 @@
#include "cartographer_grpc/framework/execution_context.h"
#include "cartographer_grpc/framework/server.h"
#include "cartographer_grpc/local_trajectory_uploader.h"
#include "cartographer_grpc/map_builder_context.h"
#include "cartographer_grpc/proto/map_builder_server_options.pb.h"
namespace cartographer_grpc {
class MapBuilderServer {
public:
struct LocalSlamResult {
int trajectory_id;
cartographer::common::Time time;
cartographer::transform::Rigid3d local_pose;
std::shared_ptr<const cartographer::sensor::RangeData> range_data;
std::unique_ptr<const cartographer::mapping::TrajectoryBuilderInterface::
InsertionResult>
insertion_result;
};
// Calling with 'nullptr' signals subscribers that the subscription has ended.
using LocalSlamSubscriptionCallback =
std::function<void(std::unique_ptr<LocalSlamResult>)>;
struct Data {
int trajectory_id;
std::unique_ptr<cartographer::sensor::Data> data;
};
struct SubscriptionId {
const int trajectory_id;
const int subscription_index;
};
class MapBuilderContext : public framework::ExecutionContext {
public:
MapBuilderContext(MapBuilderServer* map_builder_server);
cartographer::mapping::MapBuilderInterface& map_builder();
cartographer::common::BlockingQueue<std::unique_ptr<Data>>&
sensor_data_queue();
cartographer::mapping::TrajectoryBuilderInterface::LocalSlamResultCallback
GetLocalSlamResultCallbackForSubscriptions();
void AddSensorDataToTrajectory(const Data& sensor_data);
SubscriptionId SubscribeLocalSlamResults(
int trajectory_id, LocalSlamSubscriptionCallback callback);
void UnsubscribeLocalSlamResults(const SubscriptionId& subscription_id);
void NotifyFinishTrajectory(int trajectory_id);
std::unique_ptr<cartographer::mapping::LocalSlamResultData>
ProcessLocalSlamResultData(
const std::string& sensor_id, cartographer::common::Time time,
const cartographer::mapping::proto::LocalSlamResultData& proto);
LocalTrajectoryUploader* local_trajectory_uploader() {
return map_builder_server_->local_trajectory_uploader_.get();
}
template <typename DataType>
void EnqueueSensorData(int trajectory_id, const std::string& sensor_id,
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)}));
}
private:
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);
MapBuilderServer* map_builder_server_;
cartographer::mapping::MapById<
cartographer::mapping::SubmapId,
std::shared_ptr<cartographer::mapping::Submap>>
unfinished_submaps_;
};
friend MapBuilderContext;
MapBuilderServer(
@ -129,7 +56,8 @@ class MapBuilderServer {
private:
using LocalSlamResultHandlerSubscriptions =
std::map<int /* subscription_index */, LocalSlamSubscriptionCallback>;
std::map<int /* subscription_index */,
MapBuilderContextInterface::LocalSlamSubscriptionCallback>;
void ProcessSensorDataQueue();
void StartSlamThread();
@ -140,16 +68,19 @@ class MapBuilderServer {
std::unique_ptr<const cartographer::mapping::TrajectoryBuilderInterface::
InsertionResult>
insertion_result);
SubscriptionId SubscribeLocalSlamResults(
int trajectory_id, LocalSlamSubscriptionCallback callback);
void UnsubscribeLocalSlamResults(const SubscriptionId& subscription_id);
MapBuilderContextInterface::SubscriptionId SubscribeLocalSlamResults(
int trajectory_id,
MapBuilderContextInterface::LocalSlamSubscriptionCallback callback);
void UnsubscribeLocalSlamResults(
const MapBuilderContextInterface::SubscriptionId& subscription_id);
void NotifyFinishTrajectory(int trajectory_id);
bool shutting_down_ = false;
std::unique_ptr<std::thread> slam_thread_;
std::unique_ptr<framework::Server> grpc_server_;
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder_;
cartographer::common::BlockingQueue<std::unique_ptr<Data>>
cartographer::common::BlockingQueue<
std::unique_ptr<MapBuilderContextInterface::Data>>
incoming_data_queue_;
cartographer::common::Mutex local_slam_subscriptions_lock_;
int current_subscription_index_ = 0;