Autoware1.15 + OpenPlanner2.5 下的laneChange解析（1）

近期一直使用lgsvl测试autoware的各种功能，前期使用autoware1.14+OpenPlanner_v1，测试期间发现OpenPlanner_v1可以正常生成local trajactories，但local trajactory的evalutor和selector部分有问题，大部分时刻没有做出正确的选择，且检测到障碍物的trajactory没有正常被Block掉。因此升级使用Autoware1.15 + OpenPlanner2.5。

Autoware1.15 + OpenPlanner2.5地址：

GitHub - hatem-darweesh/autoware.ai.openplanner: The workspace for directly downloading and installing Autoware.AI versions with the latest OpenPlanner 2.5 updatesThe workspace for directly downloading and installing Autoware.AI versions with the latest OpenPlanner 2.5 updates - GitHub - hatem-darweesh/autoware.ai.openplanner: The workspace for directly downloading and installing Autoware.AI versions with the latest OpenPlanner 2.5 updateshttps://github.com/hatem-darweesh/autoware.ai.openplanner

本篇先研究global planner里的laneChange功能，分为以下几个部分：

Global planner里的laneChange

1. 对vector map的要求

2. Vector map的loader如何提供laneChange的功能

3. Global planner中laneChange的代码实现

1.对vector map的要求

LaneChange顾名思义就是车辆行驶过程中，从一条lane change到另一条lane，那么势必要求地图中本身存在并行的车道，我们可以从vector map的lane.csv中，每个lane的LCnt看出该lane的并行lane数量。

将vector map load进tier4，选择某条lane，也能看出它的并行lane数量，并在可视化的vector map里得到确认。

2. Vector map的loader如何提供laneChange的功能

Vector map部分的代码是如何处理map数据，实现laneChange的呢？Open planner会创建VectorMapLoader，调用ConstructRoadNetworkFromROSMessageV2函数构建RoadNetwork，其中有一个关键参数m_params.bEnableLaneChange，也就是之前我们在Runtime Manager中勾选的Lane Changing。其中跟变道有关的函数为FindAdjacentLanesV2。具体解析参考这个博客：

OpenPlanner变道遇到的问题及解决_此心安处是吾乡-Aaron的博客-CSDN博客_openplanner111https://blog.csdn.net/weixin_39940512/article/details/117451918

3. 车辆运行中laneChange的代码实现

首先看op_global_planner_core.cpp里的MainLoop()，其中实现global plan功能的函数GenerateGlobalPlan：

