沉默杀手HomeArchivesTagsCategories
(十七) 可视化
|路径规划TEB算法|
Word count: 697|Reading time: 3 min
1
2
3
4
5
6
planner_->visualize();
visualization_->publishObstacles(obstacles_);
// 很简单，显示 via_point, 坐标来源是 via_points_
visualization_->publishViaPoints(via_points_);
// 很简单，实际调用的是 base_local_planner::publishPlan
visualization_->publishGlobalPlan(global_plan_);

TebOptimalPlanner::visualize

1
2
3
4
5
6
7
8
9
10
11
12
void TebOptimalPlanner::visualize()
{
  if (!visualization_)
    return;
 
  visualization_->publishLocalPlanAndPoses(teb_);
  if (teb_.sizePoses() > 0)
      visualization_->publishRobotFootprintModel(teb_.Pose(0), *robot_model_);
  
  if (cfg_->trajectory.publish_feedback)
      visualization_->publishFeedbackMessage(*this, *obstacles_);
}

最后部分是发布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
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
void TebVisualization::publishFeedbackMessage(const TebOptimalPlanner& teb_planner, const ObstContainer& obstacles)
{
  FeedbackMsg msg;
  msg.header.stamp = ros::Time::now();
  msg.header.frame_id = cfg_->map_frame;
  msg.selected_trajectory_idx = 0;
  msg.trajectories.resize(1);
  msg.trajectories.front().header = msg.header;

  teb_planner.getFullTrajectory(msg.trajectories.front().trajectory);
  // add obstacles
  msg.obstacles_msg.obstacles.resize(obstacles.size());
  for (std::size_t i=0; i<obstacles.size(); ++i)
  {
    msg.obstacles_msg.header = msg.header;
    // copy polygon
    msg.obstacles_msg.obstacles[i].header = msg.header;
    obstacles[i]->toPolygonMsg(msg.obstacles_msg.obstacles[i].polygon);
    // copy id
    msg.obstacles_msg.obstacles[i].id = i; // TODO: we do not have any id stored yet

    // orientation
    //msg.obstacles_msg.obstacles[i].orientation =; // TODO
    // copy velocities
    obstacles[i]->toTwistWithCovarianceMsg(msg.obstacles_msg.obstacles[i].velocities);
  }
  feedback_pub_.publish(msg);
}

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
41
visualization_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
32
if (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.");