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

Change MapBuilderServer::LocalSlamSubscriptionCallback (#776) 

PAIR=gaschler
master
Christoph Schütte 2017-12-20 09:46:54 +01:00 committed by Wally B. Feed
parent 383b988548
commit e0faf7094e
3 changed files with 27 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
cartographer_grpc/handlers/receive_local_slam_results_handler.h
View File

@ -37,14 +37,9 @@ class ReceiveLocalSlamResultsHandler
GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
->SubscribeLocalSlamResults(
request.trajectory_id(),
[writer](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::NodeId>
node_id) {
writer(GenerateResponse(trajectory_id, time, local_pose,
range_data, std::move(node_id)));
[writer](std::unique_ptr<MapBuilderServer::LocalSlamResult>
local_slam_result) {
writer(GenerateResponse(std::move(local_slam_result)));
});
subscription_id_ =
@ -54,23 +49,23 @@ class ReceiveLocalSlamResultsHandler
static std::unique_ptr<proto::ReceiveLocalSlamResultsResponse>
GenerateResponse(
int trajectory_id, cartographer::common::Time time,
const 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<MapBuilderServer::LocalSlamResult> local_slam_result) {
auto response = cartographer::common::make_unique<
proto::ReceiveLocalSlamResultsResponse>();
response->set_trajectory_id(trajectory_id);
response->set_timestamp(cartographer::common::ToUniversal(time));
response->set_trajectory_id(local_slam_result->trajectory_id);
response->set_timestamp(
cartographer::common::ToUniversal(local_slam_result->time));
*response->mutable_local_pose() =
cartographer::transform::ToProto(local_pose);
if (range_data) {
cartographer::transform::ToProto(local_slam_result->local_pose);
if (local_slam_result->range_data) {
*response->mutable_range_data() =
cartographer::sensor::ToProto(*range_data);
cartographer::sensor::ToProto(*local_slam_result->range_data);
}
if (node_id) {
response->mutable_node_id()->set_trajectory_id(node_id->trajectory_id);
response->mutable_node_id()->set_node_index(node_id->node_index);
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);
}
return response;
}

5
cartographer_grpc/map_builder_server.cc
View File

@ -171,8 +171,9 @@ void MapBuilderServer::OnLocalSlamResult(
const cartographer::mapping::NodeId>(*node_id)
: nullptr;
LocalSlamSubscriptionCallback callback = entry.second;
callback(trajectory_id, time, local_pose, shared_range_data,
std::move(copy_of_node_id));
callback(cartographer::common::make_unique<LocalSlamResult>(
LocalSlamResult{trajectory_id, time, local_pose, shared_range_data,
std::move(copy_of_node_id)}));
}
}

15
cartographer_grpc/map_builder_server.h
View File

@ -30,12 +30,15 @@ namespace cartographer_grpc {
class MapBuilderServer {
public:
using LocalSlamSubscriptionCallback = std::function<void(
int /* trajectory ID */, cartographer::common::Time,
cartographer::transform::Rigid3d /* local pose estimate */,
std::shared_ptr<
const cartographer::sensor::RangeData> /* in local frame */,
std::unique_ptr<const cartographer::mapping::NodeId>)>;
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::NodeId> node_id;
};
using LocalSlamSubscriptionCallback =
std::function<void(std::unique_ptr<LocalSlamResult>)>;
struct SensorData {
int trajectory_id;
std::unique_ptr<cartographer::sensor::Data> sensor_data;