OpenPlanner变道遇到的问题及解决

最新推荐文章于 2024-04-11 17:42:58 发布

此心安处是吾乡-Aaron

最新推荐文章于 2024-04-11 17:42:58 发布

阅读量2.3k

点赞数 6

分类专栏：路径规划 ROS 文章标签：算法自动驾驶

本文链接：https://blog.csdn.net/weixin_39940512/article/details/117451918

版权

路径规划同时被 2 个专栏收录

3 篇文章 6 订阅

订阅专栏

ROS

1 篇文章 0 订阅

订阅专栏

OpenPlanner规划全局路径，如果要支持实时根据环境更新路径（变道、改变路径）需要在op_global_planner中勾选Lane Changing和Replanning两个功能

在这里插入图片描述
此外还要在op_global_planner.launch文件中使能enableDynamicMapUpdate，整个变道的流程如下图所示：

在这里插入图片描述
首先讲一下wayarea2grid这个节点，主要作用是读取vector_map中的WAY_AREA信息，这里遇到了第一个问题：绘制的vector_map没有WAY_AREA信息，甚至Autoware自带的地图也不包含WAY_AREA信息，经过查找发现原本使用的插件版本是MapToolbox-0.1.1-preview.6不包含WAY_AREA，而新版本MapToolbox-0.1.1-preview.9是包含WAY_AREA的，于是重新绘制，问题解决。

然后是road_occupancy_processor这个节点，它结合点云信息和grid_map_wayarea生成gridmap_road_status：一个包含障碍物信息的占位栅格图；op_global_planner会根据占位栅格图的信息更新vector_map的权重然后重新规划路径。

要想知道op_global_planner是如何规划路径并变道的，就必须先了解它是如何组织构建地图信息的。

基本道路元素：

Road
Node、Lane (车道、节点)
Way area(可行驶区域)
Dtlane:(目前不支持)
Road Shape
Curb
Roadedge(路边缘)
Gutter(侧水沟)
Intersection(路口)
Road Surface(路面)
Whiteline(白线)
Stopline(停车线)
Zebrazone(斑马线)
Crosswalk(人行横道)
RoadSurfaceMark(路面标识)
Roadside
Guardrail(护栏)
Sidewalk(人行道)
Structure
Pole(杆)
Utilitypole(电线杆)
Roadsign(标识)
Signaldata(信号灯)
Streetlight(路灯)
Curvemirror
Wall
Fence(围栏)
RailroadCrossing(铁路道口)

上图是Autoware自带的矢量地图在这里插入图片描述

上图是Autoware自带的矢量地图在Rviz上的显示和保存地图的文件，可以看出不同的文件对应不同的基本道路元素，但从存储的文件到在Rviz上进行可视化，中间经历了怎样的过程对我们理解路径规划是至关重要的。

读取和发布

在Runtime Manager中选择好文件路径并点击Vector Map，就完成了矢量地图的读取：

在这里插入图片描述
通过查看autoware.ai/src/autoware/utilities/runtime_manager/scripts/map.yaml可以知道点击Vector Map时运行了map_file包里的vector_map_loader节点：

它读取矢量地图信息并按照一定的组织方式发布出去，这样不同的节点只需要按需订阅即可，不用反复读取，此时的Vector Map并不能用于路径规划，还需要进一步组织。

RoadNetwork构建

