lane_change_planner.cpp 6.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196
  1. #include <adc_planner/lane_change_planner.h>
  2. #include <adc_tools/transfer.h>
  3. #include <adc_tools/parameter_status.h>
  4. #include <common/constants.h>
  5. #include <math.h>
  6. iv::decition::LaneChangePlanner::LaneChangePlanner(){
  7. this->planner_id = 0;
  8. this->planner_name = "LaneChange";
  9. }
  10. iv::decition::LaneChangePlanner::~LaneChangePlanner(){
  11. }
  12. /**
  13. * @brief iv::decition::LaneChangePlanner::getPath
  14. * 采用的车道变换的局部路径生成方法。计算原始地图路径偏移offset距离之后的路径。
  15. *
  16. * @param now_gps_ins 实时gps信息
  17. * @param gpsMapLine 地图数据点
  18. * @param PathPoint 地图路线中距离车辆位置最近的一个点的序号
  19. * @param offset 在车辆避障中,地图源路线所在车道 与 即将去往的车道 之间的距离差值。可以为负值。
  20. * @param lidarGridPtr 激光雷达信息网格
  21. * @return 返回一条车辆坐标系下的路径
  22. */
  23. 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){
  24. std::vector<iv::Point2D> trace;
  25. trace = this->getGpsTrace(now_gps_ins,gpsMapLine,PathPoint);
  26. if(offset!=0){
  27. trace = this->getGpsTraceOffset(trace,offset);
  28. }
  29. return trace;
  30. }
  31. bool iv::decition::LaneChangePlanner::checkAvoidEnable(const std::vector<double>& obsDisVector, int roadAim){
  32. int roadNow = ServiceParameterStatus.now_road_num;
  33. if ((obsDisVector[roadAim]>0 && obsDisVector[roadAim]<obsDisVector[roadNow]+ServiceCarStatus.msysparam.vehLenth)||
  34. (obsDisVector[roadAim]>0 && obsDisVector[roadAim]<15))
  35. {
  36. return false;
  37. }
  38. if (roadAim-roadNow>1)
  39. {
  40. for (int i = roadNow+1; i < roadAim; i++)
  41. {
  42. if (obsDisVector[i] < 8 + (i-roadNow) * 2 && obsDisVector[i]>0) {
  43. return false;
  44. }
  45. }
  46. }
  47. else if (roadNow-roadAim>1)
  48. {
  49. for (int i = roadNow-1; i >roadAim; i--)
  50. {
  51. if (obsDisVector[i] < 8 + (roadNow-i) * 2 && obsDisVector[i]>0) {
  52. return false;
  53. }
  54. }
  55. }
  56. return true;
  57. }
  58. std::vector<iv::Point2D> iv::decition::LaneChangePlanner::getGpsTrace(iv::GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint){
  59. std::vector<iv::Point2D> trace;
  60. if (gpsMapLine.size() > 400 && PathPoint >= 0) {
  61. for (int i = PathPoint; i < PathPoint + 600; i++)
  62. {
  63. int index = i % gpsMapLine.size();
  64. Point2D pt = Coordinate_Transfer((*gpsMapLine[index]).gps_x, (*gpsMapLine[index]).gps_y, now_gps_ins);
  65. // pt.x += offset_real * 0.032;
  66. pt.v1 = (*gpsMapLine[index]).speed_mode;
  67. pt.v2 = (*gpsMapLine[index]).mode2;
  68. pt.roadMode=(*gpsMapLine[index]).roadMode;
  69. // if (gpsMapLine[index]->roadMode==3 && !hasZhuched)
  70. // {
  71. // readyZhucheMode = true;
  72. // DecideGps00::zhuchePointNum = index;
  73. // }
  74. // if (gpsMapLine[index]->roadMode==3 && !hasZhuched)
  75. // {
  76. // readyZhucheMode = true;
  77. // DecideGps00::zhuchePointNum = index;
  78. // }
  79. // //csvv7
  80. // if (gpsMapLine[index]->speed_mode==22 || gpsMapLine[index]->speed_mode==23)
  81. // {
  82. // readyParkMode = true;
  83. // DecideGps00::finishPointNum = index;
  84. // }
  85. switch (pt.v1)
  86. {
  87. case 0:
  88. pt.speed = 36;
  89. break;
  90. case 1:
  91. pt.speed = 25;
  92. break;
  93. case 2:
  94. pt.speed =25;
  95. break;
  96. case 3:
  97. pt.speed = 20;
  98. break;
  99. case 4:
  100. pt.speed =18;
  101. break;
  102. case 5:
  103. pt.speed = 18;
  104. break;
  105. case 7:
  106. pt.speed = 10;
  107. break;
  108. case 22:
  109. pt.speed = 5;
  110. break;
  111. case 23:
  112. pt.speed = 5;
  113. break;
  114. default:
  115. break;
  116. }
  117. trace.push_back(pt);
  118. }
  119. }
  120. return trace;
  121. }
  122. std::vector<iv::Point2D> iv::decition::LaneChangePlanner::getGpsTraceOffset(const std::vector<Point2D>& gpsTrace, double offset){
  123. if (offset==0)
  124. {
  125. return gpsTrace;
  126. }
  127. std::vector<iv::Point2D> trace;
  128. for (int j = 0; j < gpsTrace.size(); j++)
  129. {
  130. double sumx1 = 0, sumy1 = 0, count1 = 0;
  131. double sumx2 = 0, sumy2 = 0, count2 = 0;
  132. for (int k = std::max(0, j - 4); k <= j; k++)
  133. {
  134. count1 = count1 + 1;
  135. sumx1 += gpsTrace[k].x;
  136. sumy1 += gpsTrace[k].y;
  137. }
  138. for (int k = j; k <= std::min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
  139. {
  140. count2 = count2 + 1;
  141. sumx2 += gpsTrace[k].x;
  142. sumy2 += gpsTrace[k].y;
  143. }
  144. sumx1 /= count1; sumy1 /= count1;
  145. sumx2 /= count2; sumy2 /= count2;
  146. double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
  147. double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
  148. double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
  149. double avoidLenth = abs(offset);
  150. if (offset<0)
  151. {
  152. Point2D ptLeft(carFrontx + avoidLenth * cos(anglevalue + M_PI / 2),
  153. carFronty + avoidLenth * sin(anglevalue + M_PI / 2));
  154. ptLeft.speed = gpsTrace[j].speed;
  155. ptLeft.v1 = gpsTrace[j].v1;
  156. ptLeft.v2 = gpsTrace[j].v2;
  157. ptLeft.roadMode = gpsTrace[j].roadMode;
  158. trace.push_back(ptLeft);
  159. }
  160. else
  161. {
  162. Point2D ptRight(carFrontx + avoidLenth * cos(anglevalue - M_PI / 2),
  163. carFronty + avoidLenth * sin(anglevalue - M_PI / 2));
  164. ptRight.speed = gpsTrace[j].speed;
  165. ptRight.v1 = gpsTrace[j].v1;
  166. ptRight.v2 = gpsTrace[j].v2;
  167. ptRight.roadMode = gpsTrace[j].roadMode;
  168. trace.push_back(ptRight);
  169. }
  170. }
  171. return trace;
  172. }