Autoware实战：Open Planner

Yosion_

已于 2024-11-19 16:57:15 修改

阅读量1.4k

点赞数 25

文章标签：自动驾驶人工智能学习

于 2024-11-19 14:48:25 首次发布

本文链接：https://blog.csdn.net/m0_65512360/article/details/143436798

版权

前言

在网络上看到很多关于Open_Planner优质的文章，但大家似乎都更乐意从原理性出发去讲解，关于其实战好像并不多，所以喜欢直接上手或是在自己小车上部署的同学可能资料就没那么丰富。在实战中过程并没有那么理想化，会碰到各种各样的问题，也会发现代码中一些欠缺的部分。写这篇文章的目的就是从实战出发，给想要上手但是不清楚怎么做的同学一点参考以及一些报错解决的分享，也是自己积累回顾的一些总结，希望能帮助到大家！

仿真测试

仿真这部分我用的是autoware.ai自带的仿真环境autoware.world以及阿克曼小车测试算法的可行性，需要注意的一点是，最好先启动runtime manager再启动仿真环境，不然可能会有一些时间戳的问题。前期关于Setup、Map、Sensing的启动相信大家都比较熟悉我就不细说了，主要就是多了一个Vector Map的加载，附图如下：（想要仿真点云地图和矢量地图的自取哈，搞了一个简单的，用来学习没啥问题，后续也打算出几篇关于点云地图构建以及矢量地图绘制的文章：sim_world地图文件提取码: kp38）

打开RVIZ看下效果，应该是可以将点云地图、矢量地图的roadedge、车道线显示出来，车辆行驶的轨迹lane需要加载op_global_planner才会显示。

在Computing界面如下，由于我当下主要是为了测试全局规划的作用，所以网上大部分说的欧式聚类和卡尔曼滤波跟踪就没有加载，这不影响全局的规控效果:

在op_global_planner部分需要选中Replanning，Smoothing，Rviz Goals，其他模块基本默认就可以用。

然后在Rviz中给一个目标点，就可以规划好全局路径并跟踪了，并且这是全局路径是沿着矢量地图中画的lane走的，具体话题名为/vector_map_center_lines_rviz，大家初始阶段在Rviz中可能需要添加一下，我比较习惯第一次就将Open Planner所有Rviz可视化的话题就保存起来，后续可视化效果会好一点，对自己理解里面的一些东西也有帮助。

全局规划的效果如下：

到达第一个目标点后，再给一个目标点重新规划跟踪，有些同学可能Replanning有些问题，到达第一个目标点后车就不动了，不走第二个目标点，具体处理方法见下文ERROR解决（2）：

上左图用的op_local_planner做的局部规划，右图用的是astar_avoid+velocity_set，当然如果用后者需要加载costmap进行避障，因为Hybrid A*是图搜索算法，但是正常的停障靠雷达点云信息和velocity_set的配合就可以实现。

ERROR解决

（1）在启动op_trajectory_generator时发现rviz中没有生成open planner局部规划的采样轨迹，并且会出现如下报错:

Client [/rviz_1730432156284902839] wants topic /local_trajectories to have
 datatype/md5sum [visualization_msgs/MarkerArray/d155b9ce5188fbaf89745847fd5882d7],
 but our version has [autoware_msgs/LaneArray/23abb100bdfa4ee58530bb628c974c2a].
 Dropping connection

当时查了挺多资料都没得出好的答案，最后发现问题其实很简单。启动rviz，在Display中Planning模块下有个OP Planner包，里面的Local Rollouts加载的就是/local_trajectories这个话题。

但是去看op_trajectory_generator的源码发现，他想在rviz中显示的其实是/local_trajectories_gen_rviz这个话题，/local_trajectories这个话题不是用于可视化的。

所以问题很简单，替换话题名即可，将/local_trajectories换成/local_trajectories_gen_rviz，再次运行op_trajectory_generator终端就不会有这个报错了，且局部采样的轨迹在rviz中也可以可视化。