op_global_planner首先订阅了所需的vector_map_info：

  //Mapping Section
  sub_lanes = nh.subscribe("/vector_map_info/lane", 1, &GlobalPlanner::callbackGetVMLanes,  this);
  sub_points = nh.subscribe("/vector_map_info/point", 1, &GlobalPlanner::callbackGetVMPoints,  this);
  sub_dt_lanes = nh.subscribe("/vector_map_info/dtlane", 1, &GlobalPlanner::callbackGetVMdtLanes,  this);
  sub_intersect = nh.subscribe("/vector_map_info/cross_road", 1, &GlobalPlanner::callbackGetVMIntersections,  this);
  sup_area = nh.subscribe("/vector_map_info/area", 1, &GlobalPlanner::callbackGetVMAreas,  this);
  sub_lines = nh.subscribe("/vector_map_info/line", 1, &GlobalPlanner::callbackGetVMLines,  this);
  sub_stop_line = nh.subscribe("/vector_map_info/stop_line", 1, &GlobalPlanner::callbackGetVMStopLines,  this);
  sub_signals = nh.subscribe("/vector_map_info/signal", 1, &GlobalPlanner::callbackGetVMSignal,  this);
  sub_vectors = nh.subscribe("/vector_map_info/vector", 1, &GlobalPlanner::callbackGetVMVectors,  this);
  sub_curbs = nh.subscribe("/vector_map_info/curb", 1, &GlobalPlanner::callbackGetVMCurbs,  this);
  sub_edges = nh.subscribe("/vector_map_info/road_edge", 1, &GlobalPlanner::callbackGetVMRoadEdges,  this);
  sub_way_areas = nh.subscribe("/vector_map_info/way_area", 1, &GlobalPlanner::callbackGetVMWayAreas,  this);
  sub_cross_walk = nh.subscribe("/vector_map_info/cross_walk", 1, &GlobalPlanner::callbackGetVMCrossWalks,  this);
  sub_nodes = nh.subscribe("/vector_map_info/node", 1, &GlobalPlanner::callbackGetVMNodes,  this);

然后在回调函数中将对应的数据保存到m_MapRaw中：

//Mapping Section
void GlobalPlanner::callbackGetVMLanes(const vector_map_msgs::LaneArray& msg)
{
  std::cout << "Received Lanes" << msg.data.size() << endl;
  if(m_MapRaw.pLanes == nullptr)
    m_MapRaw.pLanes = new UtilityHNS::AisanLanesFileReader(msg);
}

void GlobalPlanner::callbackGetVMPoints(const vector_map_msgs::PointArray& msg)
{
  std::cout << "Received Points" << msg.data.size() << endl;
  if(m_MapRaw.pPoints  == nullptr)
    m_MapRaw.pPoints = new UtilityHNS::AisanPointsFileReader(msg);
}

void GlobalPlanner::callbackGetVMdtLanes(const vector_map_msgs::DTLaneArray& msg)
{
  std::cout << "Received dtLanes" << msg.data.size() << endl;
  if(m_MapRaw.pCenterLines == nullptr)
    m_MapRaw.pCenterLines = new UtilityHNS::AisanCenterLinesFileReader(msg);
}

void GlobalPlanner::callbackGetVMIntersections(const vector_map_msgs::CrossRoadArray& msg)
{
  std::cout << "Received CrossRoads" << msg.data.size() << endl;
  if(m_MapRaw.pIntersections == nullptr)
    m_MapRaw.pIntersections = new UtilityHNS::AisanIntersectionFileReader(msg);
}

void GlobalPlanner::callbackGetVMAreas(const vector_map_msgs::AreaArray& msg)
{
  std::cout << "Received Areas" << msg.data.size() << endl;
  if(m_MapRaw.pAreas == nullptr)
    m_MapRaw.pAreas = new UtilityHNS::AisanAreasFileReader(msg);
}

void GlobalPlanner::callbackGetVMLines(const vector_map_msgs::LineArray& msg)
{
  std::cout << "Received Lines" << msg.data.size() << endl;
  if(m_MapRaw.pLines == nullptr)
    m_MapRaw.pLines = new UtilityHNS::AisanLinesFileReader(msg);
}

void GlobalPlanner::callbackGetVMStopLines(const vector_map_msgs::StopLineArray& msg)
{
  std::cout << "Received StopLines" << msg.data.size() << endl;
  if(m_MapRaw.pStopLines == nullptr)
    m_MapRaw.pStopLines = new UtilityHNS::AisanStopLineFileReader(msg);
}

void GlobalPlanner::callbackGetVMSignal(const vector_map_msgs::SignalArray& msg)
{
  std::cout << "Received Signals" << msg.data.size() << endl;
  if(m_MapRaw.pSignals  == nullptr)
    m_MapRaw.pSignals = new UtilityHNS::AisanSignalFileReader(msg);
}

