车道偏离自动回正功能_车道偏离自动回正功能

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是如何规划路径并变道的，就必须先了解它是如何组织构建地图信息的。

基本道路元素：

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

上图是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;，这导致有些地方可以从右向左变道但是不能从左向右变道，更改过后立刻就可以了。