yy1717
2024-02-28 27fc91fbe8f88b6885356e68828cfe1ce1db7601
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
//
// Created by YY on 2019/10/23.
//
//
// 0 ____________________________________7
//
//
// 1 _____________2     5_________________6
//              8|      |9
//               |      |
//               |      |
//               |      |
//              3--------4
//
 
#include "park_bottom.h"
#include "../common/apptimer.h"
#include "../test_common/Geometry.h"
#include "../native-lib.h"
#include "../jni_log.h"
#include "../driver_test.h"
#include "../utils/xconvert.h"
#include "../master/comm_if.h"
#include "area_exam.h"
#include "../test_common/car_sensor.h"
#include "../test_common/odo_graph.h"
#include "../teach/train.h"
#include "../teach/teach.h"
#include <vector>
#include <cstdlib>
#include <tuple>
 
#define DEBUG(fmt, args...)     LOGD("<park_bottom> <%s>: " fmt, __func__, ##args)
 
using namespace std;
 
typedef enum {
    NONE_CTRL_LINE,
    LEFT_CTRL_LINE,
    RIGHT_CTRL_LINE
} ctrl_line_t;
 
static bool occurCrashRedLine;
static int parkCount;
static int reverseCnt = 0;
static bool stopFlag;
static ctrl_line_t prevCrossedCtrlLine;
 
static bool checkCtrlLine, checkParkspace;
static int handleGearOb, handleMoveOb;
 
static bool BodyCollidingLine(prime_t &prime);
static ctrl_line_t CrossCtrlLine(prime_t &prime);
static bool CheckParkspace(prime_t &prime);
static void MoveOb(move_status_t curr, move_status_t prev, double distance);
static void GearOb(int gear, int prevGear, double move_distance);
 
enum {
    PREPARE_REVERSE,
    REVERSE_LEFT,
    REVERSE_RIGHT,
    OBVERSE_LEFT,
    OBVERSE_RIGHT
};
 
/*
1. 进入 --- 首次倒库前
2. 倒库 ---- 停车到位前(右倒库)
3. 左出库
4. 倒库 ---- 停车到位前(左倒库)
5. 右出库(离开场地)
*/
 
void StartParkBottom(prime_t &prime)
{
    DEBUG("进入倒库场地");
    stopFlag = false;
    parkCount = 0;
    reverseCnt = 0;
    checkCtrlLine = false;
    checkParkspace = false;
    occurCrashRedLine = false;
    prime.examing_area.stage = PREPARE_REVERSE;
    testAll();
    LoadStageTips(prime);
    prevCrossedCtrlLine = NONE_CTRL_LINE;
    handleGearOb = RegisterShiftObserver(GearOb);
    handleMoveOb = RegisterCarMoveObserver(MoveOb);
    PlayTTS("您已进入倒车入库区域", NULL);
}
 
static void ParkTimeout(apptimer_var_t var)
{
    DEBUG("项目超时");
    AddExamFault(20105);
}
 
static void MoveOb(move_status_t curr, move_status_t prev, double distance)
{
    // 车辆由运动到停止,记录一下,当换挡发生时,避免原地来回换挡造成误判
    if (curr == STOP && distance > 0.5) {
        stopFlag = true;
    }
}
 
static void GearOb(int gear, int prevGear, double move_distance)
{
    DEBUG("GearOb %d %f", gear, move_distance);
 
    if (gear == GEAR_R && stopFlag) {   // 移入倒挡
        stopFlag = false;
        reverseCnt++;
        if (reverseCnt == 1) {
            AppTimer_delete(ParkTimeout);
            AppTimer_add(ParkTimeout, GetPrime().examParam.park_bottom_limit_time);
        }
        checkCtrlLine = true;
    } else if (prevGear == GEAR_R && stopFlag) {    // 从倒挡移出
        stopFlag = false;
        checkParkspace = true;
    }
}
 
void StopParkBottom(prime_t &prime)
{
    if (prime.examing_area.type != MAP_TYPE_PARK_BUTTOM)
        return;
 
    DEBUG("离开倒库场地");
 
    if (parkCount < 2) {
        DEBUG("直接驶离测试区,不按考试员指令驾驶");
        AddExamFault(10103);
    }
    AppTimer_delete(ParkTimeout);
    prime.examing_area.type = MAP_TYPE_NONE;
    UnregisterShiftObserver(handleGearOb);
    UnregisterCarMoveObserver(handleMoveOb);
}
 
