From d313af867406a0888879b1eaea3198e3a98ee64c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= Date: Wed, 10 Jan 2018 17:26:04 +0100 Subject: [PATCH] Add InsertionResult to LocalSlamResult. (#801) --- .../mapping/global_trajectory_builder.h | 19 ++++++++++------ cartographer/mapping/map_builder_test.cc | 4 +++- .../mapping/trajectory_builder_interface.h | 8 ++++++- cartographer_grpc/client_server_test.cc | 10 +++++---- .../receive_local_slam_results_handler.h | 8 +++---- cartographer_grpc/map_builder_server.cc | 22 ++++++++++++------- cartographer_grpc/map_builder_server.h | 8 +++++-- .../mapping/trajectory_builder_stub.cc | 14 ++++++------ .../proto/map_builder_service.proto | 6 ++++- 9 files changed, 63 insertions(+), 36 deletions(-) diff --git a/cartographer/internal/mapping/global_trajectory_builder.h b/cartographer/internal/mapping/global_trajectory_builder.h index ff47580..05a22c2 100644 --- a/cartographer/internal/mapping/global_trajectory_builder.h +++ b/cartographer/internal/mapping/global_trajectory_builder.h @@ -58,18 +58,23 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface { // The range data has not been fully accumulated yet. return; } - std::unique_ptr node_id; + std::unique_ptr insertion_result; if (matching_result->insertion_result != nullptr) { - node_id = ::cartographer::common::make_unique( - pose_graph_->AddNode( - matching_result->insertion_result->constant_data, trajectory_id_, - matching_result->insertion_result->insertion_submaps)); - CHECK_EQ(node_id->trajectory_id, trajectory_id_); + auto node_id = pose_graph_->AddNode( + matching_result->insertion_result->constant_data, trajectory_id_, + matching_result->insertion_result->insertion_submaps); + CHECK_EQ(node_id.trajectory_id, trajectory_id_); + insertion_result = common::make_unique(InsertionResult{ + node_id, matching_result->insertion_result->constant_data, + std::vector>( + matching_result->insertion_result->insertion_submaps.begin(), + matching_result->insertion_result->insertion_submaps.end())}); } if (local_slam_result_callback_) { local_slam_result_callback_( trajectory_id_, matching_result->time, matching_result->local_pose, - std::move(matching_result->range_data_in_local), std::move(node_id)); + std::move(matching_result->range_data_in_local), + std::move(insertion_result)); } } diff --git a/cartographer/mapping/map_builder_test.cc b/cartographer/mapping/map_builder_test.cc index 465f373..f11dd50 100644 --- a/cartographer/mapping/map_builder_test.cc +++ b/cartographer/mapping/map_builder_test.cc @@ -81,7 +81,9 @@ class MapBuilderTest : public ::testing::Test { return [=](const int trajectory_id, const ::cartographer::common::Time time, const ::cartographer::transform::Rigid3d local_pose, ::cartographer::sensor::RangeData range_data_in_local, - const std::unique_ptr) { + const std::unique_ptr< + const cartographer::mapping::TrajectoryBuilderInterface:: + InsertionResult>) { local_slam_result_poses_.push_back(local_pose); }; } diff --git a/cartographer/mapping/trajectory_builder_interface.h b/cartographer/mapping/trajectory_builder_interface.h index af009a7..c84194f 100644 --- a/cartographer/mapping/trajectory_builder_interface.h +++ b/cartographer/mapping/trajectory_builder_interface.h @@ -44,6 +44,12 @@ proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions( // optimized pose estimates. class TrajectoryBuilderInterface { public: + struct InsertionResult { + NodeId node_id; + std::shared_ptr constant_data; + std::vector> insertion_submaps; + }; + // A callback which is called after local SLAM processes an accumulated // 'sensor::RangeData'. If the data was inserted into a submap, reports the // assigned 'NodeId', otherwise 'nullptr' if the data was filtered out. @@ -51,7 +57,7 @@ class TrajectoryBuilderInterface { std::function)>; + std::unique_ptr)>; TrajectoryBuilderInterface() {} virtual ~TrajectoryBuilderInterface() {} diff --git a/cartographer_grpc/client_server_test.cc b/cartographer_grpc/client_server_test.cc index 3668a80..87da928 100644 --- a/cartographer_grpc/client_server_test.cc +++ b/cartographer_grpc/client_server_test.cc @@ -111,10 +111,12 @@ class ClientServerTest : public ::testing::Test { cartographer::mapping::CreateTrajectoryBuilderOptions( trajectory_builder_parameters.get()); local_slam_result_callback_ = - [this](int, cartographer::common::Time, - cartographer::transform::Rigid3d local_pose, - cartographer::sensor::RangeData, - std::unique_ptr) { + [this]( + int, cartographer::common::Time, + cartographer::transform::Rigid3d local_pose, + cartographer::sensor::RangeData, + std::unique_ptr) { std::unique_lock lock(local_slam_result_mutex_); local_slam_result_poses_.push_back(local_pose); lock.unlock(); diff --git a/cartographer_grpc/handlers/receive_local_slam_results_handler.h b/cartographer_grpc/handlers/receive_local_slam_results_handler.h index 534c39c..0730a32 100644 --- a/cartographer_grpc/handlers/receive_local_slam_results_handler.h +++ b/cartographer_grpc/handlers/receive_local_slam_results_handler.h @@ -68,11 +68,9 @@ class ReceiveLocalSlamResultsHandler *response->mutable_range_data() = cartographer::sensor::ToProto(*local_slam_result->range_data); } - if (local_slam_result->node_id) { - response->mutable_node_id()->set_trajectory_id( - local_slam_result->node_id->trajectory_id); - response->mutable_node_id()->set_node_index( - local_slam_result->node_id->node_index); + if (local_slam_result->insertion_result) { + local_slam_result->insertion_result->node_id.ToProto( + response->mutable_insertion_result()->mutable_node_id()); } return response; } diff --git a/cartographer_grpc/map_builder_server.cc b/cartographer_grpc/map_builder_server.cc index 2f42214..50e07a0 100644 --- a/cartographer_grpc/map_builder_server.cc +++ b/cartographer_grpc/map_builder_server.cc @@ -62,10 +62,12 @@ MapBuilderServer::MapBuilderContext:: int trajectory_id, cartographer::common::Time time, cartographer::transform::Rigid3d local_pose, cartographer::sensor::RangeData range_data, - std::unique_ptr node_id) { + std::unique_ptr + insertion_result) { map_builder_server->OnLocalSlamResult(trajectory_id, time, local_pose, std::move(range_data), - std::move(node_id)); + std::move(insertion_result)); }; } @@ -184,19 +186,23 @@ void MapBuilderServer::OnLocalSlamResult( int trajectory_id, cartographer::common::Time time, cartographer::transform::Rigid3d local_pose, cartographer::sensor::RangeData range_data, - std::unique_ptr node_id) { + std::unique_ptr + insertion_result) { auto shared_range_data = std::make_shared(std::move(range_data)); cartographer::common::MutexLocker locker(&local_slam_subscriptions_lock_); for (auto& entry : local_slam_subscriptions_[trajectory_id]) { - auto copy_of_node_id = - node_id ? cartographer::common::make_unique< - const cartographer::mapping::NodeId>(*node_id) - : nullptr; + auto copy_of_insertion_result = + insertion_result + ? cartographer::common::make_unique< + const cartographer::mapping::TrajectoryBuilderInterface:: + InsertionResult>(*insertion_result) + : nullptr; LocalSlamSubscriptionCallback callback = entry.second; callback(cartographer::common::make_unique( LocalSlamResult{trajectory_id, time, local_pose, shared_range_data, - std::move(copy_of_node_id)})); + std::move(copy_of_insertion_result)})); } } diff --git a/cartographer_grpc/map_builder_server.h b/cartographer_grpc/map_builder_server.h index 9367198..86e37ce 100644 --- a/cartographer_grpc/map_builder_server.h +++ b/cartographer_grpc/map_builder_server.h @@ -35,7 +35,9 @@ class MapBuilderServer { cartographer::common::Time time; cartographer::transform::Rigid3d local_pose; std::shared_ptr range_data; - std::unique_ptr node_id; + std::unique_ptr + insertion_result; }; // Calling with 'nullptr' signals subscribers that the subscription has ended. using LocalSlamSubscriptionCallback = @@ -106,7 +108,9 @@ class MapBuilderServer { int trajectory_id, cartographer::common::Time time, cartographer::transform::Rigid3d local_pose, cartographer::sensor::RangeData range_data, - std::unique_ptr node_id); + std::unique_ptr + insertion_result); SubscriptionId SubscribeLocalSlamResults( int trajectory_id, LocalSlamSubscriptionCallback callback); void UnsubscribeLocalSlamResults(const SubscriptionId& subscription_id); diff --git a/cartographer_grpc/mapping/trajectory_builder_stub.cc b/cartographer_grpc/mapping/trajectory_builder_stub.cc index 46aeec1..79421c3 100644 --- a/cartographer_grpc/mapping/trajectory_builder_stub.cc +++ b/cartographer_grpc/mapping/trajectory_builder_stub.cc @@ -144,15 +144,15 @@ void TrajectoryBuilderStub::RunLocalSlamResultReader( cartographer::transform::ToRigid3(response.local_pose()); cartographer::sensor::RangeData range_data = cartographer::sensor::FromProto(response.range_data()); - auto node_id = - response.has_node_id() - ? cartographer::common::make_unique( - cartographer::mapping::NodeId{ - response.node_id().trajectory_id(), - response.node_id().node_index()}) + auto insertion_result = + response.has_insertion_result() + ? cartographer::common::make_unique( + InsertionResult{cartographer::mapping::NodeId{ + response.insertion_result().node_id().trajectory_id(), + response.insertion_result().node_id().node_index()}}) : nullptr; local_slam_result_callback(trajectory_id, time, local_pose, range_data, - std::move(node_id)); + std::move(insertion_result)); } client_reader->Finish(); } diff --git a/cartographer_grpc/proto/map_builder_service.proto b/cartographer_grpc/proto/map_builder_service.proto index ac062a5..b4cfe34 100644 --- a/cartographer_grpc/proto/map_builder_service.proto +++ b/cartographer_grpc/proto/map_builder_service.proto @@ -66,12 +66,16 @@ message ReceiveLocalSlamResultsRequest { int32 trajectory_id = 1; } +message LocalSlamInsertionResult { + cartographer.mapping.proto.NodeId node_id = 1; +} + message ReceiveLocalSlamResultsResponse { int32 trajectory_id = 1; int64 timestamp = 2; cartographer.transform.proto.Rigid3d local_pose = 3; cartographer.sensor.proto.RangeData range_data = 4; - cartographer.mapping.proto.NodeId node_id = 5; + LocalSlamInsertionResult insertion_result = 5; } message GetSubmapRequest {