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

Track googlecartographer/cartographer#352. (#388)

master
Holger Rapp 2017-06-21 15:19:58 +02:00 committed by GitHub
parent d0d7ebf173
commit 9244ada458
1 changed files with 7 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -106,7 +106,9 @@ bool MapBuilderBridge::HandleSubmapQuery(
cartographer_ros_msgs::SubmapQuery::Response& response) { cartographer_ros_msgs::SubmapQuery::Response& response) {
cartographer::mapping::proto::SubmapQuery::Response response_proto; cartographer::mapping::proto::SubmapQuery::Response response_proto;
const std::string error = map_builder_.SubmapToProto( const std::string error = map_builder_.SubmapToProto(
request.trajectory_id, request.submap_index, &response_proto); cartographer::mapping::SubmapId{request.trajectory_id,
request.submap_index},
&response_proto);
if (!error.empty()) { if (!error.empty()) {
LOG(ERROR) << error; LOG(ERROR) << error;
return false; return false;
@ -127,16 +129,13 @@ cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
cartographer_ros_msgs::SubmapList submap_list; cartographer_ros_msgs::SubmapList submap_list;
submap_list.header.stamp = ::ros::Time::now(); submap_list.header.stamp = ::ros::Time::now();
submap_list.header.frame_id = node_options_.map_frame; submap_list.header.frame_id = node_options_.map_frame;
for (int trajectory_id = 0; const auto all_submap_data =
trajectory_id < map_builder_.num_trajectory_builders(); map_builder_.sparse_pose_graph()->GetAllSubmapData();
for (size_t trajectory_id = 0; trajectory_id < all_submap_data.size();
++trajectory_id) { ++trajectory_id) {
auto* const trajectory_builder =
map_builder_.GetTrajectoryBuilder(trajectory_id);
const int num_submaps = trajectory_builder->num_submaps();
cartographer_ros_msgs::TrajectorySubmapList trajectory_submap_list; cartographer_ros_msgs::TrajectorySubmapList trajectory_submap_list;
for (int submap_index = 0; submap_index != num_submaps; ++submap_index) { for (const auto& submap_data : all_submap_data[trajectory_id]) {
cartographer_ros_msgs::SubmapEntry submap_entry; cartographer_ros_msgs::SubmapEntry submap_entry;
const auto submap_data = trajectory_builder->GetSubmapData(submap_index);
submap_entry.submap_version = submap_data.submap->num_range_data(); submap_entry.submap_version = submap_data.submap->num_range_data();
submap_entry.pose = ToGeometryMsgPose(submap_data.pose); submap_entry.pose = ToGeometryMsgPose(submap_data.pose);
trajectory_submap_list.submap.push_back(submap_entry); trajectory_submap_list.submap.push_back(submap_entry);