1 | planner_->visualize(); |
TebOptimalPlanner::visualize
1 | void TebOptimalPlanner::visualize() |
最后部分是发布feedback消息 (single trajectory overload)，话题teb_feedback. 需要设置publish_feedback为true，默认为false
feedback message包括规划的轨迹，速度和时间信息(temporal information)，也包括 active obstacles
publishLocalPlanAndPoses很简单，需要注意的是1
2
3
4// 不明白z为什么这样赋值 ???
pose.pose.position.z = cfg_->hcp.visualize_with_time_as_z_axis_scale *
teb.getSumOfTimeDiffsUpToIdx(i);
// getSumOfTimeDiffsUpToIdx 就是从起点到当前位姿i的所有deltaT的总和
publishRobotFootprintModel也很简单，机器人在局部路径第一个点的时候，可视化轮廓，就是一些对visualization_msgs::Marker的操作。不过对不同的轮廓类型，调用的函数不同
publishFeedbackMessage
1 | void TebVisualization::publishFeedbackMessage(const TebOptimalPlanner& teb_planner, const ObstContainer& obstacles) |
publishObstacles
这里需要注意的是点障碍的可视化1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41visualization_msgs::Marker marker;
marker.header.frame_id = cfg_->map_frame;
marker.header.stamp = ros::Time::now();
marker.ns = "PointObstacles"; marker.id = 0;
marker.type = visualization_msgs::Marker::POINTS;
marker.action = visualization_msgs::Marker::ADD;
marker.lifetime = ros::Duration(2.0);
for (ObstContainer::const_iterator obst = obstacles.begin(); obst != obstacles.end(); ++obst)
{
boost::shared_ptr<PointObstacle> pobst = boost::dynamic_pointer_cast<PointObstacle>(*obst);
if (!pobst) continue;
if (cfg_->hcp.visualize_with_time_as_z_axis_scale < 0.001)
{
geometry_msgs::Point point;
point.x = pobst->x();
point.y = pobst->y();
point.z = 0;
marker.points.push_back(point);
}
else // Spatiotemporally point obstacles become a line
{
marker.type = visualization_msgs::Marker::LINE_LIST;
geometry_msgs::Point start;
start.x = pobst->x(); start.y = pobst->y();
start.z = 0;
marker.points.push_back(start);
geometry_msgs::Point end;
double t = 20;
Eigen::Vector2d pred;
pobst->predictCentroidConstantVelocity(t, pred);
end.x = pred[0];
end.y = pred[1];
end.z = cfg_->hcp.visualize_with_time_as_z_axis_scale * t;
marker.points.push_back(end);
}
}
// 省略 marker的着色
teb_marker_pub_.publish( marker );
HomotopyClassPlanner::visualize
visualization_的赋值在HomotopyClassPlanner::initialize 最后的 setVisualization(visual);1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32if (visualization_)
{
// Visualize graph
if (cfg_->hcp.visualize_hc_graph && graph_search_)
visualization_->publishGraph(graph_search_->graph_);
// Visualize active tebs as marker
visualization_->publishTebContainer(tebs_);
// Visualize best teb and feedback message if desired
TebOptimalPlannerConstPtr best_teb = bestTeb();
if (best_teb)
{
// 只发布best_teb的局部路径，类型是nav_msgs::Path 和 geometry_msgs::PoseArray
// 对应话题 local_plan 和 teb_poses
// 位姿点的z = cfg_->hcp.visualize_with_time_as_z_axis_scale *
// teb.getSumOfTimeDiffsUpToIdx(i)
visualization_->publishLocalPlanAndPoses(best_teb->teb() );
if (best_teb->teb().sizePoses() > 0) // 路径的位姿点个数
visualization_->publishRobotFootprintModel(best_teb->teb().Pose(0), *robot_model_);
if (cfg_->trajectory.publish_feedback) // feedback message
{
int best_idx = bestTebIdx();
if (best_idx>=0)
visualization_->publishFeedbackMessage(tebs_,
(unsigned int) best_idx, *obstacles_ );
}
}
}
else ROS_DEBUG("Ignoring HomotopyClassPlanner::visualize() call, since no visualization class was instantiated before.");