void TestParkBottom(prime_t &prime)
{
    if (prime.examing_area.type != MAP_TYPE_PARK_BUTTOM)
        return;
 
    train(prime);
 
    if (checkCtrlLine) {
        ctrl_line_t crossCtrlLine = CrossCtrlLine(prime);
 
        if (crossCtrlLine == NONE_CTRL_LINE) {
            // 倒车前,前轮未驶过控制线
            DEBUG("倒车前,前轮未驶过控制线");
            AddExamFault(20104);
        } else if (crossCtrlLine == prevCrossedCtrlLine) {
            // 重复跨越同一控制线,不按规定线路,顺序形式,不合格
            AddExamFault(20101);
            DEBUG("不按规定线路,顺序形式, 同 %d 侧", prevCrossedCtrlLine);
        } else {
            prevCrossedCtrlLine = crossCtrlLine;
            DEBUG("开始 %d 侧 倒库", prevCrossedCtrlLine);
        }
 
        if (DeltaYaw(YawOf(std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[3],
                           std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[4]), prime.pModeling[prime.curr_modeling_index].yaw) < 90) {
            DEBUG("右侧倒库");
            prime.examing_area.stage = REVERSE_RIGHT;
        } else {
            DEBUG("左侧倒库");
            prime.examing_area.stage = REVERSE_LEFT;
        }
        LoadStageTips(prime);
        checkCtrlLine = false;
    }
 
    if (checkParkspace) {
        if (CheckParkspace(prime)) {
            DEBUG("倒库成功");
        } else {
            DEBUG("倒库不入");
            AddExamFault(20103);
        }
        parkCount++;
        if (prime.examing_area.stage == REVERSE_RIGHT) {
            prime.examing_area.stage = OBVERSE_LEFT;       // 从左侧倒库
        } else {
            prime.examing_area.stage = OBVERSE_RIGHT;      // 离开场地
        }
        LoadStageTips(prime);
        checkParkspace = false;
    }
 
    if (BodyCollidingLine(prime)) {
        if (!occurCrashRedLine) {
            occurCrashRedLine = true;
            // 车身出线,不合格
            AddExamFault(10116);
            DEBUG("车身出线");
        }
    } else {
        occurCrashRedLine = false;
    }
 
    teach(prime);
}
 
// 车身或车轮皆不得碰触实线
static bool BodyCollidingLine(prime_t &prime)
{
    MakePolygon car_body(prime.pModel->body.size());
 
    for (int i = 0; i < prime.pModel->body.size(); ++i) {
        car_body.AddPoint(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[i]]);
    }
 
    Line frontAxle, rearAxle;
 
    MAKE_LINE(frontAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]], prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]]);
    MAKE_LINE(rearAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]], prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]]);
 
    const int red_lines[][2] = {{0, 7}, {1, 2}, {2, 3}, {3, 4}, {4, 5}, {5, 6}};
 
    for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
        Line red_line;
        MAKE_LINE(red_line, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[red_lines[i][0]], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[red_lines[i][1]]);
        if (IntersectionOf(red_line, frontAxle) == GM_Intersection
            || IntersectionOf(red_line, rearAxle) == GM_Intersection
            || IntersectionOf(red_line, car_body.GetPolygon()) != GM_None) {
            return true;
        }
    }
    return false;
}
 
// 检测2前轮是否正向越过左右控制线
static ctrl_line_t CrossCtrlLine(prime_t &prime)
{
    // 过右控制线
    Line left_trace, right_trace;
    Line left_ctrl, right_ctrl;
 
    MAKE_LINE(left_trace, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
              prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]]);
    MAKE_LINE(right_trace, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]],
              prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]]);
 
    MAKE_LINE(left_ctrl, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[1], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[0]);
    MAKE_LINE(right_ctrl, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[6], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[7]);
 
 
    if (IntersectionOf(left_trace, left_ctrl) == GM_Intersection &&
            IntersectionOf(right_trace, left_ctrl) == GM_Intersection &&
            IntersectionOfLine(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]], left_ctrl) == REL_POS_LEFT) {
        return LEFT_CTRL_LINE;
    }
 
    if (IntersectionOf(left_trace, right_ctrl) == GM_Intersection &&
        IntersectionOf(right_trace, right_ctrl) == GM_Intersection &&
        IntersectionOfLine(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]], right_ctrl) == REL_POS_RIGHT) {
        return RIGHT_CTRL_LINE;
    }
 
    return NONE_CTRL_LINE;
}
 
static bool CheckParkspace(prime_t &prime) {
    MakePolygon car_body(prime.pModel->body.size());
 
    for (int i = 0; i < prime.pModel->body.size(); ++i) {
        car_body.AddPoint(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[i]]);
    }
 
    MakePolygon park_area({std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[8],
                           std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[3],
                           std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[4],
                           std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[9]});
 
    return (bool) (IntersectionOf(car_body.GetPolygon(), park_area.GetPolygon()) == GM_Containment);
}