Autoware代码op_motion_predictor解读

（注：本人的第一篇文章，能力有限，写的不好，请见谅！）

1. Autoware中的op_motion_predictor位于core_planning文件下的op_local-planner，大体思路文为: 加载全局地图环境(predict_traj.MainLoop()；)，轨迹预测部分主要在callbackGetTrackedObjects(const autoware_msgs::DetectedObjectArrayConstPtr& msg)；中。

void MotionPrediction::callbackGetTrackedObjects(const autoware_msgs::DetectedObjectArrayConstPtr& msg)//跟踪对象
{
  UtilityHNS::UtilityH::GetTickCount(m_SensingTimer);//计算程序运行时间
  m_TrackedObjects.clear();
  bTrackedObjects = true;

  PlannerHNS::DetectedObject obj;

  for(unsigned int i = 0 ; i <msg->objects.size(); i++)
  {
    if(msg->objects.at(i).id > 0)
    {
      PlannerHNS::ROSHelpers::ConvertFromAutowareDetectedObjectToOpenPlannerDetectedObject(msg->objects.at(i), obj);
      m_TrackedObjects.push_back(obj);//存放障碍物
    }
//    else
//    {
//      std::cout << " Ego Car avoid detecting itself from motion prediction node! ID: " << msg->objects.at(i).id << std::endl;
//    }

  }

  if(bMap)
  {
    if(m_PredictBeh.m_bStepByStep && m_bGoNextStep)
    {
      m_bGoNextStep = false;
      m_PredictBeh.DoOneStep(m_TrackedObjects, m_CurrentPos, m_PlanningParams.minSpeed, m_CarInfo.max_deceleration,  m_Map);
    }
    else if(!m_PredictBeh.m_bStepByStep)
    {
      m_PredictBeh.DoOneStep(m_TrackedObjects, m_CurrentPos, m_PlanningParams.minSpeed, m_CarInfo.max_deceleration,  m_Map);
    }


    m_PredictedResultsResults.objects.clear();
    autoware_msgs::DetectedObject pred_obj;
    for(unsigned int i = 0 ; i <m_PredictBeh.m_ParticleInfo_II.size(); i++)
    {
      PlannerHNS::ROSHelpers::ConvertFromOpenPlannerDetectedObjectToAutowareDetectedObject(m_PredictBeh.m_ParticleInfo_II.at(i)->obj, false, pred_obj);
      if(m_PredictBeh.m_ParticleInfo_II.at(i)->best_beh_track)
        pred_obj.behavior_state = m_PredictBeh.m_ParticleInfo_II.at(i)->best_beh_track->best_beh;
      m_PredictedResultsResults.objects.push_back(pred_obj);
    }

    if(m_bEnableCurbObstacles)
    {
      curr_curbs_obstacles.clear();
      GenerateCurbsObstacles(curr_curbs_obstacles);
      //std::cout << "Curbs No: " << curr_curbs_obstacles.size() << endl;
      for(unsigned int i = 0 ; i <curr_curbs_obstacles.size(); i++)
      {
        PlannerHNS::ROSHelpers::ConvertFromOpenPlannerDetectedObjectToAutowareDetectedObject(curr_curbs_obstacles.at(i), false, pred_obj);
        m_PredictedResultsResults.objects.push_back(pred_obj);
      }
    }

    m_PredictedResultsResults.header.stamp = ros::Time().now();
    pub_predicted_objects_trajectories.publish(m_PredictedResultsResults);
  }
}

ConvertFromAutowareDetectedObjectToOpenPlannerDetectedObject(msg->objects.at(i), obj)函数作用:障碍物属性信息格式的转换。

m_PredictBeh.DoOneStep(m_TrackedObjects, m_CurrentPos, m_PlanningParams.minSpeed, m_CarInfo.max_deceleration, m_Map)函数作用：基于地图和障碍物位置，生成障碍物的预测轨迹。

2. DoOneStep()函数

void BehaviorPrediction::DoOneStep(const std::vector<DetectedObject>& obj_list, const WayPoint& currPose, const double& minSpeed, const double& maxDeceleration, RoadNetwork& map)
{
  if(!m_bUseFixedPrediction && maxDeceleration !=0)
    m_PredictionDistance = -pow(currPose.v, 2)/(maxDeceleration);

  ExtractTrajectoriesFromMap(obj_list, map, m_ParticleInfo_II);//提取轨迹/m_TrackedObjects
  CalculateCollisionTimes(minSpeed);

  if(m_bParticleFilter)
  {
    ParticleFilterSteps(m_ParticleInfo_II);//微粒过滤步骤
  }
}

2.1 ExtractTrajectoriesFromMap(obj_list, map, m_ParticleInfo_II);

