From 27fc91fbe8f88b6885356e68828cfe1ce1db7601 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期三, 28 二月 2024 09:53:13 +0800
Subject: [PATCH] 坐标
---
lib/src/main/cpp/test_items/area_exam.cpp | 463 ++++++++++++++++++++++++++++++++++++++++++++-------------
1 files changed, 353 insertions(+), 110 deletions(-)
diff --git a/lib/src/main/cpp/test_items/area_exam.cpp b/lib/src/main/cpp/test_items/area_exam.cpp
index bed7a7c..a005b7f 100644
--- a/lib/src/main/cpp/test_items/area_exam.cpp
+++ b/lib/src/main/cpp/test_items/area_exam.cpp
@@ -3,65 +3,158 @@
//
#include <cstdlib>
+#include <tuple>
+#include <map>
+
#include "area_exam.h"
#include "../test_common/car_sensor.h"
#include "../driver_test.h"
#include "../jni_log.h"
#include "park_bottom.h"
-#include "stop_and_start.h"
+#include "uphill.h"
#include "park_edge.h"
#include "driving_curve.h"
-#include "turn_a90.h"
+#include "right_corner.h"
#include "../utils/xconvert.h"
#include "../common/apptimer.h"
#include "../test_common/odo_graph.h"
#include "../test_common/Geometry.h"
#include "../common/observer.h"
+#include "../native-lib.h"
#define DEBUG(fmt, args...) LOGD("<area_exam> <%s>: " fmt, __func__, ##args)
-ilovers::Observer<std::function<void(move_status_t)>> CarMoveEvent;
+enum class ProximityStatus {
+ Near,
+ Bounce,
+ Far
+};
-static bool ProximityArea(Line &base_line, Line &line);
+ilovers::Observer<std::function<void(move_status_t, move_status_t, double)>> CarMoveEvent;
+ilovers::Observer<std::function<void(int, int, double)>> ShiftEvent;
+
+static void CheckProximity(prime_t &prime, Line &base_line, Line &line, int this_id);
static bool CrossingStartLine(Line &trace, Line &start_line);
static void ProximityReminders(prime_t &prime);
static void DetectCarMove(prime_t &prime);
+static void DetectShift(prime_t &prime);
+static void EnterMap(prime_t &prime);
+static void ExitMap(prime_t &prime);
void AreaExam(prime_t &prime)
{
- switch (prime.curr_exam_map.type) {
- case MAP_TYPE_PARK_BUTTOM:
- break;
- case MAP_TYPE_PARK_EDGE:
- break;
- case MAP_TYPE_TURN_90:
- break;
- case MAP_TYPE_UPHILL:
- break;
- case MAP_TYPE_CURVE:
- break;
- default:
- if (EnterMap()) {
-
- }
- break;
+ if (prime.prev_modeling_index == -1 || prime.curr_modeling_index == -1) {
+ return;
}
+ DEBUG("--------------> %d", __LINE__);
+ DetectShift(prime);
+ DEBUG("--------------> %d", __LINE__);
DetectCarMove(prime);
+ DEBUG("--------------> %d", __LINE__);
+ ProximityReminders(prime);
+ DEBUG("--------------> %d", __LINE__);
+ EnterMap(prime);
+ DEBUG("--------------> %d", __LINE__);
+ TestDrivingCurve(prime);
+ DEBUG("--------------> %d", __LINE__);
+ TestParkBottom(prime);
+ DEBUG("--------------> %d", __LINE__);
+ TestParkEdge(prime);
+ DEBUG("--------------> %d", __LINE__);
+ TestUphill(prime);
+ DEBUG("--------------> %d", __LINE__);
+ TestRightCorner(prime);
+ DEBUG("--------------> %d", __LINE__);
+ ExitMap(prime);
+ DEBUG("--------------> %d", __LINE__);
}
+// 妫�鏌ユ崲鎸★紝濡傚仠杞﹀叆搴撱�佸�掕溅寮�濮嬬瓑鍔ㄤ綔锛岄渶瑕佸湪鎸′綅鍙樻崲鏃舵娴嬶紝鍚屾椂闇�鑰冭檻椹鹃┒鍛樹竴娆℃崲鎸′笉杩涳紝鍋滃湪鍘熷湴鍐嶆鎿嶄綔鐨勬儏鍐�
+static void DetectShift(prime_t &prime)
+{
+ static int prevGear = prime.sensor.gear;
+ static double odo = prime.odo; // 璁板綍杩欎釜鎸′綅涓嬭椹剁殑璺濈
+
+ if (prime.sensor.gear != prevGear) {
+ if (prime.odo - odo > 1) {
+
+ }
+ ShiftEvent.Notify(prime.sensor.gear, prevGear, prime.odo - odo);
+ prevGear = prime.sensor.gear;
+ odo = prime.odo;
+ }
+}
+
+static bool stopTimeout = false;
+
+static void StoppedTimeout(apptimer_var_t val) {
+ stopTimeout = true;
+}
+
+// 澶勭悊鍚勪釜椤圭洰鐨勮秴鏃跺仠杞︼紙鍋滀笅鏃跺拰鍐嶆鍚姩鏃讹紝鎸′綅鏄竴鑷寸殑锛�
static void DetectCarMove(prime_t &prime)
{
- static move_status_t prevMove = STOP;
+ static double odo = prime.odo;
+ static int gearWhenStop = GEAR_N;
+ static move_status_t prevMove = prime.pMotion->move;
if (prime.pMotion->move != prevMove) {
// Notify
- CarMoveEvent.Notify(prime.pMotion->move);
+ DEBUG("琛岄┒鐘舵�佹敼鍙� %d", prime.pMotion->move);
+ CarMoveEvent.Notify(prime.pMotion->move, prevMove, prime.odo - odo);
+
+ AppTimer_delete(StoppedTimeout);
+ if (prime.pMotion->move == STOP) {
+ gearWhenStop = prime.sensor.gear;
+ switch (prime.examing_area.type) {
+ case MAP_TYPE_PARK_BUTTOM:
+ AppTimer_add(StoppedTimeout, prime.examParam.park_bottom_pause_criteria);
+ break;
+ case MAP_TYPE_PARK_EDGE:
+ AppTimer_add(StoppedTimeout, prime.examParam.park_edge_pause_criteria);
+ break;
+ case MAP_TYPE_RIGHT_CORNER:
+ AppTimer_add(StoppedTimeout, prime.examParam.turn_a90_pause_criteria);
+ break;
+ case MAP_TYPE_CURVE:
+ AppTimer_add(StoppedTimeout, prime.examParam.curve_pause_criteria);
+ break;
+ case MAP_TYPE_UPHILL:
+ break;
+ default:break;
+ }
+ } else {
+ if (stopTimeout && ((prime.sensor.gear == GEAR_R) == (gearWhenStop == GEAR_R))) {
+ // 鍋滆溅瓒呮椂
+ switch (prime.examing_area.type) {
+ case MAP_TYPE_PARK_BUTTOM:
+ // 鍋滆溅瓒�2绉掞紝姣忔鎵�5鍒�
+ AddExamFault(20106);
+ break;
+ case MAP_TYPE_PARK_EDGE:
+ AddExamFault(20406);
+ break;
+ case MAP_TYPE_RIGHT_CORNER:
+ AddExamFault(20703);
+ break;
+ case MAP_TYPE_CURVE:
+ // 鍋滆溅瓒�2绉掞紝涓嶅悎鏍�
+ AddExamFault(20602);
+ break;
+ case MAP_TYPE_UPHILL:
+ break;
+ default:break;
+ }
+ }
+ }
+ stopTimeout = false;
prevMove = prime.pMotion->move;
+ odo = prime.odo;
}
}
-void RegisterCarMoveObserver(void (*ptr)(move_status_t))
+int RegisterCarMoveObserver(std::function<void(move_status_t, move_status_t, double)> ob)
{
- CarMoveEvent.Connect(ptr);
+ return CarMoveEvent.Connect(ob);
}
void UnregisterCarMoveObserver(int handle)
@@ -69,163 +162,313 @@
CarMoveEvent.Disconnect(handle);
}
-void EnterMap(prime_t &prime)
+int RegisterShiftObserver(std::function<void(int, int, double)> ob)
{
- if (prime.curr_exam_map.type != 0) {
+ return ShiftEvent.Connect(ob);
+}
+
+void UnregisterShiftObserver(int handle)
+{
+ ShiftEvent.Disconnect(handle);
+}
+
+static void EnterMap(prime_t &prime)
+{
+ if (prime.examing_area.type != 0) {
return;
}
- if (prime.prev_modeling_index == -1 || prime.curr_modeling_index == -1) {
- return;
- }
+ DEBUG("--------------> %d", __LINE__);
- PointF &lp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
- PointF &lp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
- PointF &rp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];
- PointF &rp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];
+ DEBUG("1. %d", prime.pModeling[prime.curr_modeling_index].points.size());
+ DEBUG("2. %d", prime.pModeling[prime.prev_modeling_index].points.size());
+
+ DEBUG("3. %d", prime.pModel->left_front_tire[TIRE_OUTSIDE]);
+ DEBUG("4. %d", prime.pModel->right_front_tire[TIRE_OUTSIDE]);
+
+
+ PointF lp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
+ PointF lp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
+ PointF rp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];
+ PointF rp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];
Line left_trace, right_trace;
-
+ DEBUG("--------------> %d", __LINE__);
MAKE_LINE(left_trace, lp1, lp2);
MAKE_LINE(right_trace, rp1, rp2);
Line start_line;
- for (int i = 0; i < prime.pMap->park_button_map.size(); ++i) {
- MAKE_LINE(start_line, prime.pMap->park_button_map[i].map[1], prime.pMap->park_button_map[i].map[0]);
+ DEBUG("鍊掑簱> %d", std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps).size());
+ for (int i = 0; i < std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps).size(); ++i) {
+ MAKE_LINE(start_line, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[i].points[1], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[i].points[0]);
if (CrossingStartLine(left_trace, start_line)) {
- prime.curr_exam_map.type = MAP_TYPE_PARK_BUTTOM;
- prime.curr_exam_map.map_idx = i;
+ prime.examing_area.type = MAP_TYPE_PARK_BUTTOM;
+ prime.examing_area.idx = i;
StartParkBottom(prime);
return;
}
- MAKE_LINE(start_line, prime.pMap->park_button_map[i].map[7], prime.pMap->park_button_map[i].map[6]);
+ MAKE_LINE(start_line, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[i].points[7], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[i].points[6]);
if (CrossingStartLine(left_trace, start_line)) {
- prime.curr_exam_map.type = MAP_TYPE_PARK_BUTTOM;
- prime.curr_exam_map.map_idx = i;
+ prime.examing_area.type = MAP_TYPE_PARK_BUTTOM;
+ prime.examing_area.idx = i;
StartParkBottom(prime);
return;
}
}
-
- for (int i = 0; i < prime.pMap->park_edge_map.size(); ++i) {
- MAKE_LINE(start_line, prime.pMap->park_edge_map[i].map[1], prime.pMap->park_edge_map[i].map[0]);
+ DEBUG("渚ф柟> %d", std::get<MAP_TYPE_PARK_EDGE>(prime.maps).size());
+ for (int i = 0; i < std::get<MAP_TYPE_PARK_EDGE>(prime.maps).size(); ++i) {
+ MAKE_LINE(start_line, std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[i].points[1], std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[i].points[0]);
if (CrossingStartLine(left_trace, start_line)) {
- prime.curr_exam_map.type = MAP_TYPE_PARK_EDGE;
- prime.curr_exam_map.map_idx = i;
+ prime.examing_area.type = MAP_TYPE_PARK_EDGE;
+ prime.examing_area.idx = i;
+ StartParkEdge(prime);
return;
}
+ }
+ DEBUG("涓婂潯> %d", std::get<MAP_TYPE_UPHILL>(prime.maps).size());
+ for (int i = 0; i < std::get<MAP_TYPE_UPHILL>(prime.maps).size(); ++i) {
+ MAKE_LINE(start_line, std::get<MAP_TYPE_UPHILL>(prime.maps)[i].points[0], std::get<MAP_TYPE_UPHILL>(prime.maps)[i].points[10]);
+ if (CrossingStartLine(left_trace, start_line)) {
+ prime.examing_area.type = MAP_TYPE_UPHILL;
+ prime.examing_area.idx = i;
+ StartUphill(prime);
+ return;
+ }
+ }
+ DEBUG("鏇茬嚎> %d", std::get<MAP_TYPE_CURVE>(prime.maps).size());
+ for (int i = 0; i < std::get<MAP_TYPE_CURVE>(prime.maps).size(); ++i) {
+ MAKE_LINE(start_line, std::get<MAP_TYPE_CURVE>(prime.maps)[i].right_start_point, std::get<MAP_TYPE_CURVE>(prime.maps)[i].left_start_point);
+ if (CrossingStartLine(left_trace, start_line)) {
+ prime.examing_area.type = MAP_TYPE_CURVE;
+ prime.examing_area.idx = i;
+ StartDrivingCurve(prime);
+ return;
+ }
+ }
+ DEBUG("鐩磋> %d", std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps).size());
+ for (int i = 0; i < std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps).size(); ++i) {
+ MAKE_LINE(start_line, std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[i].points[0], std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[i].points[1]);
+ if (CrossingStartLine(left_trace, start_line)) {
+ prime.examing_area.type = MAP_TYPE_RIGHT_CORNER;
+ prime.examing_area.idx = i;
+ StartRightCorner(prime);
+ return;
+ }
+ }
+}
+
+/****************************************
+ * 閫�鍑哄満鍦帮細鏉′欢鏄溅澶村拰鍥涜疆閮戒笉鍦ㄥ満鍦颁腑
+ * 濡傛灉褰撳墠娴嬭瘯椤圭洰鏈畬鎴愶紝瑕佹姤涓嶅悎鏍�
+ * @param prime
+ */
+static void ExitMap(prime_t &prime)
+{
+ if (prime.examing_area.type == MAP_TYPE_NONE) {
+ return;
}
- for (int i = 0; i < prime.pMap->uphill_map.size(); ++i) {
- PointF vPoint = Calc3Point(prime.pMap->uphill_map[i].map[8], prime.pMap->uphill_map[i].map[0], DistanceOf(prime.pMap->uphill_map[i].map[8], prime.pMap->uphill_map[i].map[7]), 'R');
- MAKE_LINE(start_line, prime.pMap->uphill_map[i].map[0], vPoint);
- if (CrossingStartLine(left_trace, start_line)) {
- prime.curr_exam_map.type = MAP_TYPE_UPHILL;
- prime.curr_exam_map.map_idx = i;
- return;
+ if (prime.examing_area.type == MAP_TYPE_CURVE) {
+ // 杞︾殑鍚庤疆鏄惁瓒婅繃缁撴潫绾挎垨鏄溅杈嗕笉鍦�2涓ぇ鍦嗚寖鍥村唴
+ PointF lp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
+ PointF lp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
+ PointF rp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];
+ PointF rp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];
+
+ Line left_trace, right_trace;
+
+ MAKE_LINE(left_trace, lp1, lp2);
+ MAKE_LINE(right_trace, rp1, rp2);
+
+ Line end_line;
+ MAKE_LINE(end_line, std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].right_end_point, std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].left_end_point);
+ if (CrossingStartLine(left_trace, end_line)) {
+ // 绂诲紑鍖哄煙
+ StopDrivingCurve(prime);
}
- }
- for (int i = 0; i < prime.pMap->curve_map.size(); ++i) {
- MAKE_LINE(start_line, prime.pMap->curve_map[i].right_start_point, prime.pMap->curve_map[i].left_start_point);
- if (CrossingStartLine(left_trace, start_line)) {
- prime.curr_exam_map.type = MAP_TYPE_CURVE;
- prime.curr_exam_map.map_idx = i;
- return;
+
+ if (DistanceOf(prime.pModeling[prime.curr_modeling_index].base_point, std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].front_half_big_circle_centre) > std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].front_half_big_circle_radius
+ && DistanceOf(prime.pModeling[prime.curr_modeling_index].base_point, std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].back_half_big_circle_centre) > std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].back_half_big_circle_radius) {
+ // 绂诲紑鍖哄煙
+ StopDrivingCurve(prime);
}
- }
- for (int i = 0; i < prime.pMap->turn_a90_map.size(); ++i) {
- MAKE_LINE(start_line, prime.pMap->turn_a90_map[i].map[0], prime.pMap->turn_a90_map[i].map[1]);
- if (CrossingStartLine(left_trace, start_line)) {
- prime.curr_exam_map.type = MAP_TYPE_TURN_90;
- prime.curr_exam_map.map_idx = i;
- return;
+ } else {
+ MakePolygon area({});
+ DEBUG("------------> %d", __LINE__);
+ switch (prime.examing_area.type) {
+ case MAP_TYPE_PARK_BUTTOM:
+ area.AddPoints({std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[0],
+ 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[2],
+ 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[5],
+ 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]});
+ break;
+ case MAP_TYPE_PARK_EDGE:
+ area.AddPoints({std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[0],
+ std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[1],
+ std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[2],
+ std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[3],
+ std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[4],
+ std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[5],
+ std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[6],
+ std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[7]});
+ break;
+ case MAP_TYPE_UPHILL:
+ area.AddPoints({std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[0],
+ std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[9],
+ std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[11],
+ std::get<MAP_TYPE_UPHILL>(prime.maps)[prime.examing_area.idx].points[10]});
+ break;
+ case MAP_TYPE_RIGHT_CORNER:
+ area.AddPoints({std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[prime.examing_area.idx].points[0],
+ std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[prime.examing_area.idx].points[1],
+ std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[prime.examing_area.idx].points[2],
+ std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[prime.examing_area.idx].points[3],
+ std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[prime.examing_area.idx].points[4],
+ std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[prime.examing_area.idx].points[5]});
+ break;
+ default:
+ break;
}
+
+ DEBUG("------------> %d", __LINE__);
+ int num = 0;
+
+ if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
+ area.GetPolygon()) == GM_None) {
+ num++;
+ }
+ if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]],
+ area.GetPolygon()) == GM_None) {
+ num++;
+ }
+ if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]],
+ area.GetPolygon()) == GM_None) {
+ num++;
+ }
+ if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]],
+ area.GetPolygon()) == GM_None) {
+ num++;
+ }
+ if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[prime.pModel->axial[AXIAL_FRONT]]],
+ area.GetPolygon()) == GM_None) {
+ num++;
+ }
+ DEBUG("------------> %d", __LINE__);
+ if (num == 5) {
+ // 绂诲紑鍖哄煙
+ StopDrivingCurve(prime);
+ StopParkBottom(prime);
+ StopRightCorner(prime);
+ StopUphill(prime);
+ StopParkEdge(prime);
+ }
+ DEBUG("------------> %d", __LINE__);
}
}
// 杞﹁疆椹惰繃绾匡紝涓旇溅澶翠綅浜庡彸渚�
static bool CrossingStartLine(Line &trace, Line &start_line)
{
- PointF head = {.X = trace.X1, .Y = trace.Y1};
+ PointF head = trace.p1;
if (IntersectionOf(trace, start_line) == GM_Intersection
- && IntersectionOfLine(head, start_line) == RELATION_RIGHT) {
+ && IntersectionOfLine(head, start_line) == REL_POS_RIGHT) {
return true;
}
return false;
}
-void FarawayMap(prime_t &prime)
-{
- if (prime.arriving_map.type != 0) {
- PointF &car_head = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->axial[AXIAL_FRONT]];
- PointF car_head_trend = PointExtend(car_head, 7, prime.pModeling->yaw);
- Line car_head_line;
- MAKE_LINE(car_head_line, car_head, car_head_trend);
-
-
- }
-}
-
static void ProximityReminders(prime_t &prime)
{
- if (prime.curr_exam_map.type != MAP_TYPE_NONE && prime.arriving_map.type != MAP_TYPE_NONE) {
+ if (prime.examing_area.type != MAP_TYPE_NONE)
return;
+
+ DEBUG("----------------> %d", __LINE__);
+
+ DEBUG("test %d",std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps).size());
+ DEBUG("test %d",std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[0].points.size());
+ DEBUG("test %f,%f",std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[0].points[1].X, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[0].points[1].Y);
+ {
+ Line test;
+
+ MAKE_LINE(test, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[0].points[1], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[0].points[0]);
+
+ DEBUG("test %f,%f - %f,%f",test.p1.X, test.p1.Y, test.p2.X, test.p2.Y);
}
- PointF &car_head = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->axial[AXIAL_FRONT]];
- PointF car_head_trend = PointExtend(car_head, 6, prime.pModeling->yaw);
+ DEBUG("----------------> %d", __LINE__);
+ PointF car_head = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->axial[AXIAL_FRONT]];
+ PointF car_head_trend = PointExtend(car_head, 8, prime.pModeling->yaw);
+ DEBUG("----------------> %d", __LINE__);
+
Line car_head_line;
MAKE_LINE(car_head_line, car_head, car_head_trend);
-
+ DEBUG("----------------> %d", __LINE__);
Line start_line;
- for (int i = 0; i < prime.pMap->park_button_map.size(); ++i) { // 宸﹀彸2鏉℃帶鍒剁嚎閮藉彲浣滀负鍏ュ彛
- MAKE_LINE(start_line, prime.pMap->park_button_map[i].map[1], prime.pMap->park_button_map[i].map[0]);
-
- MAKE_LINE(start_line, prime.pMap->park_button_map[i].map[7], prime.pMap->park_button_map[i].map[6]);
-
+ for (int i = 0; i < std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps).size(); ++i) { // 宸﹀彸2鏉℃帶鍒剁嚎閮藉彲浣滀负鍏ュ彛
+ MAKE_LINE(start_line, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[i].points[1], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[i].points[0]);
+ CheckProximity(prime, car_head_line, start_line, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[i].id);
+ MAKE_LINE(start_line, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[i].points[7], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[i].points[6]);
+ CheckProximity(prime, car_head_line, start_line, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[i].id);
}
- for (int i = 0; i < prime.pMap->park_edge_map.size(); ++i) {
- MAKE_LINE(start_line, prime.pMap->park_edge_map[i].map[1], prime.pMap->park_edge_map[i].map[0]);
+ for (int i = 0; i < std::get<MAP_TYPE_PARK_EDGE>(prime.maps).size(); ++i) {
+ MAKE_LINE(start_line, std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[i].points[1], std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[i].points[0]);
+ CheckProximity(prime, car_head_line, start_line, std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[i].id);
}
- for (int i = 0; i < prime.pMap->uphill_map.size(); ++i) {
- PointF vPoint = Calc3Point(prime.pMap->uphill_map[i].map[8], prime.pMap->uphill_map[i].map[0], DistanceOf(prime.pMap->uphill_map[i].map[8], prime.pMap->uphill_map[i].map[7]), 'R');
- MAKE_LINE(start_line, prime.pMap->uphill_map[i].map[0], vPoint);
+ for (int i = 0; i < std::get<MAP_TYPE_UPHILL>(prime.maps).size(); ++i) {
+ MAKE_LINE(start_line, std::get<MAP_TYPE_UPHILL>(prime.maps)[i].points[0], std::get<MAP_TYPE_UPHILL>(prime.maps)[i].points[10]);
+ CheckProximity(prime, car_head_line, start_line, std::get<MAP_TYPE_UPHILL>(prime.maps)[i].id);
}
- for (int i = 0; i < prime.pMap->curve_map.size(); ++i) {
- MAKE_LINE(start_line, prime.pMap->curve_map[i].right_start_point, prime.pMap->curve_map[i].left_start_point);
+ for (int i = 0; i < std::get<MAP_TYPE_CURVE>(prime.maps).size(); ++i) {
+ MAKE_LINE(start_line, std::get<MAP_TYPE_CURVE>(prime.maps)[i].right_start_point, std::get<MAP_TYPE_CURVE>(prime.maps)[i].left_start_point);
+ CheckProximity(prime, car_head_line, start_line, std::get<MAP_TYPE_CURVE>(prime.maps)[i].id);
}
- for (int i = 0; i < prime.pMap->turn_a90_map.size(); ++i) {
- MAKE_LINE(start_line, prime.pMap->turn_a90_map[i].map[0], prime.pMap->turn_a90_map[i].map[1]);
+ for (int i = 0; i < std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps).size(); ++i) {
+ MAKE_LINE(start_line, std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[i].points[0], std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[i].points[1]);
+ CheckProximity(prime, car_head_line, start_line, std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)[i].id);
}
}
// 鍒ゆ柇鏄惁鎺ヨ繎鍦哄湴鐨勮捣濮嬬嚎
// 杞﹀ご瓒嬪娍绾挎槸鍚﹀拰鍏ュ彛绾跨浉浜�
-// 鍜屽叆鍙ｇ嚎鐨勫す瑙�
-static bool ProximityArea(Line &base_line, Line &line)
+static void CheckProximity(prime_t &prime, Line &base_line, Line &line, int this_id)
{
- PointF head = {.X = line.X1, .Y = line.Y1};
+ ProximityStatus status;
- if (IntersectionOf(base_line, line) == GM_Intersection
- && DistanceOf(head, base_line) > 1) {
- double angle = AngleOfTowLine(base_line, line);
- if (angle >= 240 && angle <= 300) {
- return true;
+ if (CrossingStartLine(base_line, line)) {
+ PointF p1 = base_line.p1;
+ PointF p2 = base_line.p2;
+
+ // 澧炲姞璺濈锛岀敤浜庢秷闄ゆ祴閲忚宸甫鏉ョ殑涓嶇ǔ瀹�
+ if (DistanceOf(p1, line) > 0.5 && DistanceOf(p2, line) > 0.5) {
+ status = ProximityStatus::Near;
+ } else {
+ status = ProximityStatus::Bounce;
}
+ } else {
+ status = ProximityStatus::Far;
}
- return false;
+ if (status == ProximityStatus::Near && prime.arriving_map != this_id) {
+ prime.arriving_map = this_id;
+ // 鎶ヨ闊虫彁绀�
+ PlayTTS("鍗冲皢鑰冭瘯", nullptr);
+ } else if (status == ProximityStatus::Far && prime.arriving_map == this_id) {
+ prime.arriving_map = -1;
+ }
}
void TerminateAreaExam(void)
{
- CurrExamMapIndex = -1;
+
}
void InitAreaExam(void)
{
- CurrExamMapIndex = -1;
ResetOdo();
}
--
Gitblit v1.8.0