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

5
cartographer_grpc/map_builder_server.cc
View File

@ -171,8 +171,9 @@ void MapBuilderServer::OnLocalSlamResult(
const cartographer::mapping::NodeId>(*node_id) const cartographer::mapping::NodeId>(*node_id)
: nullptr; : nullptr;
LocalSlamSubscriptionCallback callback = entry.second; LocalSlamSubscriptionCallback callback = entry.second;
callback(trajectory_id, time, local_pose, shared_range_data, callback(cartographer::common::make_unique<LocalSlamResult>(
std::move(copy_of_node_id)); 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 { class MapBuilderServer {
public: public:
using LocalSlamSubscriptionCallback = std::function<void( struct LocalSlamResult {
int /* trajectory ID */, cartographer::common::Time, int trajectory_id;
cartographer::transform::Rigid3d /* local pose estimate */, cartographer::common::Time time;
std::shared_ptr< cartographer::transform::Rigid3d local_pose;
const cartographer::sensor::RangeData> /* in local frame */, std::shared_ptr<const cartographer::sensor::RangeData> range_data;
std::unique_ptr<const cartographer::mapping::NodeId>)>; std::unique_ptr<const cartographer::mapping::NodeId> node_id;
};
using LocalSlamSubscriptionCallback =
std::function<void(std::unique_ptr<LocalSlamResult>)>;
struct SensorData { struct SensorData {
int trajectory_id; int trajectory_id;
std::unique_ptr<cartographer::sensor::Data> sensor_data; std::unique_ptr<cartographer::sensor::Data> sensor_data;