近期一直使用lgsvl测试autoware的各种功能，前期使用autoware1.14+OpenPlanner_v1，测试期间发现OpenPlanner_v1可以正常生成local trajactories，但local trajactory的evalutor和selector部分有问题，大部分时刻没有做出正确的选择，且检测到障碍物的trajactory没有正常被Block掉。因此升级使用Autoware1.15 + OpenPlanner2.5。
Autoware1.15 + OpenPlanner2.5地址：
https://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。具体解析参考这个博客：
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*s
double 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);
else
pLaneCell = 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);
else
pLaneCell = 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;
}
else
assert(pHead);
}笔者实际测试结果和log：
以上就是global planner里的laneChange功能分析，以及它是如何影响global planner工作的。下篇会继续分析local planner里的laneChange功能。
413
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
