From 3a48a0de38910517352557510882f2ff4d8436ae Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期五, 20 三月 2020 18:05:55 +0800
Subject: [PATCH] 路考
---
lib/src/main/cpp/test_items2/road_exam.h | 13 ++
lib/src/main/cpp/driver_test.cpp | 35 ++++++
lib/src/main/cpp/test_items2/through_something.h | 14 ++
lib/src/main/cpp/test_common/car_sensor.h | 7 +
lib/src/main/cpp/master/comm_if.cpp | 34 ++++++
lib/src/main/cpp/test_items2/through_something.cpp | 114 ++++++++++++++++++++++
lib/src/main/cpp/test_items2/road_exam.cpp | 41 ++++++-
lib/src/main/cpp/CMakeLists.txt | 1
lib/src/main/cpp/driver_test.h | 7
9 files changed, 247 insertions(+), 19 deletions(-)
diff --git a/lib/src/main/cpp/CMakeLists.txt b/lib/src/main/cpp/CMakeLists.txt
index f1afd23..2d0498b 100644
--- a/lib/src/main/cpp/CMakeLists.txt
+++ b/lib/src/main/cpp/CMakeLists.txt
@@ -39,6 +39,7 @@
test_items2/common_check.cpp
test_items2/dummy_light.cpp
test_items2/road_exam.cpp
+ test_items2/through_something.cpp
rtk_module/rtk.cpp
rtk_module/virtual_rtk.cpp
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 8a9c7cd..a144e68 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -252,6 +252,10 @@
}
free(map.triggerLine);
}
+
+ if (map.area.point != NULL) {
+ free(map.area.point);
+ }
}
RoadMapList.clear();
@@ -272,8 +276,13 @@
}
}
-void AddRoadMapParent(int id, int type, string tts, vector<vector<int>> &redLines,
- vector<vector<int>> &redAreas, vector<vector<int>> &greenLines, vector<vector<int>> &triggerLines)
+void AddRoadMapParent(int id, int type, string tts,
+ vector<vector<int>> &redLines,
+ vector<vector<int>> &redAreas,
+ vector<vector<int>> &greenLines,
+ vector<vector<int>> &triggerLines,
+ vector<int> area,
+ vector<int> stopLine)
{
struct road_exam_map newMap;
@@ -343,6 +352,25 @@
} else {
newMap.triggerLine = NULL;
};
+
+ if (area.size() > 0) {
+ newMap.area.num = area.size();
+ newMap.area.point = (PointF *) malloc(sizeof(PointF) * newMap.area.num);
+
+ for (int i = 0; i < newMap.area.num; ++i) {
+ newMap.area.point[i] = RoadMapPoints.point[area[i]];
+ }
+ } else {
+ newMap.area.point = NULL;
+ newMap.area.num = 0;
+ }
+
+ if (stopLine.size() == 4) {
+ newMap.stopLine.X1 = stopLine[0];
+ newMap.stopLine.Y1 = stopLine[1];
+ newMap.stopLine.X2 = stopLine[2];
+ newMap.stopLine.Y2 = stopLine[3];
+ }
RoadMapList.push_back(newMap);
}
@@ -681,7 +709,7 @@
if (ExamType == TEST_TYPE_AREA)
AddExamFault(4, rtkTime);
}
-
+DEBUG("22222222222222222222");
if (ExamType == TEST_TYPE_ROAD_DUMMY_LIGHT) {
if (exam_dummy_light == 0) {
StartDummyLightExam(DummyLightContent, DummyLightContentSize, rtkTime);
@@ -886,7 +914,6 @@
CarModelList.push_front(newModel);
while (CarModelList.size() > 100) {
- DEBUG("鍒犻櫎灏剧粨鐐�");
car_model *ptr = CarModelList.back();
if (ptr->body != NULL)
diff --git a/lib/src/main/cpp/driver_test.h b/lib/src/main/cpp/driver_test.h
index 7bff0a9..ca11dee 100644
--- a/lib/src/main/cpp/driver_test.h
+++ b/lib/src/main/cpp/driver_test.h
@@ -107,8 +107,8 @@
int triggerLineNum;
trigger_line_t *triggerLine;
- Line startLine;
- Line endLine;
+ Polygon area; // 瀛愰」鐩殑鍖哄煙
+ Line stopLine; // 璇稿浜鸿閬撱�佽矾鍙ｇ瓑鑰冪偣鐨勫仠姝㈢嚎
int flagStop; // 鍒拌揪寮�濮嬬嚎鍓嶏紝鏄惁闇�瑕佸仠杞�
};
@@ -125,7 +125,8 @@
void CleanRoadMap(void);
void SetRoadMapPoints(vector<double> &mapPoints);
void AddRoadMapParent(int id, int type, string tts, vector<vector<int>> &redLines,
- vector<vector<int>> &redAreas, vector<vector<int>> &greenLines, vector<vector<int>> &triggerLines);
+ vector<vector<int>> &redAreas, vector<vector<int>> &greenLines, vector<vector<int>> &triggerLines,
+ vector<int> area, vector<int> stopLine);
void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
diff --git a/lib/src/main/cpp/master/comm_if.cpp b/lib/src/main/cpp/master/comm_if.cpp
index 9f52d64..cf9abe8 100644
--- a/lib/src/main/cpp/master/comm_if.cpp
+++ b/lib/src/main/cpp/master/comm_if.cpp
@@ -492,11 +492,15 @@
if (!doc.HasParseError()) {
CleanRoadMap();
+ DEBUG("寮�濮嬭В鏋愯矾鑰冨湴鍥�");
+
vector<double> mapPoints;
mapPoints.clear();
if (doc.HasMember("points")) {
const Value &s = doc["points"];
+
+ DEBUG("寰楀埌鎵�鏈夌偣");
// X-Y鍧愭爣闆嗗悎
for (Value::ConstValueIterator itr2 = s.Begin();
itr2 != s.End(); ++itr2) {
@@ -510,6 +514,8 @@
const Value &a = doc["maps"];
for (Value::ConstValueIterator itr = a.Begin(); itr != a.End(); ++itr) {
+
+ DEBUG("寰楀埌鍚勫瓙鍦板浘");
if (itr->IsObject()) {
// a Map
int id, type;
@@ -520,7 +526,11 @@
vector<vector<int>> greenLines;
vector<vector<int>> triggerLines;
vector<vector<int>> redAreas;
+ vector<int> area;
+ vector<int> stopLine;
+ stopLine.clear();
+ area.clear();
tts.clear();
redLines.clear();
greenLines.clear();
@@ -557,8 +567,8 @@
}
}
- if (itr->HasMember("trigger_line")) {
- const Value &s = (*itr)["trigger_line"];
+ if (itr->HasMember("all_trigger_line")) {
+ const Value &s = (*itr)["all_trigger_line"];
for (Value::ConstValueIterator itrLine = s.Begin();
itrLine != s.End(); ++itrLine) {
@@ -585,6 +595,24 @@
}
}
+ if (itr->HasMember("area")) {
+ const Value &s = (*itr)["area"];
+
+ for (Value::ConstValueIterator itrPoint = s.Begin();
+ itrPoint != s.End(); ++itrPoint) {
+ area.push_back((*itrPoint).GetInt());
+ }
+ }
+
+ if (itr->HasMember("stop_line")) {
+ const Value &s = (*itr)["stop_line"];
+
+ for (Value::ConstValueIterator itrPoint = s.Begin();
+ itrPoint != s.End(); ++itrPoint) {
+ stopLine.push_back((*itrPoint).GetInt());
+ }
+ }
+
if (itr->HasMember("id")) {
const Value &s = (*itr)["id"];
id = s.GetInt();
@@ -605,7 +633,7 @@
tts = s.GetString();
}
- AddRoadMapParent(id, type, tts, redLines, redAreas, greenLines, triggerLines);
+ AddRoadMapParent(id, type, tts, redLines, redAreas, greenLines, triggerLines, area, stopLine);
}
}
}
diff --git a/lib/src/main/cpp/test_common/car_sensor.h b/lib/src/main/cpp/test_common/car_sensor.h
index 749dc26..b3a1157 100644
--- a/lib/src/main/cpp/test_common/car_sensor.h
+++ b/lib/src/main/cpp/test_common/car_sensor.h
@@ -19,6 +19,9 @@
MAIN_BEAM_LAMP,
SEATBELT,
ENGINE_START,
+ BREAK,
+ HAND_BREAK,
+ SECOND_BREAK,
CAR_STATUS_END //////////////
};
@@ -26,6 +29,7 @@
OFF_LIGHT = 0,
EJECT_SEATBELT = 0,
ENGINE_START_INACTIVE = 0,
+ BREAK_INACTIVE = 0,
HAZARD_LIGHTS,
LEFT_TURN_LIGHT,
RIGHT_TURN_LIGHT,
@@ -42,7 +46,8 @@
GEAR_2,
GEAR_3,
GEAR_4,
- GEAR_5
+ GEAR_5,
+ BREAK_ACTIVE
};
void CarSensorInit(void);
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index fd723b9..5e1f915 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -10,6 +10,7 @@
#include "../defs.h"
#include "../test_common/car_sensor.h"
#include "../native-lib.h"
+#include "through_something.h"
#include <vector>
#include <list>
@@ -400,14 +401,38 @@
if (currExamMapIndex == -1) {
currExamMapIndex = CrashTriggerLine(RoadMapList, car, CarModelList);
if (currExamMapIndex != -1) {
- DEBUG("杩涘叆璺�冨瓙鍦板浘 index = %d id = %d item = %d", currExamMapIndex, RoadMapList[currExamMapIndex].id, RoadMapList[currExamMapIndex].type);
- if (!RoadMapList[currExamMapIndex].tts.empty()) {
- PlayTTS(RoadMapList[currExamMapIndex].tts.c_str(), 0);
- }
+ StartThroughExam(currExamMapIndex, RoadMapList);
}
} else {
}
+}
+
+bool ExitSonArea(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car)
+{
+ bool ret = false;
+
+ if (index < 0 || index > RoadMapList.size())
+ return true;
+
+ if (RoadMapList[index].area.point != NULL) {
+ // 闇�瑕佽溅韬叏閮ㄧ寮�
+ Polygon carBody;
+
+ carBody.num = car->bodyNum;
+ carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
+ for (int i = 0; i < carBody.num; ++i) {
+ carBody.point[i] = car->carXY[car->body[i]];
+ }
+
+ if (IntersectionOf(&carBody, &RoadMapList[index].area) == GM_None) {
+ ret = true;
+ }
+
+ free(carBody.point);
+ }
+
+ return ret;
}
static void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
@@ -494,7 +519,7 @@
MakeLine(&rearAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);
for (int i = 0; i < RoadMapList.size(); ++i) {
- if (RoadMapList[i].type == 100) {
+ if (RoadMapList[i].type == GENERAL_MAP) {
// 姣忔潯绾块兘妫�娴�
for (int j = 0; j < RoadMapList[i].redLineNum; ++j) {
@@ -536,7 +561,7 @@
}
for (int i = 0; i < RoadMapList.size(); ++i) {
- if (RoadMapList[i].type == 100) {
+ if (RoadMapList[i].type == GENERAL_MAP) {
// 姣忔潯绾㈠尯閮芥娴�
for (int j = 0; j < RoadMapList[i].redAreaNum; ++j) {
@@ -566,7 +591,7 @@
MakeLine(&rearAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);
for (int i = 0; i < RoadMapList.size(); ++i) {
- if (RoadMapList[i].type == 100) {
+ if (RoadMapList[i].type == GENERAL_MAP) {
// 姣忔潯绾块兘妫�娴�
for (int j = 0; j < RoadMapList[i].greenLineNum; ++j) {
@@ -621,7 +646,7 @@
}
for (int i = 0; i < RoadMapList.size(); ++i) {
- if (RoadMapList[i].type == 100) {
+ if (RoadMapList[i].type == GENERAL_MAP) {
// 姣忔潯绾块兘妫�娴�
for (int j = 0; j < RoadMapList[i].triggerLineNum; ++j) {
diff --git a/lib/src/main/cpp/test_items2/road_exam.h b/lib/src/main/cpp/test_items2/road_exam.h
index 7cbff05..16d0fd2 100644
--- a/lib/src/main/cpp/test_items2/road_exam.h
+++ b/lib/src/main/cpp/test_items2/road_exam.h
@@ -7,7 +7,20 @@
#include "../driver_test.h"
+#define GENERAL_MAP 100
+#define DRIVE_STRAIGHT_MAP 101
+#define OP_GEAER_MAP 102
+#define STOP_CAR_MAP 103
+#define THROUGH_INTERSECTION_MAP 104
+#define TURN_LEFT_MAP 105
+#define TURN_RIGHT_MAP 106
+#define THROUGH_ZEBRA_CROSSING_MAP 107
+#define THROUGH_SCHOOL_MAP 108
+#define THROUGH_BUS_STATION_MAP 109
+#define TURN_AROUND_MAP 110
+
void InitRoadExam(void);
void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime);
+bool ExitSonArea(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car);
#endif //MYAPPLICATION2_ROAD_EXAM_H
diff --git a/lib/src/main/cpp/test_items2/through_something.cpp b/lib/src/main/cpp/test_items2/through_something.cpp
new file mode 100644
index 0000000..33fe453
--- /dev/null
+++ b/lib/src/main/cpp/test_items2/through_something.cpp
@@ -0,0 +1,114 @@
+//
+// Created by YY on 2020/3/20.
+//
+
+#include "through_something.h"
+#include "../driver_test.h"
+#include "../Geometry.h"
+#include "../native-lib.h"
+#include "../jni_log.h"
+#include "../test_common/car_sensor.h"
+#include "road_exam.h"
+
+#define DEBUG(fmt, args...) LOGD("<through_something> <%s>: " fmt, __func__, ##args)
+
+static const double LASTEST_BREAK_POINT = 30.0;
+static const double DISTANCE_STOP_CAR_TO_STOP_LINE = 5.0;
+
+static int breakActive;
+static int stopActive;
+
+void StartThroughExam(int index, LIST_ROAD_MAP &RoadMapList)
+{
+ if (index == -1)
+ return;
+ DEBUG("杩涘叆璺�冨瓙鍦板浘 index = %d id = %d item = %d", index, RoadMapList[index].id, RoadMapList[index].type);
+ if (!RoadMapList[index].tts.empty()) {
+ PlayTTS(RoadMapList[index].tts.c_str(), 0);
+ }
+ breakActive = 0;
+ stopActive = 0;
+}
+
+int ExecuteThroughExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car,
+ LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
+{
+ PointF p1, p2;
+
+ double distance2StopLine = DistanceOf(car->carXY[car->axial[AXIAL_FRONT]],
+ RoadMapList[index].stopLine);
+ p1.X = RoadMapList[index].stopLine.X1;
+ p1.Y = RoadMapList[index].stopLine.Y1;
+ p2.X = RoadMapList[index].stopLine.X2;
+ p2.Y = RoadMapList[index].stopLine.Y2;
+
+ if (IntersectionOfLine(p1, p2, car->carXY[car->axial[AXIAL_FRONT]]) == -1) {
+ distance2StopLine = -distance2StopLine;
+ }
+
+ // 璺濈鍋滄绾�30绫冲墠鏄惁鏈夊埞杞﹀姩浣�
+ if (breakActive == 0) {
+ if (distance2StopLine >= LASTEST_BREAK_POINT) {
+ if (ReadCarStatus(BREAK) == BREAK_ACTIVE) {
+ breakActive = 1;
+ }
+ } else {
+ breakActive = -1;
+ // 涓嶆寜瑙勫畾鍑忛�燂紝涓嶅悎鏍�
+ DEBUG("涓嶈俯涓嬪埞杞︼紝涓嶅悎鏍�");
+ switch (RoadMapList[index].type) {
+ case THROUGH_INTERSECTION_MAP:
+ AddExamFault(41, rtkTime);
+ break;
+ case TURN_LEFT_MAP:
+ AddExamFault(43, rtkTime);
+ break;
+ case TURN_RIGHT_MAP:
+ AddExamFault(46, rtkTime);
+ break;
+ case THROUGH_ZEBRA_CROSSING_MAP:
+ AddExamFault(48, rtkTime);
+ break;
+ case THROUGH_SCHOOL_MAP:
+ AddExamFault(49, rtkTime);
+ break;
+ case THROUGH_BUS_STATION_MAP:
+ AddExamFault(50, rtkTime);
+ break;
+ default:
+ break;
+ }
+ }
+ }
+
+ // 鏈夊仠姝㈡爣璁扮殑锛屾槸鍚﹀仠杞︼紙鍋滄绾�5绫冲唴鏈夊仠杞﹀姩浣滐級
+ if (RoadMapList[index].flagStop != 0 && stopActive == 0) {
+ if (distance2StopLine < 0.0) {
+ // 涓嶅仠杞︾灜鏈涳紝涓嶅悎鏍�
+ DEBUG("涓嶅仠杞︾灜鏈�");
+ switch (RoadMapList[index].type) {
+ case THROUGH_INTERSECTION_MAP:
+ AddExamFault(42, rtkTime);
+ break;
+ case TURN_LEFT_MAP:
+ AddExamFault(44, rtkTime);
+ break;
+ case TURN_RIGHT_MAP:
+ AddExamFault(47, rtkTime);
+ break;
+ default:
+ break;
+ }
+ } else if (distance2StopLine < DISTANCE_STOP_CAR_TO_STOP_LINE) {
+ if (moveDirect == 0) {
+ stopActive = 1;
+ }
+ }
+ }
+
+ if (ExitSonArea(index, RoadMapList, car))
+ return -1;
+
+ return index;
+}
+
diff --git a/lib/src/main/cpp/test_items2/through_something.h b/lib/src/main/cpp/test_items2/through_something.h
new file mode 100644
index 0000000..a6967e9
--- /dev/null
+++ b/lib/src/main/cpp/test_items2/through_something.h
@@ -0,0 +1,14 @@
+//
+// Created by YY on 2020/3/20.
+//
+
+#ifndef MYAPPLICATION2_THROUGH_SOMETHING_H
+#define MYAPPLICATION2_THROUGH_SOMETHING_H
+
+#include "../driver_test.h"
+
+void StartThroughExam(int index, LIST_ROAD_MAP &RoadMapList);
+int ExecuteThroughExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car,
+ LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime);
+
+#endif //MYAPPLICATION2_THROUGH_SOMETHING_H
--
Gitblit v1.8.0