From 4053e385bb8dc70ad8c8d12eeaec9c10ee87fcb9 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期一, 16 三月 2020 18:06:07 +0800
Subject: [PATCH] 更新地图
---
lib/src/main/cpp/driver_test.cpp | 281 ++++++++++++++++++++++++++++++++++----------------------
1 files changed, 171 insertions(+), 110 deletions(-)
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 8d94ecf..f3000a4 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -11,6 +11,10 @@
#include <pthread.h>
#include <cstring>
#include <vector>
+#include <list>
+#include <numeric>
+#include <algorithm>
+
#include "driver_test.h"
#include "defs.h"
#include "Geometry.h"
@@ -84,6 +88,43 @@
Polygon map2;
} MapList[MAP_LIST_SIZE];
+struct trigger_line {
+ int mapId;
+ Line tirgger;
+};
+
+struct road_exam_parent_map {
+ int id;
+ int type;
+ Polygon map;
+ int redLineNum;
+ Polygon *redLine;
+ int greenLineNum;
+ Polygon *greenLine;
+ int redAreaNum;
+ Polygon *redArea;
+ int triggerLineNum;
+ struct trigger_line *triggerLine;
+};
+
+static struct road_exam_parent_map * RoadParentMap;
+
+struct road_exam_son_map {
+ int id;
+ int type;
+ char tts[512];
+ Line triggerLine;
+ Line controlLine;
+ Line targetLine;
+ int stop_flag;
+
+ int redLineNum;
+ Polygon *redLine;
+};
+
+typedef list<struct road_exam_son_map *> LIST_ROAD_SON_MAP;
+static LIST_ROAD_SON_MAP RoadSonMapList;
+
static int MapNum = 0;
static int CurrExamMapIndex = -1;
static int CurrEnterMapIndex = -1;
@@ -92,7 +133,10 @@
static int exam_dummy_light;
static car_model *CarModel = NULL;
-static car_model *CarModelPrev = NULL;
+
+typedef list<car_model *> LIST_CAR_MODEL;
+
+static LIST_CAR_MODEL CarModelList;
static struct dummy_light_exam *DummyLightContent;
static int DummyLightContentSize;
@@ -128,7 +172,10 @@
memset(&MapList, 0, sizeof(MapList));
MapNum = 0;
CarModel = NULL;
- CarModelPrev = NULL;
+ CarModelList.clear();
+
+ RoadParentMap = NULL;
+ RoadSonMapList.clear();
CarSensorInit();
@@ -205,6 +252,8 @@
DEBUG("AddMap num %d", MapNum);
}
+
+
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, double antPitch)
@@ -223,24 +272,6 @@
free(CarModel);
CarModel = NULL;
}
-
- if (CarModelPrev != NULL) {
- if (CarModelPrev->body != NULL)
- free(CarModelPrev->body);
- if (CarModelPrev->carDesc != NULL)
- free(CarModelPrev->carDesc);
- if (CarModelPrev->carXY != NULL)
- free(CarModelPrev->carXY);
- free(CarModelPrev);
- CarModelPrev = NULL;
- }
-
- // 浠呭垎閰峜arXY, body鍜宑arDesc鍧囨病鏈�
- CarModelPrev = (car_model *)malloc(sizeof(car_model));
- CarModelPrev->body = NULL;
- CarModelPrev->carDesc = NULL;
- CarModelPrev->pointNum = 0;
- CarModelPrev->carXY = (PointF *) malloc(sizeof(PointF) * pointNum);
CarModel = (car_model *)malloc(sizeof(car_model));
CarModel->basePoint.X = basePoint[0];
@@ -623,19 +654,19 @@
switch (mtype) {
case MAP_TYPE_PARK_BUTTOM:
testing = TestParkBottom(&MapList[CurrExamMapIndex].map,
- CarModel, CarModelPrev, speed, move, rtkTime);
+ CarModel, NULL, speed, move, rtkTime);
break;
case MAP_TYPE_STOP_START:
- testing = TestSAS(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, speed, move, rtkTime);
+ testing = TestSAS(&MapList[CurrExamMapIndex].map, CarModel, NULL, speed, move, rtkTime);
break;
case MAP_TYPE_PART_EDGE:
- testing = TestParkEdge(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, speed, move, rtkTime);
+ testing = TestParkEdge(&MapList[CurrExamMapIndex].map, CarModel, NULL, speed, move, rtkTime);
break;
case MAP_TYPE_CURVE:
- testing = TestDrivingCurve(&MapList[CurrExamMapIndex].map, &MapList[CurrExamMapIndex].map2, CarModel, CarModelPrev, speed, move, rtkTime);
+ testing = TestDrivingCurve(&MapList[CurrExamMapIndex].map, &MapList[CurrExamMapIndex].map2, CarModel, NULL, speed, move, rtkTime);
break;
case MAP_TYPE_TURN_90:
- testing = TestTurnA90(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, azimuth, speed, move, rtkTime);
+ testing = TestTurnA90(&MapList[CurrExamMapIndex].map, CarModel, NULL, azimuth, speed, move, rtkTime);
break;
default:
break;
@@ -715,21 +746,51 @@
main_ant_coord.X = RtkBuffer[p1].x;
main_ant_coord.Y = RtkBuffer[p1].y;
- // 淇濆瓨涓婁竴涓溅韬疆寤撶偣
- if (CarModelPrev->pointNum > 0) {
- CarModelPrev->pointNum = CarModel->pointNum;
- CarModelPrev->basePoint = CarModel->basePoint;
- for (int i = 0; i < CarModel->pointNum; ++i) {
- CarModelPrev->carXY[i] = CarModel->carXY[i];
- }
- }
UpdateCarBodyCoord(RtkBuffer[p1].heading, RtkBuffer[p1].pitch, RtkBuffer[p1].roll, main_ant_coord, CarModel);
- if (CarModelPrev->pointNum == 0) {
- CarModelPrev->pointNum = CarModel->pointNum;
- CarModelPrev->basePoint = CarModel->basePoint;
- for (int i = 0; i < CarModel->pointNum; ++i) {
- CarModelPrev->carXY[i] = CarModel->carXY[i];
- }
+
+ car_model *newModel = (car_model *)malloc(sizeof(car_model));
+
+ newModel->basePoint = CarModel->basePoint;
+ newModel->axial[0] = CarModel->axial[0];
+ newModel->axial[1] = CarModel->axial[1];
+ newModel->left_front_tire[0] = CarModel->left_front_tire[0];
+ newModel->left_front_tire[1] = CarModel->left_front_tire[1];
+ newModel->right_front_tire[0] = CarModel->right_front_tire[0];
+ newModel->right_front_tire[1] = CarModel->right_front_tire[1];
+ newModel->left_rear_tire[0] = CarModel->left_rear_tire[0];
+ newModel->left_rear_tire[1] = CarModel->left_rear_tire[1];
+ newModel->right_rear_tire[0] = CarModel->right_rear_tire[0];
+ newModel->right_rear_tire[1] = CarModel->right_rear_tire[1];
+ newModel->bodyNum = CarModel->bodyNum;
+ newModel->body = (int *) malloc(sizeof(int) * newModel->bodyNum);
+ for (int i = 0; i < newModel->bodyNum; ++i) {
+ newModel->body[i] = CarModel->body[i];
+ }
+ newModel->pointNum = CarModel->pointNum;
+ newModel->carXY = (PointF *) malloc(sizeof(PointF) * newModel->pointNum);
+ for (int i = 0; i < newModel->pointNum; ++i) {
+ newModel->carXY[i] = CarModel->carXY[i];
+ }
+ newModel->carDesc = NULL;
+ newModel->antPitch = CarModel->antPitch;
+ newModel->yaw = CarModel->yaw;
+ newModel->pitch = CarModel->pitch;
+
+ CarModelList.push_front(newModel);
+ while (CarModelList.size() > 25) {
+ DEBUG("鍒犻櫎灏剧粨鐐�");
+ car_model *ptr = CarModelList.back();
+
+ if (ptr->body != NULL)
+ free(ptr->body);
+ if (ptr->carXY != NULL)
+ free(ptr->carXY);
+ if (ptr->carDesc != NULL)
+ free(ptr->carDesc);
+
+ free(ptr);
+
+ CarModelList.pop_back();
}
// 璁＄畻閫熷害(绫�/绉�)銆佸墠杩涘悗閫�
@@ -801,77 +862,74 @@
{
// 杞︾殑鏈�鍓嶇偣鏄惁杩涘叆鍦板浘
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;
-//
-// map.num = 4;
-// map.point = (PointF *) malloc(map.num * sizeof(PointF));
-//
-// 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 (mapList[i].type == MAP_TYPE_CURVE) {
-//
-// }
-// if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
-// if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
-// Line enterLine1, enterLine2;
-//
-// MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
-// MakeLine(&enterLine2, &(mapList[i].map.point[6]), &(mapList[i].map.point[7]));
-//
-// if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1 &&
-// DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine2) > 0.1)
-// return i;
-// }
-// }
-// if (mapList[i].type == MAP_TYPE_PART_EDGE) {
-// if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
-// Line enterLine;
-//
-// MakeLine(&enterLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
-//
-// if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine) > 0.1)
-// return i;
-// }
-// }
-// if (mapList[i].type == MAP_TYPE_TURN_90) {
-// if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
-// Line enterLine1;
-//
-// MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
-//
-// if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
-// return 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;
+
+ map.num = 4;
+ map.point = (PointF *) malloc(map.num * sizeof(PointF));
+
+ 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 (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
+ if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
+ Line enterLine1, enterLine2;
+
+ MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
+ MakeLine(&enterLine2, &(mapList[i].map.point[6]), &(mapList[i].map.point[7]));
+
+ if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1 &&
+ DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine2) > 0.1)
+ return i;
+ }
+ }
+ if (mapList[i].type == MAP_TYPE_PART_EDGE) {
+ if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
+ Line enterLine;
+
+ MakeLine(&enterLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
+
+ if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine) > 0.1)
+ return i;
+ }
+ }
+ if (mapList[i].type == MAP_TYPE_TURN_90) {
+ if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
+ Line enterLine1;
+
+ MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
+
+ if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
+ return i;
+ }
+ }
if (mapList[i].type == MAP_TYPE_CURVE) {
Line startLine;
Line carAxial;
@@ -1008,6 +1066,9 @@
{
carModel->basePoint = main_ant;
+ carModel->yaw = azimuth;
+ carModel->pitch = pitch;
+
pitch = pitch - carModel->antPitch;
for (int i = 0; i < carModel->pointNum; ++i) {
--
Gitblit v1.8.0