From 25bdbfbf6f4c9f1530be74fc194ad1dd0a13e8c9 Mon Sep 17 00:00:00 2001
From: fctom1215 <fctom1215@outlook.com>
Date: 星期日, 08 三月 2020 23:12:49 +0800
Subject: [PATCH] 角度修正。
---
lib/src/main/cpp/test_items/driving_curve.cpp | 289 +++++++++++++++++++++++++----------------------
lib/src/main/cpp/driver_test.cpp | 24 +---
lib/src/main/cpp/master/comm_if.cpp | 9 +
lib/src/main/cpp/driver_test.h | 3
lib/src/main/cpp/test_items/driving_curve.h | 1
5 files changed, 171 insertions(+), 155 deletions(-)
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 4ee19db..c961a25 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -202,7 +202,7 @@
void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
- int *body, int bodyNum,double (*point)[2], int pointNum)
+ int *body, int bodyNum,double (*point)[2], int pointNum, double antPitch)
{
DEBUG("鍔犲叆杞﹁締淇℃伅 pointNum %d", pointNum);
@@ -264,6 +264,8 @@
CarModel->body[i] = body[i];
}
}
+
+ CarModel->antPitch = antPitch;
CarModel->pointNum = pointNum;
CarModel->carDesc = (struct car_desc_ *)malloc(sizeof(struct car_desc_) * pointNum);
@@ -962,23 +964,7 @@
free(map.point);
}
if (mapList[index].type == MAP_TYPE_CURVE) {
- Line carAxial;
- Line scanLine;
-
- MakeLine(&carAxial, &car->carXY[car->axial[AXIAL_FRONT]], &car->carXY[car->axial[AXIAL_REAR]]);
-
- int startL = 0, startR = 0;
- while (startL < mapList[index].map.num || startR < mapList[index].map2.num) {
- MakeLine(&scanLine, &mapList[index].map.point[startL], &mapList[index].map.point[startR]);
- if (IntersectionOf(scanLine, carAxial) == GM_Intersection) {
- break;
- }
- if (startL < mapList[index].map.num)
- startL++;
- if (startR < mapList[index].map2.num)
- startR++;
- }
- ret = true;
+ ret = ExitDrivingCurveArea(&mapList[index].map, &mapList[index].map2, car);
}
return ret;
@@ -1032,6 +1018,8 @@
{
carModel->basePoint = main_ant;
+ pitch = pitch - carModel->antPitch;
+
for (int i = 0; i < carModel->pointNum; ++i) {
double qrx = carModel->carDesc[i].distance * sin(toRadians(carModel->carDesc[i].angle));
double qry =
diff --git a/lib/src/main/cpp/driver_test.h b/lib/src/main/cpp/driver_test.h
index 307db69..c411cf2 100644
--- a/lib/src/main/cpp/driver_test.h
+++ b/lib/src/main/cpp/driver_test.h
@@ -37,6 +37,7 @@
int bodyNum;
int *body;
int pointNum;
+ double antPitch;
struct car_desc_ *carDesc; // 缁忕幇鍦烘祴閲忕偣璁＄畻鍚庣殑鍊�
PointF *carXY; // 璁＄畻鍚庣殑杞﹁韩鍚勭偣鍧愭爣
} car_model;
@@ -85,7 +86,7 @@
void AddMap(int id, int type, const double (*map)[2], int pointNum, const double (*map2)[2], int pointNum2);
void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
- int *body, int bodyNum, double (*point)[2], int pointNum);
+ int *body, int bodyNum, double (*point)[2], int pointNum, double antPitch);
void SetSensorCfg(int (*sensor)[3], int sensorNum);
int GetSensorValidLevel(void);
void GetSensorCfg(int gpio, int &func, bool &lvl);
diff --git a/lib/src/main/cpp/master/comm_if.cpp b/lib/src/main/cpp/master/comm_if.cpp
index 3c227c9..3ee83da 100644
--- a/lib/src/main/cpp/master/comm_if.cpp
+++ b/lib/src/main/cpp/master/comm_if.cpp
@@ -564,6 +564,8 @@
int bodyNum = 0;
int *body = NULL;
int pointNum = 0;
+ double antPitch = 0;
+
double (*point)[2] = NULL;
if (doc.HasMember("main_ant")) {
@@ -656,8 +658,13 @@
}
}
+ if (doc.HasMember("ant_pitch")) {
+ const Value& s = doc["ant_pitch"];
+ antPitch = s.GetDouble();
+ }
+
SetCarMeasurePoint(basePoint, axial, left_front_tire, right_front_tire,
- left_rear_tire, right_rear_tire, body, bodyNum, point, pointNum);
+ left_rear_tire, right_rear_tire, body, bodyNum, point, pointNum, antPitch);
if (body != NULL) delete []body;
if (point != NULL) delete []point;
diff --git a/lib/src/main/cpp/test_items/driving_curve.cpp b/lib/src/main/cpp/test_items/driving_curve.cpp
index 6884863..7a33a68 100644
--- a/lib/src/main/cpp/test_items/driving_curve.cpp
+++ b/lib/src/main/cpp/test_items/driving_curve.cpp
@@ -15,10 +15,6 @@
#define DEBUG(fmt, args...) LOGD("<driving_curve> <%s>: " fmt, __func__, ##args)
-enum {
- DRIVING_ON_CURVE
-};
-
const uint32_t STOP_CAR_TIME = D_SEC(2);
static bool testing = false;
@@ -27,15 +23,16 @@
static bool reportStopCarTimeout;
static int prevMoveDirect;
static bool crashRedLine;
-static struct calc_zone_t {
+static struct scan_window_t {
int leftStart;
int leftEnd;
int rightStart;
int rightEnd;
-} calcZone;
+} scanWindow;
-static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct calc_zone_t *zone);
-static bool ExitArea(const Polygon *map, const Polygon *map2, const car_model *car);
+static bool UpdateStartLine(struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect);
+static bool UpdateEndLine(bool mode, struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect);
+static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct scan_window_t *zone);
void StartDrivingCurve(int moveDirect, const struct RtkTime *rtkTime)
{
@@ -50,141 +47,31 @@
reportStopCarTimeout = false;
crashRedLine = false;
- calcZone.leftStart = calcZone.leftEnd = calcZone.rightStart = calcZone.rightEnd = 0;
+ scanWindow.leftStart = scanWindow.leftEnd = scanWindow.rightStart = scanWindow.rightEnd = 0;
}
int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
- Line start, end;
- Line axial;
+ Polygon tireRect;
- MakeLine(&axial, &car->carXY[car->axial[AXIAL_FRONT]], &car->carXY[car->axial[AXIAL_REAR]]);
+ MakePolygon(&tireRect, {car->carXY[car->left_front_tire[TIRE_OUTSIDE]],
+ car->carXY[car->right_front_tire[TIRE_OUTSIDE]],
+ car->carXY[car->right_rear_tire[TIRE_OUTSIDE]],
+ car->carXY[car->left_rear_tire[TIRE_OUTSIDE]]});
-// DEBUG("START 绾� %d -- %d", calcZone.leftStart, calcZone.rightStart);
-// DEBUG("END 绾� %d -- %d", calcZone.leftEnd, calcZone.rightEnd);
-
- MakeLine(&start, &map->point[calcZone.leftStart], &map2->point[calcZone.rightStart]);
-
- if (IntersectionOf(start, axial) == GM_None) {
- // 鍚戣捣鐐规煡鎵�
- int tempLeft = calcZone.leftStart, tempRight = calcZone.rightStart;
-
- while (calcZone.leftStart > 0 || calcZone.rightStart > 0) {
- if (calcZone.leftStart > 0)
- tempLeft = calcZone.leftStart - 1;
- if (calcZone.rightStart > 0)
- tempRight = calcZone.rightStart - 1;
-// DEBUG("START 鍚戣捣鐐规煡鎵� %d -- %d", tempLeft, tempRight);
- MakeLine(&start, &map->point[tempLeft], &map2->point[tempRight]);
-
- if (IntersectionOf(start, axial) == GM_Intersection) {
- // 淇濇寔涔嬪墠鐨勭嚎
- break;
- } else {
- calcZone.leftStart = tempLeft;
- calcZone.rightStart = tempRight;
-
- if (calcZone.leftStart == calcZone.leftEnd && calcZone.rightStart == calcZone.rightEnd) {
- // 杞﹁締涓㈠け锛岄噸鏂版悳绱�
- calcZone.leftStart = calcZone.rightStart = 0;
- calcZone.leftEnd = calcZone.rightEnd = 0;
-
- DEBUG("杞﹁締涓㈠け锛岄噸鏂版悳绱�");
-
- while (calcZone.leftStart < map->num || calcZone.rightStart < map2->num) {
- MakeLine(&start, &map->point[calcZone.leftStart], &map2->point[calcZone.rightStart]);
-
- if (IntersectionOf(start, axial) == GM_Intersection) {
- while (calcZone.leftStart < map->num || calcZone.rightStart < map2->num) {
- MakeLine(&start, &map->point[calcZone.leftStart], &map2->point[calcZone.rightStart]);
- if (IntersectionOf(start, axial) == GM_None) {
- break;
- }
- if (calcZone.leftStart < map->num)
- calcZone.leftStart++;
- if (calcZone.rightStart < map2->num)
- calcZone.rightStart++;
- }
- break;
- }
-
- calcZone.leftEnd = calcZone.leftStart;
- calcZone.rightEnd = calcZone.rightStart;
-
- if (calcZone.leftStart < map->num)
- calcZone.leftStart++;
- if (calcZone.rightStart < map2->num)
- calcZone.rightStart++;
- }
-
- if (calcZone.leftStart >= map->num && calcZone.rightStart >= map2->num) {
- // 绂诲紑鍦哄湴
- DEBUG("绂诲紑鏇茬嚎鍦哄湴");
- testing = false;
- goto TEST_END;
- }
-
- break;
- }
- }
- }
- } else {
- // 鍚戠粓鐐规煡鎵�
- do {
- if (calcZone.leftStart >= map->num && calcZone.rightStart >= map2->num) {
- break;
- }
- if (calcZone.leftStart < map->num)
- calcZone.leftStart++;
- if (calcZone.rightStart < map2->num)
- calcZone.rightStart++;
-// DEBUG("START 鍚戠粓鐐规煡鎵� %d -- %d", calcZone.leftStart, calcZone.rightStart);
- MakeLine(&start, &map->point[calcZone.leftStart], &map2->point[calcZone.rightStart]);
- } while (IntersectionOf(start, axial) == GM_Intersection);
+ // 鏇存柊杞﹀ご鎵弿绾�
+ if (!UpdateStartLine(&scanWindow, map, map2, &tireRect)) {
+ DEBUG("绂诲紑鍦哄湴");
+ testing = false;
+ goto TEST_END;
}
- MakeLine(&end, &map->point[calcZone.leftEnd], &map2->point[calcZone.rightEnd]);
+ // 鏇存柊杞﹀熬鎵弿绾�
+ UpdateEndLine(false, &scanWindow, map, map2, &tireRect);
- if (IntersectionOf(end, axial) == GM_None) {
- // 鍚戠粓鐐规煡鎵�
- int tempLeft = calcZone.leftEnd, tempRight = calcZone.rightEnd;
- while (calcZone.leftEnd < map->num || calcZone.rightEnd < map2->num) {
- if (calcZone.leftEnd >= map->num && calcZone.rightEnd >= map2->num) {
- break;
- }
- if (calcZone.leftEnd < map->num)
- tempLeft = calcZone.leftEnd + 1;
- if (calcZone.rightEnd < map2->num)
- tempRight = calcZone.rightEnd + 1;
-// DEBUG("END 鍚戠粓鐐规煡鎵� %d -- %d", tempLeft, tempRight);
- MakeLine(&end, &map->point[tempLeft], &map2->point[tempRight]);
+ DEBUG("scanWindow leftStart %d leftEnd %d rightStart %d rightEnd %d", scanWindow.leftStart, scanWindow.leftEnd, scanWindow.rightStart, scanWindow.rightEnd);
- if (IntersectionOf(end, axial) == GM_Intersection) {
- // 淇濇寔涔嬪墠鐨勭嚎
- break;
- } else {
- calcZone.leftEnd = tempLeft;
- calcZone.rightEnd = tempRight;
- }
- }
- } else {
- // 鍚戣捣鐐规煡鎵�
- do {
- if (calcZone.leftEnd == 0 && calcZone.rightEnd == 0) {
- break;
- }
- if (calcZone.leftEnd > 0)
- calcZone.leftEnd--;
- if (calcZone.rightEnd > 0)
- calcZone.rightEnd--;
-// DEBUG("END 鍚戣捣鐐规煡鎵� %d -- %d", calcZone.leftEnd, calcZone.rightEnd);
- MakeLine(&end, &map->point[calcZone.leftEnd], &map2->point[calcZone.rightEnd]);
- } while (IntersectionOf(end, axial) == GM_Intersection);
- }
-
- DEBUG("calcZone leftStart %d leftEnd %d rightStart %d rightEnd %d", calcZone.leftStart, calcZone.leftEnd, calcZone.rightStart, calcZone.rightEnd);
-
- if (CrashRedLine(map, map2, car, &calcZone)) {
+ if (CrashRedLine(map, map2, car, &scanWindow)) {
if (!crashRedLine) {
crashRedLine = true;
// 杞﹁疆鍘嬭竟绾匡紝涓嶅悎鏍�
@@ -217,10 +104,142 @@
}
TEST_END:
+ CleanPolygon(&tireRect);
return testing ? 1 : 0;
}
-static bool ExitArea(const Polygon *map, const Polygon *map2, const car_model *car)
+static bool UpdateStartLine(struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect)
+{
+ Line start;
+
+ bool update = true;
+ int direct = 0;
+ int tempLeft = scanWindow.leftStart, tempRight = scanWindow.rightStart;
+
+ while (update) {
+ update = false;
+ MakeLine(&start, &map->point[scanWindow.leftStart], &map2->point[scanWindow.rightStart]);
+
+ if (IntersectionOf(start, tireRect) == GM_None) {
+ if (direct != 1) {
+ direct = -1;
+ // 鍏ュ満鏂瑰悜鎵弿
+ tempLeft = scanWindow.leftStart;
+ tempRight = scanWindow.rightStart;
+
+ if (scanWindow.leftStart > 0) {
+ update = true;
+ scanWindow.leftStart--;
+ }
+ if (scanWindow.rightStart > 0) {
+ update = true;
+ scanWindow.rightStart--;
+ }
+
+ if (scanWindow.leftStart <= scanWindow.leftEnd && scanWindow.rightStart <= scanWindow.rightEnd) {
+ DEBUG("杞﹁締涓㈠け锛岄噸鏂版悳绱� %d %d", scanWindow.leftStart, scanWindow.rightStart);
+
+ // 杞﹁締涓㈠け锛岄噸鏂版悳绱�
+ update = false;
+ scanWindow.leftEnd = scanWindow.rightEnd = 0;
+
+ if (UpdateEndLine(true, &scanWindow, map, map2, tireRect)) {
+ DEBUG("鍖归厤鎴愬姛 %d %d", scanWindow.leftStart, scanWindow.leftEnd);
+ direct = 0;
+ update = true;
+ } else {
+ DEBUG("鍖归厤澶辫触");
+ return false;
+ }
+ }
+ }
+ } else {
+ if (direct != -1) {
+ // 鍑哄満鏂瑰悜鎵弿
+ direct = 1;
+ if (scanWindow.leftStart < map->num - 1) {
+ update = true;
+ scanWindow.leftStart++;
+ }
+ if (scanWindow.rightStart < map2->num - 1) {
+ update = true;
+ scanWindow.rightStart++;
+ }
+ } else {
+ scanWindow.leftStart = tempLeft;
+ scanWindow.rightStart = tempRight;
+ }
+ }
+ }
+
+ return true;
+}
+
+static bool UpdateEndLine(bool mode, struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect)
+{
+ bool update = true;
+ bool crash = false;
+
+ int direct = 0;
+ int tempLeft = zone->leftEnd;
+ int tempRight = zone->rightEnd;
+
+ Line end;
+
+ while (update) {
+ update = false;
+ MakeLine(&end, &map->point[zone->leftEnd], &map2->point[zone->rightEnd]);
+
+ if (IntersectionOf(end, tireRect) == GM_None) {
+ if (direct != -1) {
+ // 鍑哄満鏂瑰悜鎵弿
+ direct = 1;
+ tempLeft = zone->leftEnd;
+ tempRight = zone->rightEnd;
+
+ if (zone->leftEnd < map->num - 1) {
+ update = true;
+ zone->leftEnd++;
+ }
+ if (zone->rightEnd < map2->num - 1) {
+ update = true;
+ zone->rightEnd++;
+ }
+ }
+ } else {
+ if (!crash) {
+ crash = true;
+ if (mode) {
+ zone->leftStart = zone->leftEnd;
+ zone->rightStart = zone->rightEnd;
+
+
+ DEBUG("绗竴娆℃帴瑙� %d %d %d", zone->leftStart, zone->leftEnd, tempLeft);
+ }
+ }
+
+ if (direct != 1) {
+ // 鍏ュ満鏂瑰悜鎵弿
+ direct = -1;
+ if (zone->leftEnd > 0) {
+ update = true;
+ zone->leftEnd--;
+ }
+ if (zone->rightEnd > 0) {
+ update = true;
+ zone->rightEnd--;
+ }
+ } else {
+ zone->leftEnd = tempLeft;
+ zone->rightEnd = tempRight;
+ }
+ }
+ }
+
+ return crash;
+}
+
+bool ExitDrivingCurveArea(const Polygon *map, const Polygon *map2, const car_model *car)
{
// 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
bool ret = false;
@@ -257,7 +276,7 @@
}
// 杞﹁疆鏄惁鍘嬭竟绾�
-static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct calc_zone_t *zone)
+static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct scan_window_t *zone)
{
bool ret = false;
diff --git a/lib/src/main/cpp/test_items/driving_curve.h b/lib/src/main/cpp/test_items/driving_curve.h
index 57f3daa..22c7181 100644
--- a/lib/src/main/cpp/test_items/driving_curve.h
+++ b/lib/src/main/cpp/test_items/driving_curve.h
@@ -11,6 +11,7 @@
using namespace std;
void StartDrivingCurve(int moveDirect, const struct RtkTime *rtkTime);
+bool ExitDrivingCurveArea(const Polygon *map, const Polygon *map2, const car_model *car);
int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime);
--
Gitblit v1.8.0