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

Add constraint-dependent trajectory visualization. (#756)

master
Kevin Daun 2018-03-26 16:45:58 +02:00 committed by gaschler
parent 913142f9c7
commit 7d09140ee4
2 changed files with 75 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

74
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -88,10 +88,8 @@ visualization_msgs::Marker CreateLandmarkMarker(int landmark_index,
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();
}
@ -247,10 +245,58 @@ MapBuilderBridge::GetTrajectoryStates() {
visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() {
visualization_msgs::MarkerArray trajectory_node_list;
const auto node_poses = map_builder_->pose_graph()->GetTrajectoryNodePoses();
// Find the last node indices for each trajectory that have either
// inter-submap or inter-trajectory constraints.
std::map<int, int /* node_index */>
trajectory_to_last_inter_submap_constrained_node;
std::map<int, int /* node_index */>
trajectory_to_last_inter_trajectory_constrained_node;
for (const int trajectory_id : node_poses.trajectory_ids()) {
trajectory_to_last_inter_submap_constrained_node[trajectory_id] = 0;
trajectory_to_last_inter_trajectory_constrained_node[trajectory_id] = 0;
}
const auto constraints = map_builder_->pose_graph()->constraints();
for (const auto& constraint : constraints) {
if (constraint.tag ==
cartographer::mapping::PoseGraph::Constraint::INTER_SUBMAP) {
if (constraint.node_id.trajectory_id ==
constraint.submap_id.trajectory_id) {
trajectory_to_last_inter_submap_constrained_node[constraint.node_id
.trajectory_id] =
std::max(trajectory_to_last_inter_submap_constrained_node.at(
constraint.node_id.trajectory_id),
constraint.node_id.node_index);
} else {
trajectory_to_last_inter_trajectory_constrained_node
[constraint.node_id.trajectory_id] =
std::max(trajectory_to_last_inter_submap_constrained_node.at(
constraint.node_id.trajectory_id),
constraint.node_id.node_index);
}
}
}
for (const int trajectory_id : node_poses.trajectory_ids()) {
visualization_msgs::Marker marker =
CreateTrajectoryMarker(trajectory_id, node_options_.map_frame);
int last_inter_submap_constrained_node = std::max(
node_poses.trajectory(trajectory_id).begin()->id.node_index,
trajectory_to_last_inter_submap_constrained_node.at(trajectory_id));
int last_inter_trajectory_constrained_node = std::max(
node_poses.trajectory(trajectory_id).begin()->id.node_index,
trajectory_to_last_inter_trajectory_constrained_node.at(trajectory_id));
last_inter_submap_constrained_node =
std::max(last_inter_submap_constrained_node,
last_inter_trajectory_constrained_node);
if (map_builder_->pose_graph()->IsTrajectoryFrozen(trajectory_id)) {
last_inter_submap_constrained_node =
(--node_poses.trajectory(trajectory_id).end())->id.node_index;
last_inter_trajectory_constrained_node =
last_inter_submap_constrained_node;
}
marker.color.a = 1.0;
for (const auto& node_id_data : node_poses.trajectory(trajectory_id)) {
if (!node_id_data.data.has_constant_data) {
PushAndResetLineMarker(&marker, &trajectory_node_list.markers);
@ -259,6 +305,18 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() {
const ::geometry_msgs::Point node_point =
ToGeometryMsgPoint(node_id_data.data.global_pose.translation());
marker.points.push_back(node_point);
if (node_id_data.id.node_index ==
last_inter_trajectory_constrained_node) {
PushAndResetLineMarker(&marker, &trajectory_node_list.markers);
marker.points.push_back(node_point);
marker.color.a = 0.5;
}
if (node_id_data.id.node_index == last_inter_submap_constrained_node) {
PushAndResetLineMarker(&marker, &trajectory_node_list.markers);
marker.points.push_back(node_point);
marker.color.a = 0.25;
}
// Work around the 16384 point limit in RViz by splitting the
// trajectory into multiple markers.
if (marker.points.size() == 16384) {
@ -268,6 +326,18 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() {
}
}
PushAndResetLineMarker(&marker, &trajectory_node_list.markers);
size_t current_last_marker_id = static_cast<size_t>(marker.id - 1);
if (trajectory_to_highest_marker_id_.count(trajectory_id) == 0) {
trajectory_to_highest_marker_id_[trajectory_id] = current_last_marker_id;
} else {
marker.action = visualization_msgs::Marker::DELETE;
while (static_cast<size_t>(marker.id) <=
trajectory_to_highest_marker_id_[trajectory_id]) {
trajectory_node_list.markers.push_back(marker);
++marker.id;
}
trajectory_to_highest_marker_id_[trajectory_id] = current_last_marker_id;
}
}
return trajectory_node_list;
}

1
cartographer_ros/cartographer_ros/map_builder_bridge.h
View File

@ -107,6 +107,7 @@ class MapBuilderBridge {
// These are keyed with 'trajectory_id'.
std::unordered_map<int, TrajectoryOptions> trajectory_options_;
std::unordered_map<int, std::unique_ptr<SensorBridge>> sensor_bridges_;
std::unordered_map<int, size_t> trajectory_to_highest_marker_id_;
};
} // namespace cartographer_ros