if(m_iCurrentGoalIndex >= 0 && (m_bReplanSignal || bMakeNewPlan || m_GeneratedTotalPaths.size() == 0)){PlannerHNS::WayPoint goalPoint = m_GoalsPos.at(m_iCurrentGoalIndex);bool bNewPlan = GenerateGlobalPlan(m_CurrentPose, goalPoint, m_GeneratedTotalPaths);

具体实现：

bool GlobalPlanner::GenerateGlobalPlan(PlannerHNS::WayPoint& startPoint, PlannerHNS::WayPoint& goalPoint, std::vector<std::vector<PlannerHNS::WayPoint> >& generatedTotalPaths)
{std::vector<int> predefinedLanesIds;double ret = 0;//The distance that is needed to brake from current speed to zero with acceleration of -1 m/s*sdouble planning_distance = pow((m_CurrentPose.v), 2);if(planning_distance < MIN_EXTRA_PLAN_DISTANCE){planning_distance = MIN_EXTRA_PLAN_DISTANCE;}// ... ignore if(m_bEnableAnimation)...else{if(m_bStoppingState){generatedTotalPaths.clear();ret = m_PlannerH.PlanUsingDPRandom(startPoint, 20, m_Map, generatedTotalPaths);}else if(m_bExploreMode){std::cout << "Generating Random Trajectories ..  " << std::endl;generatedTotalPaths.clear();ret = m_PlannerH.PlanUsingDPRandom(m_CurrentPose, 250, m_Map, generatedTotalPaths);}else //do this{ret = m_PlannerH.PlanUsingDP(startPoint, goalPoint, MAX_GLOBAL_PLAN_SEARCH_DISTANCE, planning_distance,  m_params.bEnableLaneChange, predefinedLanesIds, m_Map, generatedTotalPaths);}}if(ret == 0){std::cout << "Can't Generate Global Path for Start (" << startPoint.pos.ToString()<< ") and Goal (" << goalPoint.pos.ToString() << ") in GenerateGlobalPlan()" << std::endl;return false;}

会调用PlanUsingDP函数，具体实现在PlannerH.cpp里：

double PlannerH::PlanUsingDP(const WayPoint& start,const WayPoint& goalPos,const double& maxSearchDistance,const double& planning_distance,const bool bEnableLaneChange,const std::vector<int>& globalPath,RoadNetwork& map,std::vector<std::vector<WayPoint> >& paths, vector<WayPoint*>* all_cell_to_delete) //paths's size is 0 at the beginning
{PlannerHNS::WayPoint* pStart = PlannerHNS::MappingHelpers::GetClosestWaypointFromMap(start, map);PlannerHNS::WayPoint* pGoal = PlannerHNS::MappingHelpers::GetClosestWaypointFromMap(goalPos, map);if(!pStart ||  !pGoal){GPSPoint sp = start.pos;GPSPoint gp = goalPos.pos;cout << endl << "Error: PlannerH -> Can't Find Global Waypoint Nodes in the Map, " << endl;cout << "  Start: " <<  sp.ToString() << "  Goal: " << gp.ToString() << "" << endl;return 0;}if(!pStart->pLane || !pGoal->pLane){cout << endl << "Error: PlannerH -> Null Lane," << endl << "  Start Lane: " << pStart->pLane << ",  Goal Lane: " << pGoal->pLane << endl;return 0;}RelativeInfo start_info, goal_info;PlanningHelpers::GetRelativeInfo(pStart->pLane->points, start, start_info);PlanningHelpers::GetRelativeInfo(pGoal->pLane->points, goalPos, goal_info);vector<WayPoint> goal_path;if(fabs(start_info.perp_distance) > START_POINT_MAX_DISTANCE){GPSPoint sp = start.pos;cout << endl << "Error: PlannerH -> Start Distance to Lane is: " << start_info.perp_distance<< ", Pose: " << sp.ToString() << ", LanePose:" << start_info.perp_point.pos.ToString()<< ", LaneID: " << pStart->pLane->id << " -> Check origin and vector map. " << endl;return 0;}if(fabs(goal_info.perp_distance) > GOAL_POINT_MAX_DISTANCE) //8{if(fabs(goal_info.perp_distance) > 20){GPSPoint gp = goalPos.pos;cout << endl << "Error: PlannerH -> Goal Distance to Lane is: " << goal_info.perp_distance<< ", Pose: " << gp.ToString() << ", LanePose:" << goal_info.perp_point.pos.ToString()<< ", LaneID: " << pGoal->pLane->id << " -> Check origin and vector map. " << endl;return 0;}else{WayPoint wp = *pGoal;wp.pos.x = (goalPos.pos.x+pGoal->pos.x)/2.0;wp.pos.y = (goalPos.pos.y+pGoal->pos.y)/2.0;goal_path.push_back(wp);goal_path.push_back(goalPos);}}vector<WayPoint*> local_cell_to_delete;WayPoint* pLaneCell = 0;char bPlan = 'A';if(all_cell_to_delete)pLaneCell =  PlanningHelpers::BuildPlanningSearchTreeV2(pStart, *pGoal, globalPath, maxSearchDistance,bEnableLaneChange, *all_cell_to_delete);elsepLaneCell =  PlanningHelpers::BuildPlanningSearchTreeV2(pStart, *pGoal, globalPath, maxSearchDistance,bEnableLaneChange, local_cell_to_delete);if(!pLaneCell){bPlan = 'B';cout << endl << "PlannerH -> Plan (A) Failed, Trying Plan (B)." << endl;if(all_cell_to_delete)pLaneCell =  PlanningHelpers::BuildPlanningSearchTreeStraight(pStart, planning_distance, *all_cell_to_delete);elsepLaneCell =  PlanningHelpers::BuildPlanningSearchTreeStraight(pStart, planning_distance, local_cell_to_delete);if(!pLaneCell){bPlan = 'Z';cout << endl << "PlannerH -> Plan (B) Failed, Sorry we Don't have plan (C) This is the END." << endl;return 0;}}vector<WayPoint> path;vector<vector<WayPoint> > tempCurrentForwardPathss;PlanningHelpers::TraversePathTreeBackwards(pLaneCell, pStart, globalPath, path, tempCurrentForwardPathss);if(path.size()==0) return 0;/*** Next line is added on 27 September 2020, when planning with map with sparse waypoints, skipping the start waypoint is a problem,* so I inserted after generating the initial plan*/path.insert(path.begin(), *pStart);paths.clear();if(bPlan == 'A'){PlanningHelpers::ExtractPlanAlernatives(path, planning_distance, paths, LANE_CHANGE_SMOOTH_FACTOR_DISTANCE);}else if (bPlan == 'B'){paths.push_back(path);}cout << endl <<"Info: PlannerH -> Plan (" << bPlan << ") Path With Size (" << (int)path.size() << "), MultiPaths No(" << paths.size() << ") Extraction Time : " << endl;if(path.size()<2){cout << endl << "Err: PlannerH -> Invalid Path, Car Should Stop." << endl;if(pLaneCell && !all_cell_to_delete)DeleteWaypoints(local_cell_to_delete);return 0 ;}if(pLaneCell && !all_cell_to_delete)DeleteWaypoints(local_cell_to_delete);for (int i = 0; i < path.size(); ++i) {std::cout << "path.at(" << i << ") 's distance is: " << path.at(i).distanceCost << ", laneId is: " << path.at(i).laneId << ", timeCost is: " << path.at(i).timeCost << std::endl;}double totalPlanningDistance = path.at(path.size()-1).distanceCost;return 1; //original is return totalPlanningDistance
}

此函数首先获取起点和终点的相关信息：

在由用户输入的start和goalPos（一般在Rviz上画出来）调用GetClosestWaypointFromMap，获得vector map中的最近的位置pStart和pGoal后，再调用GetRelativeInfo获取pStart和pGoal的relativeInfo start_info和goal_info。

再调用BuildPlanningSearchTreeV2函数，寻找是否存在由pStart到达pGoal的最短路径，具体实现在PlanningHelpers.cpp中：

WayPoint* PlanningHelpers::BuildPlanningSearchTreeV2(WayPoint* pStart,const WayPoint& goalPos,const vector<int>& globalPath,const double& DistanceLimit,const bool& bEnableLaneChange,vector<WayPoint*>& all_cells_to_delete)
{if(!pStart) return NULL;vector<pair<WayPoint*, WayPoint*> >nextLeafToTrace;WayPoint* pZero = 0;WayPoint* wp    = new WayPoint();*wp = *pStart;nextLeafToTrace.push_back(make_pair(pZero, wp));all_cells_to_delete.push_back(wp);double      distance        = 0;double         before_change_distance  = 0;WayPoint*  pGoalCell       = 0;double         nCounter        = 0;while(nextLeafToTrace.size()>0){nCounter++;unsigned int min_cost_index = 0;double min_cost = DBL_MAX;for(unsigned int i=0; i < nextLeafToTrace.size(); i++){if(nextLeafToTrace.at(i).second->cost < min_cost){min_cost = nextLeafToTrace.at(i).second->cost;min_cost_index = i;}}WayPoint* pH  = nextLeafToTrace.at(min_cost_index).second;assert(pH != 0);nextLeafToTrace.erase(nextLeafToTrace.begin()+min_cost_index);double distance_to_goal = distance2points(pH->pos, goalPos.pos);double angle_to_goal = UtilityHNS::UtilityH::AngleBetweenTwoAnglesPositive(UtilityHNS::UtilityH::FixNegativeAngle(pH->pos.a), UtilityHNS::UtilityH::FixNegativeAngle(goalPos.pos.a));if( distance_to_goal <= 0.1 && angle_to_goal < M_PI_4){cout << "Goal Found, LaneID: " << pH->laneId <<", Distance : " << distance_to_goal << ", Angle: " << angle_to_goal*RAD2DEG << endl;pGoalCell = pH;break;}else{if(pH->pLeft && !CheckLaneExits(all_cells_to_delete, pH->pLeft->pLane) && !CheckNodeExits(all_cells_to_delete, pH->pLeft) && bEnableLaneChange && before_change_distance > LANE_CHANGE_MIN_DISTANCE){wp = new WayPoint();*wp = *pH->pLeft;double d = hypot(wp->pos.y - pH->pos.y, wp->pos.x - pH->pos.x);distance += d;before_change_distance = -LANE_CHANGE_MIN_DISTANCE*2;for(unsigned int a = 0; a < wp->actionCost.size(); a++){//if(wp->actionCost.at(a).first == LEFT_TURN_ACTION)d += wp->actionCost.at(a).second;}wp->cost = pH->cost + d;wp->pRight = pH;wp->pLeft = 0;nextLeafToTrace.push_back(make_pair(pH, wp));all_cells_to_delete.push_back(wp);}if(pH->pRight && !CheckLaneExits(all_cells_to_delete, pH->pRight->pLane) && !CheckNodeExits(all_cells_to_delete, pH->pRight) && bEnableLaneChange && before_change_distance > LANE_CHANGE_MIN_DISTANCE){wp = new WayPoint();*wp = *pH->pRight;double d = hypot(wp->pos.y - pH->pos.y, wp->pos.x - pH->pos.x);distance += d;before_change_distance = -LANE_CHANGE_MIN_DISTANCE*2;for(unsigned int a = 0; a < wp->actionCost.size(); a++){//if(wp->actionCost.at(a).first == RIGHT_TURN_ACTION)d += wp->actionCost.at(a).second;}wp->cost = pH->cost + d;wp->pLeft = pH;wp->pRight = 0;nextLeafToTrace.push_back(make_pair(pH, wp));all_cells_to_delete.push_back(wp);}for(unsigned int i =0; i< pH->pFronts.size(); i++){if(CheckLaneIdExits(globalPath, pH->pLane) && pH->pFronts.at(i) && !CheckNodeExits(all_cells_to_delete, pH->pFronts.at(i))){wp = new WayPoint();*wp = *pH->pFronts.at(i);double d = hypot(wp->pos.y - pH->pos.y, wp->pos.x - pH->pos.x);distance += d;before_change_distance += d;for(unsigned int a = 0; a < wp->actionCost.size(); a++){//if(wp->actionCost.at(a).first == FORWARD_ACTION)d += wp->actionCost.at(a).second;}wp->cost = pH->cost + d;wp->pBacks.push_back(pH);nextLeafToTrace.push_back(make_pair(pH, wp));all_cells_to_delete.push_back(wp);}}}if(distance > DistanceLimit && globalPath.size()==0){//if(!pGoalCell)cout << "Goal Not Found, LaneID: " << pH->laneId <<", Distance : " << distance << endl;pGoalCell = pH;break;}//pGoalCell = pH;}while(nextLeafToTrace.size()!=0)nextLeafToTrace.pop_back();//closed_nodes.clear();return pGoalCell;
}

这个函数的大致逻辑为：

使用动态规划DP算法寻找是否存在由pStart到达pGoal的最短路径。会从pStart开始，不断遍历它能够到达的周边的下一个waypoint，（判断如果enbaleLaneChange，会找它的pLeft和pRight的wayPoint，这在前面构建vector map时会创建；如果没有enbaleLaneChange，则找它pFronts里的所有waypoints），加入到nextLeafToTrace，然后找到nextLeafToTrace里面WayPoint->cost最小的那个waypoint，把它设为选择的当前waypoint（代码里为pH），然后继续遍历nextLeafToTrace。本质上为贪心算法，只要保证在所有可到达的waypoint里，选择cost最小的那个waypoint，就保证里local optimal solution。这时保证有一条path with min_cost能够到达goal，并返回最终找到的那个waypoint（代码里为pH）。

笔者加的log:

回到PlanUsingDP函数，如果BuildPlanningSearchTreeV2失败，则会调用BuildPlanningSearchTreeStraight再次寻找路径，它和BuildPlanningSearchTreeV2的区别在于它不会考虑laneChange造成的pLeft和pRight的wayPoint，只考虑pFronts里的waypoints。

接下来调用TraversePathTreeBackwards函数，它是由BuildPlanningSearchTreeV2返回的能到达的最接近goal的waypoint（pHead)，遍历回到pStart。首先pHead会按直路遍历回pStart，如果存在laneChange导致的pLeft或者pRight，就会先调到pLeft或者pRight，然后遍历回到pStart。找到的全部路径会存放进PlanUsingDP函数里的vector<WayPoint> path里。

void PlanningHelpers::TraversePathTreeBackwards(WayPoint* pHead, WayPoint* pStartWP,const vector<int>& globalPathIds,vector<WayPoint>& localPath, std::vector<std::vector<WayPoint> >& localPaths)
{if(pHead != NULL && pHead->id != pStartWP->id){if(pHead->pBacks.size()>0){localPaths.push_back(localPath);TraversePathTreeBackwards(GetMinCostCell(pHead->pBacks, globalPathIds),pStartWP, globalPathIds, localPath, localPaths);pHead->bDir = FORWARD_DIR;localPath.push_back(*pHead);}else if(pHead->pLeft && pHead->cost > 0){//vector<Vector2D> forward_path;//TravesePathTreeForwards(pHead->pLeft, forward_path, FORWARD_RIGHT);//localPaths.push_back(forward_path);cout << "Global Lane Change  Right " << endl;TraversePathTreeBackwards(pHead->pLeft,pStartWP, globalPathIds, localPath, localPaths);pHead->bDir = FORWARD_RIGHT_DIR;localPath.push_back(*pHead);}else if(pHead->pRight && pHead->cost > 0){//vector<Vector2D> forward_path;//TravesePathTreeForwards(pHead->pRight, forward_path, FORWARD_LEFT);//localPaths.push_back(forward_path);cout << "Global Lane Change  Left " << endl;TraversePathTreeBackwards(pHead->pRight,pStartWP, globalPathIds, localPath, localPaths);pHead->bDir = FORWARD_LEFT_DIR;localPath.push_back(*pHead);}
//      else
//          cout << "Err: PlannerZ -> NULL Back Pointer " << pHead;}elseassert(pHead);
}

笔者实际测试结果和log：

以上就是global planner里的laneChange功能分析，以及它是如何影响global planner工作的。下篇会继续分析local planner里的laneChange功能。