void GlobalPlanner::callbackGetVMVectors(const vector_map_msgs::VectorArray& msg)
{
  std::cout << "Received Vectors" << msg.data.size() << endl;
  if(m_MapRaw.pVectors  == nullptr)
    m_MapRaw.pVectors = new UtilityHNS::AisanVectorFileReader(msg);
}

void GlobalPlanner::callbackGetVMCurbs(const vector_map_msgs::CurbArray& msg)
{
  std::cout << "Received Curbs" << msg.data.size() << endl;
  if(m_MapRaw.pCurbs == nullptr)
    m_MapRaw.pCurbs = new UtilityHNS::AisanCurbFileReader(msg);
}

void GlobalPlanner::callbackGetVMRoadEdges(const vector_map_msgs::RoadEdgeArray& msg)
{
  std::cout << "Received Edges" << msg.data.size() << endl;
  if(m_MapRaw.pRoadedges  == nullptr)
    m_MapRaw.pRoadedges = new UtilityHNS::AisanRoadEdgeFileReader(msg);
}

void GlobalPlanner::callbackGetVMWayAreas(const vector_map_msgs::WayAreaArray& msg)
{
  std::cout << "Received Wayareas" << msg.data.size() << endl;
  if(m_MapRaw.pWayAreas  == nullptr)
    m_MapRaw.pWayAreas = new UtilityHNS::AisanWayareaFileReader(msg);
}

void GlobalPlanner::callbackGetVMCrossWalks(const vector_map_msgs::CrossWalkArray& msg)
{
  std::cout << "Received CrossWalks" << msg.data.size() << endl;
  if(m_MapRaw.pCrossWalks == nullptr)
    m_MapRaw.pCrossWalks = new UtilityHNS::AisanCrossWalkFileReader(msg);
}

void GlobalPlanner::callbackGetVMNodes(const vector_map_msgs::NodeArray& msg)
{
  std::cout << "Received Nodes" << msg.data.size() << endl;
  if(m_MapRaw.pNodes == nullptr)
    m_MapRaw.pNodes = new UtilityHNS::AisanNodesFileReader(msg);
}

回到op_global_planner，的MainLoop()函数中：

