| | |
| | | |
| | | using namespace std; |
| | | |
| | | #define MAP_TYPE_PARK_BUTTOM 1 |
| | | #define MAP_TYPE_STOP_START 2 |
| | | #define MAP_TYPE_PART_EDGE 3 |
| | | #define MAP_TYPE_CURVE 4 |
| | | #define MAP_TYPE_TURN_90 5 |
| | | |
| | | #define RTK_INVALID 0 |
| | | #define RTK_SINGLE_POINT 1 |
| | | #define RTK_DIFF 2 |
| | |
| | | static void UpdateCarBodyCoord(struct RtkTime *rtkTime, double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel); |
| | | static bool UpdateCarCoord(double &spd, int &mov, int &idx); |
| | | |
| | | static int EnterMap(const car_model *car, const struct map_list *mapList, int mapNum); |
| | | static bool CrashTriggerLine(Line triggerLine, const car_model *car, LIST_CAR_MODEL &CarModelList); |
| | | static int EnterMap(const car_model *car, LIST_CAR_MODEL &CarModelList, const struct map_list *mapList, int mapNum); |
| | | static bool ExitMap(const car_model *car, int mapId, const struct map_list *mapList, int mapNum); |
| | | static int GetMapId(int index, const struct map_list *mapList, int mapNum); |
| | | static int GetMapType(int index, const struct map_list *mapList, int mapNum); |
| | |
| | | RtkBuffer[index].mm, RtkBuffer[index].ss, RtkBuffer[index].dss); |
| | | |
| | | brief.qf = RtkBuffer[index].qf; |
| | | brief.map_id = GetMapId(CurrExamMapIndex, MapList, MapNum); |
| | | brief.map_id = -1;//GetMapId(CurrExamMapIndex, MapList, MapNum); |
| | | brief.move = move; |
| | | brief.speed = speed * 3.6; |
| | | brief.heading = RtkBuffer[index].heading; |
| | |
| | | } |
| | | if (CurrExamMapIndex < 0) { |
| | | if (CurrEnterMapIndex < 0) { |
| | | CurrEnterMapIndex = EnterMap(CarModel, MapList, MapNum); |
| | | CurrEnterMapIndex = EnterMap(CarModel, CarModelList, MapList, MapNum); |
| | | if (CurrEnterMapIndex >= 0) { |
| | | DEBUG("发送进入场地报告 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum)); |
| | | MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 1); |
| | | // DEBUG("发送进入场地报告 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum)); |
| | | // MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 1); |
| | | |
| | | CurrExamMapIndex = CurrEnterMapIndex; |
| | | CurrExamStatus = EXAM_AREA_START; |
| | | } |
| | | } else { |
| | | if (ExitMap(CarModel, CurrEnterMapIndex, MapList, MapNum)) { |
| | | DEBUG("发送离开场地报告 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum)); |
| | | MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 0); |
| | | // DEBUG("发送离开场地报告 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum)); |
| | | // MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 0); |
| | | CurrEnterMapIndex = -1; |
| | | } |
| | | } |
| | |
| | | DEBUG("进入倒车入库场地"); |
| | | MA_SendDebugInfo("进入倒车入库场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); |
| | | |
| | | StartParkBottom(move, rtkTime); |
| | | StartParkBottom(CurrExamMapIndex, move, rtkTime); |
| | | CurrExamStatus = EXAM_AREA_RUN; |
| | | break; |
| | | case MAP_TYPE_STOP_START: |
| | | DEBUG("进入上坡起步场地"); |
| | | MA_SendDebugInfo("进入上坡起步场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); |
| | | StartSAS(move, rtkTime); |
| | | StartSAS(CurrExamMapIndex, move, rtkTime); |
| | | CurrExamStatus = EXAM_AREA_RUN; |
| | | break; |
| | | case MAP_TYPE_PART_EDGE: |
| | | DEBUG("进入侧方位停车场地"); |
| | | MA_SendDebugInfo("进入侧方位停车场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); |
| | | StartParkEdge(move, rtkTime); |
| | | StartParkEdge(CurrExamMapIndex, move, rtkTime); |
| | | |
| | | CurrExamStatus = EXAM_AREA_RUN; |
| | | break; |
| | | case MAP_TYPE_CURVE: |
| | | DEBUG("进入曲线行驶场地"); |
| | | MA_SendDebugInfo("进入曲线行驶场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); |
| | | StartDrivingCurve(move, rtkTime); |
| | | StartDrivingCurve(CurrExamMapIndex, move, rtkTime); |
| | | |
| | | CurrExamStatus = EXAM_AREA_RUN; |
| | | break; |
| | | case MAP_TYPE_TURN_90: |
| | | DEBUG("进入直角转弯场地"); |
| | | MA_SendDebugInfo("进入直角转弯场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); |
| | | StartTurnA90(move, azimuth, rtkTime); |
| | | StartTurnA90(CurrExamMapIndex, move, azimuth, rtkTime); |
| | | CurrExamStatus = EXAM_AREA_RUN; |
| | | break; |
| | | default:break; |
| | |
| | | return true; |
| | | } |
| | | |
| | | static int EnterMap(const car_model *car, const struct map_list *mapList, int mapNum) |
| | | static bool CrashTriggerLine(Line triggerLine, const car_model *car, LIST_CAR_MODEL &CarModelList) |
| | | { |
| | | // 车的最前点是否进入地图 |
| | | bool trigger = false; |
| | | |
| | | if (CarModelList.size() < 5) |
| | | return trigger; |
| | | |
| | | Polygon trace, trace2; |
| | | int pn = 0; |
| | | |
| | | trace2.num = trace.num = 5; |
| | | trace.point = (PointF *) malloc(sizeof(PointF) * trace.num); |
| | | trace2.point = (PointF *) malloc(sizeof(PointF) * trace2.num); |
| | | |
| | | list<car_model *>::iterator iter = CarModelList.begin(); |
| | | |
| | | car_model *c1 = *iter; |
| | | |
| | | trace.point[pn] = c1->carXY[c1->left_front_tire[TIRE_OUTSIDE]]; |
| | | trace2.point[pn++] = c1->carXY[c1->left_rear_tire[TIRE_OUTSIDE]]; |
| | | |
| | | ++iter; |
| | | |
| | | while (iter != CarModelList.end() && pn < trace.num) { |
| | | car_model *c2 = *iter; |
| | | |
| | | uint32_t tdiff = TimeGetDiff(c1->tm.hh, c1->tm.mm, c1->tm.ss, c1->tm.mss * 10, c2->tm.hh, c2->tm.mm, c2->tm.ss, c2->tm.mss*10); |
| | | |
| | | if (tdiff >= D_SEC(1)) { |
| | | trace.point[pn] = c2->carXY[c2->left_front_tire[TIRE_OUTSIDE]]; |
| | | trace2.point[pn++] = c2->carXY[c2->left_rear_tire[TIRE_OUTSIDE]]; |
| | | c1 = c2; |
| | | } |
| | | ++iter; |
| | | } |
| | | |
| | | PointF p1, p2; |
| | | |
| | | p1.X = triggerLine.X1; |
| | | p1.Y = triggerLine.Y1; |
| | | p2.X = triggerLine.X2; |
| | | p2.Y = triggerLine.Y2; |
| | | |
| | | int pp = 0; |
| | | for (int p = 1; p < pn; ++p) { |
| | | Line trace_line, trace2_line; |
| | | |
| | | MakeLine(&trace_line, &trace.point[pp], &trace.point[p]); |
| | | MakeLine(&trace2_line, &trace2.point[pp], &trace2.point[p]); |
| | | |
| | | if ((IntersectionOf(trace_line, triggerLine) == GM_Intersection || IntersectionOf(trace2_line, triggerLine) == GM_Intersection) && |
| | | IntersectionOfLine(p1, p2, car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1 && |
| | | DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], triggerLine) > 0.1) { |
| | | // 碰到触发线 |
| | | DEBUG("碰撞触发线 引发地图"); |
| | | trigger = true; |
| | | goto SEARCH_TRIGGER_LINE_END; |
| | | } |
| | | } |
| | | |
| | | SEARCH_TRIGGER_LINE_END: |
| | | free(trace.point); |
| | | |
| | | return trigger; |
| | | } |
| | | |
| | | static int EnterMap(const car_model *car, LIST_CAR_MODEL &CarModelList, const struct map_list *mapList, int mapNum) |
| | | { |
| | | for (int i = 0; i < mapNum && car != NULL; ++i) { |
| | | // 车前轮或后轮轨迹越过触发线 |
| | | if (mapList[i].type == MAP_TYPE_STOP_START) { |
| | | // 构造虚拟的左上角点 |
| | | double x9, y9, xo, yo; |
| | | |
| | | bool enter = false; |
| | | |
| | | xo = (mapList[i].map.point[0].X + mapList[i].map.point[7].X) / 2; |
| | | yo = (mapList[i].map.point[0].Y + mapList[i].map.point[7].Y) / 2; |
| | |
| | | x9 = 2*xo - mapList[i].map.point[8].X; |
| | | y9 = 2*yo - mapList[i].map.point[8].Y; |
| | | |
| | | Polygon map; |
| | | Line triggerLine; |
| | | |
| | | map.num = 4; |
| | | map.point = (PointF *) malloc(map.num * sizeof(PointF)); |
| | | triggerLine.X1 = mapList[i].map.point[0].X; |
| | | triggerLine.Y1 = mapList[i].map.point[0].Y; |
| | | triggerLine.X2 = x9; |
| | | triggerLine.Y2 = y9; |
| | | |
| | | map.point[0] = mapList[i].map.point[0]; |
| | | map.point[1] = mapList[i].map.point[8]; |
| | | map.point[2] = mapList[i].map.point[7]; |
| | | map.point[3].X = x9; |
| | | map.point[3].Y = y9; |
| | | |
| | | if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &map) == GM_Containment) { |
| | | Line enterLine1; |
| | | |
| | | MakeLine(&enterLine1, &(map.point[0]), &(map.point[3])); |
| | | |
| | | if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1) |
| | | enter = true; |
| | | } |
| | | |
| | | free(map.point); |
| | | |
| | | if (enter) return i; |
| | | if (CrashTriggerLine(triggerLine, car, CarModelList)) |
| | | return i; |
| | | } |
| | | if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) { |
| | | // 车头顶点在场地内 |
| | | if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) { |
| | | Line enterLine1, enterLine2; |
| | | |
| | |
| | | } |
| | | } |
| | | if (mapList[i].type == MAP_TYPE_PART_EDGE) { |
| | | // 车头顶点在场地内 |
| | | if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) { |
| | | Line enterLine; |
| | | |
| | |
| | | } |
| | | } |
| | | if (mapList[i].type == MAP_TYPE_TURN_90) { |
| | | if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) { |
| | | Line enterLine1; |
| | | // 车前轮或后轮轨迹越过触发线 |
| | | Line triggerLine; |
| | | |
| | | MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1])); |
| | | MakeLine(&triggerLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1])); |
| | | |
| | | if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1) |
| | | return i; |
| | | } |
| | | if (CrashTriggerLine(triggerLine, car, CarModelList)) |
| | | return i; |
| | | } |
| | | if (mapList[i].type == MAP_TYPE_CURVE) { |
| | | Line startLine; |
| | | Line carAxial; |
| | | Line triggerLine; |
| | | |
| | | MakeLine(&startLine, &mapList[i].map.point[0], &mapList[i].map2.point[0]); |
| | | MakeLine(&carAxial, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]]); |
| | | |
| | | if (IntersectionOf(startLine, carAxial) == GM_Intersection) { |
| | | if (DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], startLine) > 0.1) |
| | | return i; |
| | | } |
| | | MakeLine(&triggerLine, &mapList[i].map2.point[0], &mapList[i].map.point[0]); |
| | | if (CrashTriggerLine(triggerLine, car, CarModelList)) |
| | | return i; |
| | | } |
| | | } |
| | | return -1; |