同理在启动op_behavior_selector时出现如下问题也是RVIZ的显示问题，可以检查下默认RVIZ里的话题名是否可行，我发现这个默认的RVIZ配置是有一些问题的，可以将有问题的默认话题删除，自己重新Add话题进行可视化。

[ERROR] [1730453360.727729269, 5559.357000000]: Client [/rviz_1730451385023753443]
wants topic /behavior_state to have datatype/md5sum [visualization_msgs/Marker
/4048c9de2a16f4ae8e0538085ebf1b97], but our version has
 [visualization_msgs/MarkerArray/d155b9ce5188fbaf89745847fd5882d7]. Dropping connection.

（2）实车调试时Replanning无法使用，多个目标点时，在到达第一个目标点附近就停止，无法继续走到下一个目标点。

原因：在行驶的过程中，车辆的状态为Forward，/final_waypoints就有速度信息，Pure Pursuit节点接收/final_waypoints就能有速度的跟踪。但是行驶到第一个目标点附近时，车辆的状态切换成End，此时发现/final_waypoints中只有位姿信息，速度信息全为0，故车辆停止不动。

首先修改op_global_planner下op_global_planner_core.cpp中的MainLoop()，将if(m_params.bEnableReplanning)修改成if(m_params.bEnableReplanning && m_iCurrentGoalIndex<m_GoalsPos.size()-1)，if中间执行的代码不变。

if(m_GoalsPos.size() > 0)
    {
      if(m_GeneratedTotalPaths.size() > 0 && m_GeneratedTotalPaths.at(0).size() > 3)
      {
        // if(m_params.bEnableReplanning) 将这句if判断替换成下面的if判断
        if(m_params.bEnableReplanning && m_iCurrentGoalIndex<m_GoalsPos.size()-1)
        {
          PlannerHNS::RelativeInfo info;
          bool ret = PlannerHNS::PlanningHelpers::GetRelativeInfoRange(m_GeneratedTotalPaths, m_CurrentPose, 0.75, info);
          if(ret == true && info.iGlobalPath >= 0 &&  info.iGlobalPath < m_GeneratedTotalPaths.size() && info.iFront > 0 && info.iFront < m_GeneratedTotalPaths.at(info.iGlobalPath).size())
          {
            double remaining_distance =    m_GeneratedTotalPaths.at(info.iGlobalPath).at(m_GeneratedTotalPaths.at(info.iGlobalPath).size()-1).cost - (m_GeneratedTotalPaths.at(info.iGlobalPath).at(info.iFront).cost + info.to_front_distance);
            if(remaining_distance <= REPLANNING_DISTANCE)
            {
              bMakeNewPlan = true;
              if(m_GoalsPos.size() > 0)
                m_iCurrentGoalIndex = (m_iCurrentGoalIndex + 1) % m_GoalsPos.size();
              std::cout << "Current Goal Index = " << m_iCurrentGoalIndex << std::endl << std::endl;
            }
          }
        }
      }

然后修改common/op_planner/src下的BehaviorStateMachine.cpp，找到函数MissionAccomplishedStateII::GetNextState()，注意是StateII，因为这个cpp文件有个函数是MissionAccomplishedState别搞错了。修改return如下：

BehaviorStateMachine* MissionAccomplishedStateII::GetNextState()
{
  // return FindBehaviorState(this->m_Behavior);
  PreCalculatedConditions* pCParams = GetCalcParams();

  if(pCParams->currentGoalID == -1)
      return FindBehaviorState(this->m_Behavior);

  else
  {
      pCParams->prevGoalID = pCParams->currentGoalID;
      return FindBehaviorState(FORWARD_STATE);
  }
}

参考blog（非常感谢大佬分享的代码）：https://blog.csdn.net/qq_38861347/article/details/134973913?ops_request_misc=&request_id=&biz_id=102&utm_term=openPlanner&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduweb~default-5-134973913.142^v100^pc_search_result_base4&spm=1018.2226.3001.4187