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

Use trajectory ID as namespace for trajectory marker (#473) 

Also simplifies marker IDs since they must only be unique within a unique namespace.
master
Juraj Oršulić 2017-08-07 15:59:41 +02:00 committed by Damon Kohler
parent ab1ca30353
commit 7f616b4cc7
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -169,13 +169,13 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() {
visualization_msgs::MarkerArray trajectory_node_list; visualization_msgs::MarkerArray trajectory_node_list;
const auto all_trajectory_nodes = const auto all_trajectory_nodes =
map_builder_.sparse_pose_graph()->GetTrajectoryNodes(); map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
int marker_id = 0;
for (int trajectory_id = 0; for (int trajectory_id = 0;
trajectory_id < static_cast<int>(all_trajectory_nodes.size()); trajectory_id < static_cast<int>(all_trajectory_nodes.size());
++trajectory_id) { ++trajectory_id) {
const auto& single_trajectory_nodes = all_trajectory_nodes[trajectory_id]; const auto& single_trajectory_nodes = all_trajectory_nodes[trajectory_id];
visualization_msgs::Marker marker; visualization_msgs::Marker marker;
marker.id = marker_id++; marker.ns = "Trajectory " + std::to_string(trajectory_id);
marker.id = 0;
marker.type = visualization_msgs::Marker::LINE_STRIP; marker.type = visualization_msgs::Marker::LINE_STRIP;
marker.header.stamp = ::ros::Time::now(); marker.header.stamp = ::ros::Time::now();
marker.header.frame_id = node_options_.map_frame; marker.header.frame_id = node_options_.map_frame;
@ -198,7 +198,7 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() {
// trajectory into multiple markers. // trajectory into multiple markers.
if (marker.points.size() == 16384) { if (marker.points.size() == 16384) {
trajectory_node_list.markers.push_back(marker); trajectory_node_list.markers.push_back(marker);
marker.id = marker_id++; ++marker.id;
marker.points.clear(); marker.points.clear();
// Push back the last point, so the two markers appear connected. // Push back the last point, so the two markers appear connected.
marker.points.push_back(node_point); marker.points.push_back(node_point);