void BehaviorPrediction::ExtractTrajectoriesFromMap(const std::vector<DetectedObject>& curr_obj_list,RoadNetwork& map, std::vector<ObjParticles*>& old_obj_list)
{
  PlannerH planner;
  m_temp_list_ii.clear();//存放当前帧的障碍物列表

  std::vector<ObjParticles*> delete_me_list = old_obj_list;//m_ParticleInfo_II，起始为空

  for(unsigned int i=0; i < curr_obj_list.size(); i++)
  {
    bool bMatch = false;
    for(unsigned int ip=0; ip < old_obj_list.size(); ip++)//遍历旧的障碍物列表，是否找到与当前新障碍物对应的旧障碍物
    {
      if(old_obj_list.at(ip)->obj.id == curr_obj_list.at(i).id)//如果有
      {
        bool bFound = false;
        for(unsigned int k=0; k < m_temp_list_ii.size(); k++)//遍历当前障碍物表m_temp_list_ii，是否存在障碍物与旧障碍物相同
        {
          if(m_temp_list_ii.at(k) == old_obj_list.at(ip))//若有，不加入m_temp_list_ii
          {
            bFound = true;
            break;
          }
        }

        if(!bFound)//若m_temp_list_ii没有找到对应的障碍物，把new_obj加入m_temp_list_ii
        {
          old_obj_list.at(ip)->obj = curr_obj_list.at(i);
          m_temp_list_ii.push_back(old_obj_list.at(ip));
        }

        DeleteFromList(delete_me_list, old_obj_list.at(ip));

        old_obj_list.erase(old_obj_list.begin()+ip);
        bMatch = true;
        break;
      }
    }

    if(!bMatch)//如果old_obj_list.at(ip)->obj.id ！=curr_obj_list.at(i).id，curr_obj_list.at(i)加入 m_temp_list_ii
    {
      ObjParticles* pNewObj = new  ObjParticles();
      pNewObj->obj = curr_obj_list.at(i);
      m_temp_list_ii.push_back(pNewObj);
    }
  }

  DeleteTheRest(delete_me_list);
  old_obj_list.clear();
  old_obj_list = m_temp_list_ii;

  //m_PredictedObjects.clear();遍历每一个障碍物生成多条预测轨迹
  for(unsigned int ip=0; ip < old_obj_list.size(); ip++)
  {
    PredictCurrentTrajectory(map, old_obj_list.at(ip));
    //m_PredictedObjects.push_back(old_obj_list.at(ip)->obj);
    old_obj_list.at(ip)->MatchTrajectories();
  }

}

2.2 planner.PredictTrajectoriesUsingDP();

double PlannerH::PredictTrajectoriesUsingDP(const WayPoint& startPose, std::vector<WayPoint*> closestWPs, const double& maxPlanningDistance, std::vector<std::vector<WayPoint> >& paths, const bool& bFindBranches , const bool bDirectionBased, const bool pathDensity)
{
  vector<vector<WayPoint> > tempCurrentForwardPathss;
  vector<WayPoint*> all_cell_to_delete;
  vector<int> globalPath;

  vector<WayPoint*> pLaneCells;
  vector<int> unique_lanes;
  std::vector<WayPoint> path;
  //遍历当前障碍物的每一个可行最近点
  for(unsigned int j = 0 ; j < closestWPs.size(); j++)
  {
    pLaneCells.clear();
    //从最近点开始用dp开始搜索，遍历获得几条路径
    int nPaths =  PlanningHelpers::PredictiveIgnorIdsDP(closestWPs.at(j), maxPlanningDistance, all_cell_to_delete, pLaneCells, unique_lanes);
    for(unsigned int i = 0; i< pLaneCells.size(); i++)
    {
      path.clear();
      //回溯路经给path
      PlanningHelpers::TraversePathTreeBackwards(pLaneCells.at(i), closestWPs.at(j), globalPath, path, tempCurrentForwardPathss);
     //遍历获得的路径上每个点，找到对应的 unique_lanes，没找到，加入
      for(unsigned int k = 0; k< path.size(); k++)
      {
        bool bFoundLaneID = false;
        //unique_lanes起始为空，判断unique_lanes中是否存在path.at(k).laneId，如存在，不加入unique_lanes中
        for(unsigned int l_id = 0 ; l_id < unique_lanes.size(); l_id++)
        {
          if(unique_lanes.at(k).laneId == unique_lanes.at(l_id))
          {
            bFoundLaneID = true;
            break;
          }
        }

        if(!bFoundLaneID)
          unique_lanes.push_back(path.at(k).laneId);//存放当前path中所有的不同path.at(k).laneId
      }

      if(path.size()>0)
      {//把障碍物位置加入path中，设置属性
        path.insert(path.begin(), startPose);
        if(!bDirectionBased)
          path.at(0).pos.a = path.at(1).pos.a;

        path.at(0).beh_state = path.at(1).beh_state = PlannerHNS::BEH_FORWARD_STATE;
        path.at(0).laneId = path.at(1).laneId;

        PlanningHelpers::FixPathDensity(path, pathDensity);
        PlanningHelpers::SmoothPath(path,0.4,0.3,0.1);
        PlanningHelpers::CalcAngleAndCost(path);
        paths.push_back(path);
      }
    }
  }

  if(bDirectionBased && bFindBranches)
  {
    WayPoint p1, p2;
    if(paths.size()> 0 && paths.at(0).size() > 0)
      p2 = p1 = paths.at(0).at(0);
    else
      p2 = p1 = startPose;

    double branch_length = maxPlanningDistance*0.5;

    p2.pos.y = p1.pos.y + branch_length*0.4*sin(p1.pos.a);
    p2.pos.x = p1.pos.x + branch_length*0.4*cos(p1.pos.a);

    vector<WayPoint> l_branch;
    vector<WayPoint> r_branch;
   //手工生成分支，以p1, p2,为起点，branch_length为距离生成终点
    PlanningHelpers::CreateManualBranchFromTwoPoints(p1, p2, branch_length, FORWARD_RIGHT_DIR,r_branch);
    PlanningHelpers::CreateManualBranchFromTwoPoints(p1, p2, branch_length, FORWARD_LEFT_DIR, l_branch);

    PlanningHelpers::FixPathDensity(l_branch, pathDensity);
    PlanningHelpers::SmoothPath(l_branch,0.4,0.3,0.1);
    PlanningHelpers::CalcAngleAndCost(l_branch);

    PlanningHelpers::FixPathDensity(r_branch, pathDensity);
    PlanningHelpers::SmoothPath(r_branch,0.4,0.3,0.1);
    PlanningHelpers::CalcAngleAndCost(r_branch);

    paths.push_back(l_branch);
    paths.push_back(r_branch);
  }

  DeleteWaypoints(all_cell_to_delete);

  return paths.size();
}

