Autoware代码op-创新互联

(注:本人的第一篇文章,能力有限,写的不好,请见谅!)

成都创新互联公司是专业的乐陵网站建设公司,乐陵接单;提供网站制作、成都网站建设,网页设计,网站设计,建网站,PHP网站建设等专业做网站服务;采用PHP框架,可快速的进行乐陵网站开发网页制作和功能扩展;专业做搜索引擎喜爱的网站,专业的做网站团队,希望更多企业前来合作!
  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 ; iobjects.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 ; iobj, 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

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& 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& curr_obj_list,RoadNetwork& map, std::vector& old_obj_list)
{
  PlannerH planner;
  m_temp_list_ii.clear();//存放当前帧的障碍物列表

  std::vectordelete_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::vectorclosestWPs, const double& maxPlanningDistance, std::vector>& paths, const bool& bFindBranches , const bool bDirectionBased, const bool pathDensity)
{
  vector>tempCurrentForwardPathss;
  vectorall_cell_to_delete;
  vectorglobalPath;

  vectorpLaneCells;
  vectorunique_lanes;
  std::vectorpath;
  //遍历当前障碍物的每一个可行最近点
  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);

    vectorl_branch;
    vectorr_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& all_cells_to_delete,vector& end_waypoints, std::vector& lanes_ids)
{
  if(!pStart) return 0;

    vector>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();
}

你是否还在寻找稳定的海外服务器提供商?创新互联www.cdcxhl.cn海外机房具备T级流量清洗系统配攻击溯源,准确流量调度确保服务器高可用性,企业级服务器适合批量采购,新人活动首月15元起,快前往官网查看详情吧


网站栏目:Autoware代码op-创新互联
转载注明:http://hbruida.cn/article/cdhpeo.html