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

Add InsertionResult to LocalSlamResult. (#801)

master
Christoph Schütte 2018-01-10 17:26:04 +01:00 committed by GitHub
parent c7a8c5fda9
commit d313af8674
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
9 changed files with 63 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
cartographer/internal/mapping/global_trajectory_builder.h
View File

@ -58,18 +58,23 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {
// The range data has not been fully accumulated yet.
return;
}
std::unique_ptr<mapping::NodeId> node_id;
std::unique_ptr<InsertionResult> insertion_result;
if (matching_result->insertion_result != nullptr) {
node_id = ::cartographer::common::make_unique<mapping::NodeId>(
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>(InsertionResult{
node_id, matching_result->insertion_result->constant_data,
std::vector<std::shared_ptr<const Submap>>(
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));
}
}

4
cartographer/mapping/map_builder_test.cc
View File

@ -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 ::cartographer::mapping::NodeId>) {
const std::unique_ptr<
const cartographer::mapping::TrajectoryBuilderInterface::
InsertionResult>) {
local_slam_result_poses_.push_back(local_pose);
};
}

8
cartographer/mapping/trajectory_builder_interface.h
View File

@ -44,6 +44,12 @@ proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions(
// optimized pose estimates.
class TrajectoryBuilderInterface {
public:
struct InsertionResult {
NodeId node_id;
std::shared_ptr<const TrajectoryNode::Data> constant_data;
std::vector<std::shared_ptr<const Submap>> 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<void(int /* trajectory ID */, common::Time,
transform::Rigid3d /* local pose estimate */,
sensor::RangeData /* in local frame */,
std::unique_ptr<const mapping::NodeId>)>;
std::unique_ptr<const InsertionResult>)>;
TrajectoryBuilderInterface() {}
virtual ~TrajectoryBuilderInterface() {}

10
cartographer_grpc/client_server_test.cc
View File

@ -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<const cartographer::mapping::NodeId>) {
[this](
int, cartographer::common::Time,
cartographer::transform::Rigid3d local_pose,
cartographer::sensor::RangeData,
std::unique_ptr<const cartographer::mapping::
TrajectoryBuilderInterface::InsertionResult>) {
std::unique_lock<std::mutex> lock(local_slam_result_mutex_);
local_slam_result_poses_.push_back(local_pose);
lock.unlock();

8
cartographer_grpc/handlers/receive_local_slam_results_handler.h
View File

@ -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;
}

22
cartographer_grpc/map_builder_server.cc
View File

@ -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<const cartographer::mapping::NodeId> node_id) {
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(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<const cartographer::mapping::NodeId> node_id) {
std::unique_ptr<const cartographer::mapping::TrajectoryBuilderInterface::
InsertionResult>
insertion_result) {
auto shared_range_data =
std::make_shared<cartographer::sensor::RangeData>(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>(
LocalSlamResult{trajectory_id, time, local_pose, shared_range_data,
std::move(copy_of_node_id)}));
std::move(copy_of_insertion_result)}));
}
}

8
cartographer_grpc/map_builder_server.h
View File

@ -35,7 +35,9 @@ class MapBuilderServer {
cartographer::common::Time time;
cartographer::transform::Rigid3d local_pose;
std::shared_ptr<const cartographer::sensor::RangeData> range_data;
std::unique_ptr<const cartographer::mapping::NodeId> node_id;
std::unique_ptr<const cartographer::mapping::TrajectoryBuilderInterface::
InsertionResult>
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<const cartographer::mapping::NodeId> node_id);
std::unique_ptr<const cartographer::mapping::TrajectoryBuilderInterface::
InsertionResult>
insertion_result);
SubscriptionId SubscribeLocalSlamResults(
int trajectory_id, LocalSlamSubscriptionCallback callback);
void UnsubscribeLocalSlamResults(const SubscriptionId& subscription_id);

14
cartographer_grpc/mapping/trajectory_builder_stub.cc
View File

@ -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>(
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>(
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();
}

6
cartographer_grpc/proto/map_builder_service.proto
View File

@ -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 {