123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196 |
- #include <adc_planner/lane_change_planner.h>
- #include <adc_tools/transfer.h>
- #include <adc_tools/parameter_status.h>
- #include <common/constants.h>
- #include <math.h>
- iv::decition::LaneChangePlanner::LaneChangePlanner(){
- this->planner_id = 0;
- this->planner_name = "LaneChange";
- }
- iv::decition::LaneChangePlanner::~LaneChangePlanner(){
- }
- /**
- * @brief iv::decition::LaneChangePlanner::getPath
- * 采用的车道变换的局部路径生成方法。计算原始地图路径偏移offset距离之后的路径。
- *
- * @param now_gps_ins 实时gps信息
- * @param gpsMapLine 地图数据点
- * @param PathPoint 地图路线中距离车辆位置最近的一个点的序号
- * @param offset 在车辆避障中,地图源路线所在车道 与 即将去往的车道 之间的距离差值。可以为负值。
- * @param lidarGridPtr 激光雷达信息网格
- * @return 返回一条车辆坐标系下的路径
- */
- std::vector<iv::Point2D> iv::decition::LaneChangePlanner::getPath(GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint, double offset, iv::LidarGridPtr lidarGridPtr){
- std::vector<iv::Point2D> trace;
- trace = this->getGpsTrace(now_gps_ins,gpsMapLine,PathPoint);
- if(offset!=0){
- trace = this->getGpsTraceOffset(trace,offset);
- }
- return trace;
- }
- bool iv::decition::LaneChangePlanner::checkAvoidEnable(const std::vector<double>& obsDisVector, int roadAim){
- int roadNow = ServiceParameterStatus.now_road_num;
- if ((obsDisVector[roadAim]>0 && obsDisVector[roadAim]<obsDisVector[roadNow]+ServiceCarStatus.msysparam.vehLenth)||
- (obsDisVector[roadAim]>0 && obsDisVector[roadAim]<15))
- {
- return false;
- }
- if (roadAim-roadNow>1)
- {
- for (int i = roadNow+1; i < roadAim; i++)
- {
- if (obsDisVector[i] < 8 + (i-roadNow) * 2 && obsDisVector[i]>0) {
- return false;
- }
- }
- }
- else if (roadNow-roadAim>1)
- {
- for (int i = roadNow-1; i >roadAim; i--)
- {
- if (obsDisVector[i] < 8 + (roadNow-i) * 2 && obsDisVector[i]>0) {
- return false;
- }
- }
- }
- return true;
- }
- std::vector<iv::Point2D> iv::decition::LaneChangePlanner::getGpsTrace(iv::GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint){
- std::vector<iv::Point2D> trace;
- if (gpsMapLine.size() > 400 && PathPoint >= 0) {
- for (int i = PathPoint; i < PathPoint + 600; i++)
- {
- int index = i % gpsMapLine.size();
- Point2D pt = Coordinate_Transfer((*gpsMapLine[index]).gps_x, (*gpsMapLine[index]).gps_y, now_gps_ins);
- // pt.x += offset_real * 0.032;
- pt.v1 = (*gpsMapLine[index]).speed_mode;
- pt.v2 = (*gpsMapLine[index]).mode2;
- pt.roadMode=(*gpsMapLine[index]).roadMode;
- // if (gpsMapLine[index]->roadMode==3 && !hasZhuched)
- // {
- // readyZhucheMode = true;
- // DecideGps00::zhuchePointNum = index;
- // }
- // if (gpsMapLine[index]->roadMode==3 && !hasZhuched)
- // {
- // readyZhucheMode = true;
- // DecideGps00::zhuchePointNum = index;
- // }
- // //csvv7
- // if (gpsMapLine[index]->speed_mode==22 || gpsMapLine[index]->speed_mode==23)
- // {
- // readyParkMode = true;
- // DecideGps00::finishPointNum = index;
- // }
- switch (pt.v1)
- {
- case 0:
- pt.speed = 36;
- break;
- case 1:
- pt.speed = 25;
- break;
- case 2:
- pt.speed =25;
- break;
- case 3:
- pt.speed = 20;
- break;
- case 4:
- pt.speed =18;
- break;
- case 5:
- pt.speed = 18;
- break;
- case 7:
- pt.speed = 10;
- break;
- case 22:
- pt.speed = 5;
- break;
- case 23:
- pt.speed = 5;
- break;
- default:
- break;
- }
- trace.push_back(pt);
- }
- }
- return trace;
- }
- std::vector<iv::Point2D> iv::decition::LaneChangePlanner::getGpsTraceOffset(const std::vector<Point2D>& gpsTrace, double offset){
- if (offset==0)
- {
- return gpsTrace;
- }
- std::vector<iv::Point2D> trace;
- for (int j = 0; j < gpsTrace.size(); j++)
- {
- double sumx1 = 0, sumy1 = 0, count1 = 0;
- double sumx2 = 0, sumy2 = 0, count2 = 0;
- for (int k = std::max(0, j - 4); k <= j; k++)
- {
- count1 = count1 + 1;
- sumx1 += gpsTrace[k].x;
- sumy1 += gpsTrace[k].y;
- }
- for (int k = j; k <= std::min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
- {
- count2 = count2 + 1;
- sumx2 += gpsTrace[k].x;
- sumy2 += gpsTrace[k].y;
- }
- sumx1 /= count1; sumy1 /= count1;
- sumx2 /= count2; sumy2 /= count2;
- double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
- double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
- double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
- double avoidLenth = abs(offset);
- if (offset<0)
- {
- Point2D ptLeft(carFrontx + avoidLenth * cos(anglevalue + M_PI / 2),
- carFronty + avoidLenth * sin(anglevalue + M_PI / 2));
- ptLeft.speed = gpsTrace[j].speed;
- ptLeft.v1 = gpsTrace[j].v1;
- ptLeft.v2 = gpsTrace[j].v2;
- ptLeft.roadMode = gpsTrace[j].roadMode;
- trace.push_back(ptLeft);
- }
- else
- {
- Point2D ptRight(carFrontx + avoidLenth * cos(anglevalue - M_PI / 2),
- carFronty + avoidLenth * sin(anglevalue - M_PI / 2));
- ptRight.speed = gpsTrace[j].speed;
- ptRight.v1 = gpsTrace[j].v1;
- ptRight.v2 = gpsTrace[j].v2;
- ptRight.roadMode = gpsTrace[j].roadMode;
- trace.push_back(ptRight);
- }
- }
- return trace;
- }
|