void GlobalPlanner::MainLoop()
{
  ros::Rate loop_rate(25);//循环频率
  timespec animation_timer;//计时器
  UtilityHNS::UtilityH::GetTickCount(animation_timer);

  while (ros::ok())
  {
    ros::spinOnce();
    bool bMakeNewPlan = false;
    //地图种类选择，这里默认地图类型为PlannerHNS::MAP_AUTOWARE
    if(m_params.mapSource == PlannerHNS::MAP_KML_FILE && !m_bKmlMap)
    {
      m_bKmlMap = true;
      PlannerHNS::MappingHelpers::LoadKML(m_params.KmlMapPath, m_Map);
      visualization_msgs::MarkerArray map_marker_array;
      PlannerHNS::ROSHelpers::ConvertFromRoadNetworkToAutowareVisualizeMapFormat(m_Map, map_marker_array);
      pub_MapRviz.publish(map_marker_array);
    }
    else if (m_params.mapSource == PlannerHNS::MAP_FOLDER && !m_bKmlMap)
    {
      m_bKmlMap = true;
      PlannerHNS::MappingHelpers::ConstructRoadNetworkFromDataFiles(m_params.KmlMapPath, m_Map, true);
      visualization_msgs::MarkerArray map_marker_array;
      PlannerHNS::ROSHelpers::ConvertFromRoadNetworkToAutowareVisualizeMapFormat(m_Map, map_marker_array);

      pub_MapRviz.publish(map_marker_array);
    }
    else if (m_params.mapSource == PlannerHNS::MAP_AUTOWARE && !m_bKmlMap)
    {
      std::vector<UtilityHNS::AisanDataConnFileReader::DataConn> conn_data;;
      //根据地图是否有节点(Node):有节点：m_MapRaw.GetVersion()==2，无节点：m_MapRaw.GetVersion()==1
      if(m_MapRaw.GetVersion()==2)
      {
        std::cout << "Map Version 2" << endl;
        m_bKmlMap = true;
        PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessageV2(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list,
            m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list,
            m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list,  m_MapRaw.pSignals->m_data_list,
            m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list,
            m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data,
            m_MapRaw.pLanes, m_MapRaw.pPoints, m_MapRaw.pNodes, m_MapRaw.pLines, PlannerHNS::GPSPoint(), m_Map, true, m_params.bEnableLaneChange, false);
      }
      else if(m_MapRaw.GetVersion()==1)
      {
        std::cout << "Map Version 1" << endl;
        m_bKmlMap = true;
        //初始化地图
        PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessage(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list,
            m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list,
            m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list,  m_MapRaw.pSignals->m_data_list,
            m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list,
            m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data,  PlannerHNS::GPSPoint(), m_Map, true, m_params.bEnableLaneChange, false);
      }
      //用于地图可视化
      if(m_bKmlMap)
      {
        visualization_msgs::MarkerArray map_marker_array;
        PlannerHNS::ROSHelpers::ConvertFromRoadNetworkToAutowareVisualizeMapFormat(m_Map, map_marker_array);
        pub_MapRviz.publish(map_marker_array);
      }
    }

    ClearOldCostFromMap();

    if(m_GoalsPos.size() > 0)
    {
      //判断m_GeneratedTotalPaths是否为空
      if(m_GeneratedTotalPaths.size() > 0 && m_GeneratedTotalPaths.at(0).size() > 3)
      {
        if(m_params.bEnableReplanning)
        {
          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())
          {
            //prep\_point到目标点的距离
            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;
            }
          }
        }
      }
      else
        bMakeNewPlan = true;
      //判断是否需要规划新的路径
      if(bMakeNewPlan || (m_params.bEnableDynamicMapUpdate && UtilityHNS::UtilityH::GetTimeDiffNow(m_ReplnningTimer) > REPLANNING_TIME))
      {
        UtilityHNS::UtilityH::GetTickCount(m_ReplnningTimer);
        PlannerHNS::WayPoint goalPoint = m_GoalsPos.at(m_iCurrentGoalIndex);
        bool bNewPlan = GenerateGlobalPlan(m_CurrentPose, goalPoint, m_GeneratedTotalPaths);


        if(bNewPlan)
        {
          bMakeNewPlan = false;
          VisualizeAndSend(m_GeneratedTotalPaths);
        }
      }
      VisualizeDestinations(m_GoalsPos, m_iCurrentGoalIndex);
    }

    loop_rate.sleep();
  }
}

这里调用ConstructRoadNetworkFromROSMessageV2构建RoadNetwork，其中有一个关键参数m_params.bEnableLaneChange，也就是之前我们在Runtime Manager中勾选的Lane Changing。

