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

Visualize gaps in trajectories due to trimming. (#500)

master
Jihoon Lee 2017-10-17 14:16:26 +02:00 committed by Wolfgang Hess
parent 6b22cfaf8d
commit 4c54a545a1
1 changed files with 30 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -36,6 +36,30 @@ constexpr double kConstraintMarkerScale = 0.025;
return result; return result;
} }
visualization_msgs::Marker CreateTrajectoryMarker(const int trajectory_id,
const std::string& frame_id) {
visualization_msgs::Marker marker;
marker.ns = "Trajectory " + std::to_string(trajectory_id);
marker.id = 0;
marker.type = visualization_msgs::Marker::LINE_STRIP;
marker.header.stamp = ::ros::Time::now();
marker.header.frame_id = frame_id;
marker.color = ToMessage(cartographer::io::GetColor(trajectory_id));
marker.scale.x = kTrajectoryLineStripMarkerScale;
marker.pose.orientation.w = 1.;
marker.pose.position.z = 0.05;
return marker;
}
void PushAndResetLineMarker(visualization_msgs::Marker* marker,
std::vector<visualization_msgs::Marker>* markers) {
if (marker->points.size() > 1) {
markers->push_back(*marker);
++marker->id;
}
marker->points.clear();
}
} // namespace } // namespace
MapBuilderBridge::MapBuilderBridge(const NodeOptions& node_options, MapBuilderBridge::MapBuilderBridge(const NodeOptions& node_options,
@ -184,18 +208,12 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() {
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.ns = "Trajectory " + std::to_string(trajectory_id); CreateTrajectoryMarker(trajectory_id, node_options_.map_frame);
marker.id = 0;
marker.type = visualization_msgs::Marker::LINE_STRIP;
marker.header.stamp = ::ros::Time::now();
marker.header.frame_id = node_options_.map_frame;
marker.color = ToMessage(cartographer::io::GetColor(trajectory_id));
marker.scale.x = kTrajectoryLineStripMarkerScale;
marker.pose.orientation.w = 1.0;
marker.pose.position.z = 0.05;
for (const auto& node : single_trajectory_nodes) { for (const auto& node : single_trajectory_nodes) {
if (node.trimmed()) { if (node.trimmed()) {
PushAndResetLineMarker(&marker, &trajectory_node_list.markers);
continue; continue;
} }
const ::geometry_msgs::Point node_point = const ::geometry_msgs::Point node_point =
@ -204,14 +222,12 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() {
// Work around the 16384 point limit in RViz by splitting the // Work around the 16384 point limit in RViz by splitting the
// 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); PushAndResetLineMarker(&marker, &trajectory_node_list.markers);
++marker.id;
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);
} }
} }
trajectory_node_list.markers.push_back(marker); PushAndResetLineMarker(&marker, &trajectory_node_list.markers);
} }
return trajectory_node_list; return trajectory_node_list;
} }