【Autoware】astar路径规划分析

Autoware astar路径规划分析

Autoware使用astar算法进行基础的路径规划。

接上篇Runtime_Manager的分析，我们可以在Computing Tab页的配置文件computing.yaml中找到astar的相关应用。

找到相关配置

- subs :
  - name : Mission Planning
    desc : Mission Planning desc sample
    subs :
    ...
    - name : freespace_planner
      desc : freespace_planner desc sample
      subs :
      - name : astar_navi
        desc : astar_navi desc sample
        cmd  : roslaunch freespace_planner astar_navi.launch
        param: astar_navi_param
      ...

- name : waypoint_planner
      desc : waypoint_planner desc sample
      subs :
      - name : astar_avoid
        desc : astar_avoid desc sample
        cmd  : roslaunch waypoint_planner astar_avoid.launch
        param: astar_avoid_param
      ...

主要是这两个应用，后面还分别有一个astar_avoid_param和astar_navi_param参数。

---

astar_avoid

流程图

先看一下流程，这部分参考自文章Autoware调试心得-astar_avoid的避障实现
成功避障状态转换：

如果没有避障成功，大概率就是卡在某一个环节，如下图所示：
没有避障：

两张图片对比可以看到astar_avoid的流程和状态转换过程。

---

接下来分析下相关的文件：

在路径/Autoware/src/autoware/core_planning/waypoint_planner/src/astar_avoid下可以找到相关文件。包括

astar_avoid.cpp  
astar_avoid_node.cpp

运行上面cmd中提示的命令：

roslaunch waypoint_planner astar_avoid.launch

并运行rqt命令，可以看到名为astar_avoid的node被启动，但是terminal中会提示Waiting for subscribing topics...，因为现在没有做其他的配置，没有数据输入给这个topic。

astar_avoid.launch配置文件分析

配置文件路径：
/home/autoware/Autoware/src/autoware/core_planning/waypoint_planner/launch/astar_avoid.launch

可以看到astar_avoid有两种模式，一种是Relay模式，一种是Avoidance模式。
从这行代码

<node pkg="waypoint_planner" type="astar_avoid" name="astar_avoid" output="screen">

容易发现，astar_avoid配置文件调用的是waypoint_planner包下面的astar_avoid=>astar_avoid.cpp

<launch>

  <!-- Relay behavior configurations -->
  <arg name="safety_waypoints_size" default="100" />
  <arg name="update_rate" default="10" />

  <!-- Avoidance behavior configurations -->
  <arg name="costmap_topic" default="semantics/costmap_generator/occupancy_grid" />
  ...

  <!-- A* search configurations -->
  <arg name="use_back" default="false" />
  ...

  <node pkg="waypoint_planner" type="astar_avoid" name="astar_avoid" output="screen">
    ...
  </node>

</launch>

---

astar_avoid 代码分析

头文件

头文件中定义了车行驶的几种状态。私有成员包括句柄，publisher，subscriber等。私有函数，分为callback和功能函数两类。车辆的四种状态如下：

typedef enum STATE
  {
    INITIALIZING = -1,
    RELAYING = 0,
    STOPPING = 1,
    PLANNING = 2,
    AVOIDING = 3
  } State;

cpp

astar_navi

在路径/Autoware/src/autoware/core_planning/freespace_planner/src/astar_navi下可以找到相关文件。包括

astar_navi.cpp  
astar_navi_node.cpp

运行上面cmd中提示的命令：

roslaunch freespace_planner astar_navi.launch

并运行rqt命令，可以看到名为astar_navi和waypoint_marker_publisher的两个node被启动。

上述三个node在rqt中的关系如下图所示（无其他服务启动）。

astar_avoid.launch配置文件分析

配置文件路径：
/Autoware/src/autoware/core_planning/freespace_planner/launch/astar_navi.launch

配置文件的内容如下：

<launch>

  <!-- Navi configurations -->
  <arg name="costmap_topic" default="semantics/costmap_generator/occupancy_grid" />
  ...

  <!-- A* search configurations -->
  <arg name="use_back" default="true" />
  ...

  <node pkg="freespace_planner" type="astar_navi" name="astar_navi" output="screen">
    ...
  </node>

  <!-- Visualization node-->
  <node pkg="waypoint_maker" type="waypoint_marker_publisher" name="waypoint_marker_publisher" />

</launch>