2.3 PredictiveIgnorIdsDP()

int PlanningHelpers::PredictiveIgnorIdsDP(WayPoint* pStart, const double& DistanceLimit,
    vector<WayPoint*>& all_cells_to_delete,vector<WayPoint*>& end_waypoints, std::vector<int>& lanes_ids)
{
  if(!pStart) return 0;

    vector<pair<WayPoint*, WayPoint*> >nextLeafToTrace;

    WayPoint* pZero = 0;
    WayPoint* wp    = new WayPoint();
    *wp = *pStart;
    wp->cost = 0;
    wp->pLeft = 0;
    wp->pRight = 0;
    nextLeafToTrace.push_back(make_pair(pZero, wp));//当前搜索列表
    all_cells_to_delete.push_back(wp);

    double     distance     = 0;
    end_waypoints.clear();
    double     nCounter     = 0;

    while(nextLeafToTrace.size()>0)//当前搜索列表不为0
    {
      nCounter++;//搜寻次数

      WayPoint* pH   = nextLeafToTrace.at(0).second;//当前搜索列表第一个元素，设为当前搜索元素

      assert(pH != 0);  // 如果 pH == 0，则程序在此终止，下面的程序都不会执行

<<<<<<< HEAD
      nextLeafToTrace.erase(nextLeafToTrace.begin()+0);//从当前搜索列表中除去当前搜索元素

      for(unsigned int i =0; i< pH->pFronts.size(); i++)//遍历当前元素的下一个元素
=======
      nextLeafToTrace.erase(nextLeafToTrace.begin()+0);
      // All points in front of pH
      for(unsigned int i =0; i< pH->pFronts.size(); i++)
>>>>>>> 1e30b21e93b7e9bd87ade267651cd491da401bd6
      {
        if(pH->pFronts.at(i) && !CheckNodeExits(all_cells_to_delete, pH->pFronts.at(i)))//若存在，且不再all_cells_to_delete中
        {
          if(pH->cost < DistanceLimit)//当前点距离代价小于DistanceLimit
          {
            wp = new WayPoint();
            *wp = *pH->pFronts.at(i);
           //计算代价
            double d = distance2points(wp->pos, pH->pos);
            distance += d;
            wp->cost = pH->cost + d;
            wp->pBacks.push_back(pH);
            wp->pLeft = 0;
            wp->pRight = 0;

            bool bFoundLane = false;
            // lanes_ids起始为空，遍历lanes_ids，判断是否存在laneid与wp->laneId相同，若存在，不加入 nextLeafToTrace。
            for(unsigned int k = 0 ; k < lanes_ids.size(); k++)
            {
              if(wp->laneId == lanes_ids.at(k))
              {
                bFoundLane = true;
                break;
              }
            }
            // 如果在 lanes_ids 里找不到 wp 所在的 laneId
            if(!bFoundLane)
              nextLeafToTrace.push_back(make_pair(pH, wp));
            all_cells_to_delete.push_back(wp);
          }
          else//当前点距离代价大于DistanceLimit
          // 如果超出搜索距离
          {
            end_waypoints.push_back(pH);//加入end_waypoints
          }
        }
      }
    }

    while(nextLeafToTrace.size()!=0)
      nextLeafToTrace.pop_back();
    //closed_nodes.clear();

    return end_waypoints.size();
}