void MappingHelpers::ConstructRoadNetworkFromROSMessageV2(const std::vector<UtilityHNS::AisanLanesFileReader::AisanLane>& lanes_data,
    const std::vector<UtilityHNS::AisanPointsFileReader::AisanPoints>& points_data,
    const std::vector<UtilityHNS::AisanCenterLinesFileReader::AisanCenterLine>& dt_data,
    const std::vector<UtilityHNS::AisanIntersectionFileReader::AisanIntersection>& intersection_data,
    const std::vector<UtilityHNS::AisanAreasFileReader::AisanArea>& area_data,
    const std::vector<UtilityHNS::AisanLinesFileReader::AisanLine>& line_data,
    const std::vector<UtilityHNS::AisanStopLineFileReader::AisanStopLine>& stop_line_data,
    const std::vector<UtilityHNS::AisanSignalFileReader::AisanSignal>& signal_data,
    const std::vector<UtilityHNS::AisanVectorFileReader::AisanVector>& vector_data,
    const std::vector<UtilityHNS::AisanCurbFileReader::AisanCurb>& curb_data,
    const std::vector<UtilityHNS::AisanRoadEdgeFileReader::AisanRoadEdge>& roadedge_data,
    const std::vector<UtilityHNS::AisanWayareaFileReader::AisanWayarea>& wayarea_data,
    const std::vector<UtilityHNS::AisanCrossWalkFileReader::AisanCrossWalk>& crosswalk_data,
    const std::vector<UtilityHNS::AisanNodesFileReader::AisanNode>& nodes_data,
    const std::vector<UtilityHNS::AisanDataConnFileReader::DataConn>& conn_data,
    UtilityHNS::AisanLanesFileReader* pLaneData,
    UtilityHNS::AisanPointsFileReader* pPointsData,
    UtilityHNS::AisanNodesFileReader* pNodesData,
    UtilityHNS::AisanLinesFileReader* pLinedata,
    const GPSPoint& origin, RoadNetwork& map, const bool& bSpecialFlag,
    const bool& bFindLaneChangeLanes, const bool& bFindCurbsAndWayArea)
{
  vector<Lane> roadLanes;

  for(unsigned int i=0; i< pLaneData->m_data_list.size(); i++)
  {
    if(pLaneData->m_data_list.at(i).LnID > g_max_lane_id)
      g_max_lane_id = pLaneData->m_data_list.at(i).LnID;
  }

  cout << " >> Extracting Lanes ... " << endl;
  CreateLanes(pLaneData, pPointsData, pNodesData, roadLanes);

  cout << " >> Fix Waypoints errors ... " << endl;
  FixTwoPointsLanes(roadLanes);
  FixRedundantPointsLanes(roadLanes);

  ConnectLanes(pLaneData, roadLanes);

  cout << " >> Create Missing lane connections ... " << endl;
  FixUnconnectedLanes(roadLanes);
  FixTwoPointsLanes(roadLanes);

  //map has one road segment
  RoadSegment roadSegment1;
  roadSegment1.id = 1;
  roadSegment1.Lanes = roadLanes;
  map.roadSegments.push_back(roadSegment1);

  //Fix angle for lanes
  for(unsigned int rs = 0; rs < map.roadSegments.size(); rs++)
  {
    for(unsigned int i =0; i < map.roadSegments.at(rs).Lanes.size(); i++)
    {
      Lane* pL = &map.roadSegments.at(rs).Lanes.at(i);
      PlannerHNS::PlanningHelpers::FixAngleOnly(pL->points);
    }
  }

  //Link Lanes and lane's waypoints by pointers
  cout << " >> Link lanes and waypoints with pointers ... " << endl;
  LinkLanesPointers(map);

  for(unsigned int rs = 0; rs < map.roadSegments.size(); rs++)
  {
    for(unsigned int i =0; i < map.roadSegments.at(rs).Lanes.size(); i++)
    {
      Lane* pL = &map.roadSegments.at(rs).Lanes.at(i);
      for(unsigned int j = 0 ; j < pL->points.size(); j++)
      {
          if(pL->points.at(j).actionCost.size() > 0)
        {
          if(pL->points.at(j).actionCost.at(0).first == LEFT_TURN_ACTION)
          {
            AssignActionCostToLane(pL, LEFT_TURN_ACTION, LEFT_INITIAL_TURNS_COST);
            break;
          }
          else if(pL->points.at(j).actionCost.at(0).first == RIGHT_TURN_ACTION)
          {
            AssignActionCostToLane(pL, RIGHT_TURN_ACTION, RIGHT_INITIAL_TURNS_COST);
          break;

          }
        }
      }
    }
  }

  if(bFindLaneChangeLanes)
  {
    cout << " >> Extract Lane Change Information... " << endl;
    FindAdjacentLanesV2(map);
  }

  //Extract Signals and StopLines
  cout << " >> Extract Signal data ... " << endl;
  ExtractSignalDataV2(signal_data, vector_data, pPointsData, origin, map);

  //Stop Lines
  cout << " >> Extract Stop lines data ... " << endl;
  ExtractStopLinesDataV2(stop_line_data, pLinedata, pPointsData, origin, map);

  if(bFindCurbsAndWayArea)
  {
    //Curbs
    cout << " >> Extract curbs data ... " << endl;
    ExtractCurbDataV2(curb_data, pLinedata, pPointsData, origin, map);

    //Wayarea
    cout << " >> Extract wayarea data ... " << endl;
    ExtractWayArea(area_data, wayarea_data, line_data, points_data, origin, map);
  }

  //Link waypoints
  cout << " >> Link missing branches and waypoints... " << endl;
  LinkMissingBranchingWayPointsV2(map);

  //Link StopLines and Traffic Lights
  cout << " >> Link StopLines and Traffic Lights ... " << endl;
  LinkTrafficLightsAndStopLinesV2(map);
//  //LinkTrafficLightsAndStopLinesConData(conn_data, id_replace_list, map);

  cout << " >> Map loaded from data with " << roadLanes.size()  << " lanes" << endl;
}

此处我们重点关注的是跟变道有关的代码，即函数：FindAdjacentLanesV2

void MappingHelpers::FindAdjacentLanesV2(RoadNetwork& map)
{
  bool bTestDebug = false;
  for(unsigned int rs = 0; rs < map.roadSegments.size(); rs++)
  {
    for(unsigned int i =0; i < map.roadSegments.at(rs).Lanes.size(); i++)
    {
      Lane* pL = &map.roadSegments.at(rs).Lanes.at(i);
      for(unsigned int i2 =0; i2 < map.roadSegments.at(rs).Lanes.size(); i2++)
      {
        Lane* pL2 = &map.roadSegments.at(rs).Lanes.at(i2);

        if(pL->id == pL2->id) continue;


        if(pL->id == 1683)
          bTestDebug = true;

        for(unsigned int p=0; p < pL->points.size(); p++)
        {
          WayPoint* pWP = &pL->points.at(p);
          RelativeInfo info;
          PlanningHelpers::GetRelativeInfoLimited(pL2->points, *pWP, info);
          cout << "info.bAfter: " << info.bAfter << "  info.bBefore:" << info.bBefore << "    info.perp_distance:" << fabs(info.perp_distance) << "    angle:" << UtilityH::AngleBetweenTwoAnglesPositive(info.perp_point.pos.a, pWP->pos.a) << endl;
          if(!info.bAfter && !info.bBefore && fabs(info.perp_distance) > 0.5 && fabs(info.perp_distance) < 5 && UtilityH::AngleBetweenTwoAnglesPositive(info.perp_point.pos.a, pWP->pos.a) < 0.06)
          {
            WayPoint* pWP2 = &pL2->points.at(info.iFront);
            if(info.perp_distance < 0)
            {
              if(pWP->pRight == 0)
              {
                pWP->pRight = pWP2;
                pWP->RightPointId = pWP2->id;
                pWP->RightLnId = pL2->id;
                pL->pRightLane = pL2;

              }

              if(pWP2->pLeft == 0)
              {
                pWP2->pLeft = pWP;
                pWP2->LeftPointId = pWP->id;
                pWP2->LeftLnId = pL->id;
                pL2->pLeftLane = pL;
              }
            }
            else
            {
              if(pWP->pLeft == 0)
              {
                pWP->pLeft = pWP2;
                pWP->LeftPointId = pWP2->id;
                pWP->LeftLnId = pL2->id;
                pL->pLeftLane = pL2;
              }

              if(pWP2->pRight == 0)
              {
                pWP2->pRight = pWP;
                pWP2->RightPointId = pWP->id;
                pWP2->RightLnId = pL->id;
                pL2->pRightLane = pL;
              }
            }
          }
        }
      }
    }
  }
}

这段代码是理解变道过程的关键，我们知道Lane由一系列的Node组成，不同的Lane汇聚分叉组成道路网即RoadNetwork，但对于同向的并行车道现有的拓扑连通图并不足以完成变道的任务，如下图所示：

在这里插入图片描述

这就是FindAdjacentLanesV2要解决的问题：构建不同车道临近节点的连接。

在这里插入图片描述

具体到一个节点相对一条道路时有两种情况如下图所示：

在这里插入图片描述

GetRelativeInfoLimited函数计算节点pWP和道路pl2的相对位置关系，这里的计算都是以pWP为圆心进行计算的，perp_distance为道路上与pWP距离最近的两个节点所在直线在y轴上的截距，perp_distance为正表示pWP在道路的右边，为负表示pWP在道路的左边。

这里有一个bug困扰了我好久，倒数第四行原始代码中是这样的：pWP2->pRight = pWP->pLeft;，这导致有些地方可以从右向左变道但是不能从左向右变道，更改过后立刻就可以了。