From a073dc3c983b4c56c5da92642c9ad11995bdb844 Mon Sep 17 00:00:00 2001 From: yy1717 <fctom1215@outlook.com> Date: 星期一, 11 五月 2020 18:05:31 +0800 Subject: [PATCH] 新的科目三 --- lib/src/main/cpp/test_items2/road_exam.h | 6 lib/src/main/cpp/driver_test.cpp | 81 +++ lib/src/main/cpp/test_items2/through_something.h | 2 lib/src/main/cpp/master/comm_if.cpp | 260 +++++++++++ lib/src/main/cpp/test_items2/through_something.cpp | 75 ++ lib/src/main/cpp/common/net.cpp | 68 +- lib/src/main/cpp/test_items2/road_exam.cpp | 674 ++++++++++++++++++++++++++++++ lib/src/main/cpp/test_common/Geometry.h | 2 lib/src/main/cpp/driver_test.h | 70 ++- lib/src/main/cpp/test_common/Geometry.cpp | 33 + lib/src/main/java/com/anyun/exam/lib/RemoteService.java | 2 11 files changed, 1,178 insertions(+), 95 deletions(-) diff --git a/lib/src/main/cpp/common/net.cpp b/lib/src/main/cpp/common/net.cpp index 1ccea8c..3ea1083 100644 --- a/lib/src/main/cpp/common/net.cpp +++ b/lib/src/main/cpp/common/net.cpp @@ -17,6 +17,8 @@ #include "net.h" #include "../jni_log.h" +#define DEBUG(fmt, args...) LOGD("<net> <%s>: " fmt, __func__, ##args) + using namespace std; /************************************************************* @@ -31,14 +33,14 @@ bool found_first = false; if ((hptr = gethostbyname(host_name)) == NULL) { - LOGE("gethostbyname error\n"); + DEBUG("gethostbyname error\n"); return -1; } - LOGD("official hostname: %s\n", hptr->h_name); + DEBUG("official hostname: %s\n", hptr->h_name); for (pptr = hptr->h_aliases; *pptr != NULL; pptr++) { - LOGD("alias:%s\n", *pptr); + DEBUG("alias:%s\n", *pptr); } switch(hptr->h_addrtype) @@ -46,7 +48,7 @@ case AF_INET: { for (pptr = hptr->h_addr_list; *pptr != NULL; pptr++) { - LOGD("addrsss:%s\n", inet_ntop(hptr->h_addrtype, *pptr, str, sizeof(str))); + DEBUG("addrsss:%s\n", inet_ntop(hptr->h_addrtype, *pptr, str, sizeof(str))); if (!found_first) { strcpy(net_addr, str); found_first = true; @@ -56,7 +58,7 @@ } case AF_INET6: default: - LOGD("unknown address type\n"); + DEBUG("unknown address type\n"); break; } @@ -80,53 +82,53 @@ // int value, value_len; // getsockopt(fd, SOL_SOCKET, SO_RCVBUF, &value, &value_len); -// LOGD("keepalive 0 %d", value); +// DEBUG("keepalive 0 %d", value); // value = 128; // setsockopt(fd, SOL_SOCKET, SO_SNDBUF, &value, sizeof(value)); -// LOGD("keepalive 1 %d", value); +// DEBUG("keepalive 1 %d", value); // getsockopt(fd, SOL_SOCKET, SO_SNDBUF, &value, &value_len); -// LOGD("keepalive 1 %d", value); +// DEBUG("keepalive 1 %d", value); /* Set: use keepalive on fd, default 0 */ alive = 1; if (setsockopt(fd, SOL_SOCKET, SO_KEEPALIVE, &alive, sizeof(alive)) != 0) { - LOGE("TCP Set keepalive error"); + DEBUG("TCP Set keepalive error"); return -1; } /* 20 Seconds not data, send keeplive packet, default 7200 */ idle = 20; if (setsockopt (fd, SOL_TCP, TCP_KEEPIDLE, &idle, sizeof(idle)) != 0) { - LOGE("TCP Set keepalive idle error"); + DEBUG("TCP Set keepalive idle error"); return -1; } /* If not recv respond, After 5 seconds retry, default 75 */ intv = 5; if (setsockopt (fd, SOL_TCP, TCP_KEEPINTVL, &intv, sizeof(intv)) != 0) { - LOGE("TCP Set keepalive intv error"); + DEBUG("TCP Set keepalive intv error"); return -1; } /* If try 9 times and fail, we consider the tcp is disconnected, default 9 */ cnt = 9; if (setsockopt (fd, SOL_TCP, TCP_KEEPCNT, &cnt, sizeof(cnt)) != 0) { - LOGE("TCP Set keepalive cnt error"); + DEBUG("TCP Set keepalive cnt error"); return -1; } /* int timeout = 10000; // 10绉� if (setsockopt (fd, IPPROTO_TCP, TCP_USER_TIMEOUT, &timeout, sizeof(timeout)) != 0) { - LOGE("TCP Set keepalive timeout error"); + DEBUG("TCP Set keepalive timeout error"); return -1; }*/ - LOGD("TCP Set keepalive OK"); + DEBUG("TCP Set keepalive OK"); return 0; } @@ -142,13 +144,13 @@ int error_value; socklen_t error_value_len; - LOGI("%s", __func__); + DEBUG("tcp_connect... %s : %d", ip, port); error_value_len = sizeof( error_value ); if((soc = socket(PF_INET, SOCK_STREAM, 0)) == -1) { - LOGE("%s: socket", __func__); + DEBUG("%s: socket", __func__); return -1; } @@ -160,13 +162,13 @@ // Set non-blocking if( (arg = fcntl(soc, F_GETFL, NULL)) < 0) { - LOGE("%s: fcntl( F_GETFL ) error", __func__); + DEBUG("%s: fcntl( F_GETFL ) error", __func__); goto TCP_CONNECT_1; } if( fcntl(soc, F_SETFL, arg | O_NONBLOCK) < 0) { - LOGE( "%s: fcntl( F_SETFL ) error", __func__); + DEBUG( "%s: fcntl( F_SETFL ) error", __func__); goto TCP_CONNECT_1; } @@ -198,12 +200,12 @@ if(fds_ret < 0) { - LOGE( "%s: TCP select error", __func__); + DEBUG( "%s: TCP select error", __func__); goto TCP_CONNECT_1; } else if(fds_ret == 0) { - LOGE("%s: TCP Connect Timeout %ld", __func__, tv.tv_sec); + DEBUG("%s: TCP Connect Timeout %ld", __func__, tv.tv_sec); goto TCP_CONNECT_1; } else if(FD_ISSET(soc, &rdfds)) @@ -215,7 +217,7 @@ } else { - LOGE("%s: some error occur in tcp_connect()", __func__); + DEBUG("%s: some error occur in tcp_connect()", __func__); goto TCP_CONNECT_1; } @@ -244,11 +246,11 @@ arg &= ~O_NONBLOCK; if( fcntl(soc, F_SETFL, arg) < 0) { - LOGE( "%s: fcntl( F_SETFL ) error", __func__); + DEBUG( "%s: fcntl( F_SETFL ) error", __func__); goto TCP_CONNECT_1; } - LOGI("%s: tcp connected %s: %d", __func__, ip, port); + DEBUG("%s: tcp connected %s: %d", __func__, ip, port); return( soc ); @@ -317,19 +319,19 @@ fds_ret = select(fd + 1, NULL, &rdfds, NULL, &tv); if (fds_ret < 0) { - LOGE("tcp error send select error"); + DEBUG("tcp error send select error"); return -1; } else if(fds_ret == 0) { - LOGE("tcp error Occur failure(such as line disconnect)"); + DEBUG("tcp error Occur failure(such as line disconnect)"); //Occur failure(such as line disconnect) ret = -1; } else if(FD_ISSET(fd, &rdfds)) { ret = send(fd, buf, len, 0); if(ret == -1) { - LOGE("tcp error TCP Send Error"); + DEBUG("tcp error TCP Send Error"); } } else { - LOGE("tcp error tcp send has error\n"); + DEBUG("tcp error tcp send has error\n"); } return ret; @@ -375,7 +377,7 @@ void DisconnectTCP(int fd) { - LOGI("DisconnectTCP fd = %d", fd); + DEBUG("DisconnectTCP fd = %d", fd); if (fd >= 0) { shutdown(fd, SHUT_RDWR); close(fd); @@ -426,12 +428,12 @@ if (fds_ret < 0) { - LOGE("UDP send select error"); + DEBUG("UDP send select error"); return -1; } else if(fds_ret == 0) { - LOGE("Occur failure(such as line disconnect)"); + DEBUG("Occur failure(such as line disconnect)"); ret = -1; } else if(FD_ISSET(fd, &rdfds)) @@ -447,12 +449,12 @@ if(ret == -1) { - LOGE("UDP Send Error"); + DEBUG("UDP Send Error"); } } else { - LOGE("UDP send has error\n"); + DEBUG("UDP send has error\n"); } return ret; @@ -473,7 +475,7 @@ if (errno == EINTR || errno == EWOULDBLOCK || errno == EAGAIN) { return 0; } else { - LOGE("UDP ERROR!! = %d", errno); + DEBUG("UDP ERROR!! = %d", errno); return -1; } } diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp index 22b8ff2..b0811d2 100644 --- a/lib/src/main/cpp/driver_test.cpp +++ b/lib/src/main/cpp/driver_test.cpp @@ -66,6 +66,7 @@ static LIST_AREA_MAP AreaMapList; static Polygon RoadMapPoints; +static road_exam_map RoadMap; static LIST_ROAD_MAP RoadMapList; @@ -111,7 +112,10 @@ RoadMapPoints.num = 0; RoadMapPoints.point = NULL; - RoadMapList.clear(); +// RoadMapList.clear(); + RoadMap.roads.clear(); + RoadMap.specialAreas.clear(); + RoadMap.triggerLines.clear(); CarSensorInit(); @@ -190,11 +194,58 @@ { if (ExamStart) return; + DEBUG("娓呴櫎鏃х殑璺�冨湴鍥�"); + if (RoadMapPoints.point != NULL) { free(RoadMapPoints.point); } RoadMapPoints.num = 0; + for (int i = 0; i < RoadMap.roads.size(); ++i) { + for (int j = 0; j < RoadMap.roads[i].leftEdge.size(); ++j) { + RoadMap.roads[i].leftEdge[j].points.clear(); + vector<PointF>().swap(RoadMap.roads[i].leftEdge[j].points); + } + RoadMap.roads[i].leftEdge.clear(); + vector<edge_t>().swap(RoadMap.roads[i].leftEdge); + + for (int j = 0; j < RoadMap.roads[i].rightEdge.size(); ++j) { + RoadMap.roads[i].rightEdge[j].points.clear(); + vector<PointF>().swap(RoadMap.roads[i].rightEdge[j].points); + } + RoadMap.roads[i].rightEdge.clear(); + vector<edge_t>().swap(RoadMap.roads[i].rightEdge); + + for (int j = 0; j < RoadMap.roads[i].separate.size(); ++j) { + for (int k = 0; k < RoadMap.roads[i].separate[j].lines.size(); ++k) { + for (int m = 0; m < RoadMap.roads[i].separate[j].lines[k].size(); ++m) { + RoadMap.roads[i].separate[j].lines[k][m].points.clear(); + vector<PointF>().swap(RoadMap.roads[i].separate[j].lines[k][m].points); + } + + RoadMap.roads[i].separate[j].lines[k].clear(); + vector<segment_t>().swap(RoadMap.roads[i].separate[j].lines[k]); + } + RoadMap.roads[i].separate[j].lines.clear(); + vector<vector<segment_t>>().swap(RoadMap.roads[i].separate[j].lines); + } + RoadMap.roads[i].separate.clear(); + vector<separate_t>().swap(RoadMap.roads[i].separate); + } + + RoadMap.roads.clear(); + vector<road_t>().swap(RoadMap.roads); + + for (int i = 0; i < RoadMap.specialAreas.size(); ++i) { + RoadMap.specialAreas[i].area.clear(); + vector<PointF>().swap(RoadMap.specialAreas[i].area); + } + RoadMap.specialAreas.clear(); + vector<special_area_t>().swap(RoadMap.specialAreas); + + RoadMap.triggerLines.clear(); + vector<trigger_line_t>().swap(RoadMap.triggerLines); +/* for (int i = 0; i < RoadMapList.size(); ++i) { struct road_exam_map map = RoadMapList[i]; @@ -244,7 +295,18 @@ } } - RoadMapList.clear(); + RoadMapList.clear();*/ +} + +void SetRoadMap(road_exam_map &map) +{ + if (ExamStart) return; + + RoadMap.roads.assign(map.roads.begin(), map.roads.end()); + RoadMap.specialAreas.assign(map.specialAreas.begin(), map.specialAreas.end()); + RoadMap.triggerLines.assign(map.triggerLines.begin(), map.triggerLines.end()); + + DEBUG("寰楀埌鏂扮殑璺�冨湴鍥� 璺暟閲� %d 鐗规畩鍖哄煙鏁伴噺 %d 瑙﹀彂绾挎暟閲� %d", RoadMap.roads.size(), RoadMap.specialAreas.size(), RoadMap.triggerLines.size()); } void SetRoadMapPoints(vector<double> &mapPoints) @@ -271,7 +333,7 @@ vector<int> area, vector<int> stopLine) { - struct road_exam_map newMap; +/* struct road_exam_map newMap; newMap.id = id; newMap.type = type; @@ -400,7 +462,7 @@ newMap.stopLine.Y2); } - RoadMapList.push_back(newMap); + RoadMapList.push_back(newMap);*/ } void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire, @@ -554,7 +616,7 @@ // err = true; // MA_SendExamStatus(0, -3); } - if (type != TEST_TYPE_AREA && (RoadMapPoints.num == 0 || RoadMapPoints.point == NULL || RoadMapList.size() == 0)) { + if (type != TEST_TYPE_AREA && RoadMap.roads.size() == 0) { DEBUG("娌℃湁璺�冨湴鍥�"); err = true; MA_SendExamStatus(0, -1); @@ -645,8 +707,8 @@ brief.pointNum = CarModel->pointNum; brief.point = (double *) malloc(CarModel->pointNum * 2 * sizeof(double)); for (int i = 0, j = 0; i < CarModel->pointNum; ++i) { - brief.point[j++] = CarModel->carXY[i].X; - brief.point[j++] = CarModel->carXY[i].Y; + brief.point[j++] = round(CarModel->carXY[i].X, 4); + brief.point[j++] = round(CarModel->carXY[i].Y, 4); } MA_SendCarPosition(&brief); @@ -801,7 +863,7 @@ // 姹囨姤鐏厜鑰冭瘯缁撴潫 if (exam_dummy_light == 2) { DEBUG("鐏厜鑰冭瘯缁撴潫"); - InitRoadExam(); + InitRoadExam(RoadMap); } } } @@ -819,7 +881,8 @@ if (ExamType != TEST_TYPE_AREA) { if (exam_dummy_light == 2) { - TestRoadGeneral(RoadMapList, CarModel, CarModelList, speed, move, rtkTime); +// TestRoadGeneral(RoadMapList, CarModel, CarModelList, speed, move, rtkTime); + TestRoadGeneral(RoadMap, CarModel, CarModelList, speed, move, rtkTime); } } else { TestAreaGeneral(AreaMapList, CarModel, CarModelList, speed, move, azimuth, rtkTime); diff --git a/lib/src/main/cpp/driver_test.h b/lib/src/main/cpp/driver_test.h index 7d3d221..46a6abc 100644 --- a/lib/src/main/cpp/driver_test.h +++ b/lib/src/main/cpp/driver_test.h @@ -92,12 +92,14 @@ int wrong_id; }; -struct trigger_line_t { + +struct trigger_line_tx { int triggerMapId; Polygon line; }; -struct road_exam_map { + +struct road_exam_mapx { int id; int type; @@ -113,7 +115,7 @@ Polygon *redArea; int triggerLineNum; - trigger_line_t *triggerLine; + trigger_line_tx *triggerLine; int roadEdgeLineNum; // 閬撹矾杈圭嚎锛屽彧鏈夌洿绾块┚椹躲�侀潬杈瑰仠杞︽墠鏈� Polygon *roadEdgeLine; @@ -123,10 +125,23 @@ int flagStop; // 鍒拌揪寮�濮嬬嚎鍓嶏紝鏄惁闇�瑕佸仠杞� }; -typedef vector<struct road_exam_map> LIST_ROAD_MAP; +typedef vector<struct road_exam_mapx> LIST_ROAD_MAP; -#define ROAD_DOTTED_LINE 0 -#define ROAD_SOLID_LINE 1 +#define LINE_DOTTED 0 +#define LINE_SOLID 1 +#define LINE_HALF_SOLID_LEFT 2 +#define LINE_HALF_SOLID_RIGHT 3 + +//杞﹂亾鏂瑰悜锛堟寜浣嶇粍鍚堬級,濡傛灉涓�0锛屽垯琛ㄦ棤杞﹂亾鏂瑰悜璇存槑锛� +#define LANE_FORWARD 0x01 +#define LANE_LEFT 0x02 +#define LANE_RIGHT 0x04 +#define LANE_BACKWARD 0x08 + +#define ZEBRA_CROSSING 0 +#define SCHOOL_AREA 1 +#define BUS_STATION_AREA 2 +#define GRID_AREA 3 typedef struct { int character; // 灞炴�э紙瀹炵嚎銆佽櫄绾匡紝鏈変簺鍙互鎺夊ご鐨勮矾娈碉級 @@ -134,42 +149,51 @@ } edge_t; typedef struct { - int character; // 灞炴�с�婂疄绾裤�佽櫄绾裤�� - std::vector<Polygon> lines; // 姣忎竴鏉$嚎 + int character; // 灞炴�с�婂疄绾裤�佽櫄绾裤�佸崐瀹炲崐铏氱嚎銆� + int left_lane_direct; + int right_lane_direct; + std::vector<PointF> points; +} segment_t; + +// 涓�缁勫钩琛岀殑鍒嗛亾绾� +typedef struct { + std::vector<std::vector<segment_t>> lines; // 姣忔鍩熶笅鐨勫钩琛岀殑涓�缁勭嚎 } separate_t; -struct road_t { +typedef struct { int id; Line startLine; Line stopLine; int active; // 鍒拌揪璺彛灏鹃儴鐨勮杩涙柟鍚� + int targetRoad; + int stopFlag; string tts; - Polygon area; + Polygon area; // 鏁翠釜閬撹矾鍖哄煙 std::vector<edge_t> leftEdge; std::vector<edge_t> rightEdge; std::vector<separate_t> separate; -}; +} road_t; -struct special_area_t { +typedef struct { int id; int road; - int character; - Line startLine; - Polygon area; -}; + int type; + std::vector<PointF> area; // 浜鸿閬撶瓑鍙充晶2鐐癸紝缃戞牸绾�4鐐� + std::vector<PointF> leftPoints; // 瀵瑰簲鍒伴亾璺乏渚х殑鐐� +} special_area_t; -struct trigger2_line_t { +typedef struct { int id; int road; int active; string tts; Line line; -}; +} trigger_line_t; -struct road_exam2_map { - std::vector<struct road_t> roads; - std::vector<struct special_area_t> specialAreas; - std::vector<struct trigger2_line_t> triggerLines; +struct road_exam_map { + std::vector<road_t> roads; + std::vector<special_area_t> specialAreas; + std::vector<trigger_line_t> triggerLines; }; struct area_exam_map { @@ -193,6 +217,8 @@ void CleanRoadMap(void); +void SetRoadMap(road_exam_map &map); + void SetRoadMapPoints(vector<double> &mapPoints); void AddRoadMapParent(int id, int type, string tts, int stopFlag, vector<vector<int>> &redLines, vector<vector<int>> &redAreas, vector<vector<int>> &greenLines, diff --git a/lib/src/main/cpp/master/comm_if.cpp b/lib/src/main/cpp/master/comm_if.cpp index 2dd5537..c99a0d4 100644 --- a/lib/src/main/cpp/master/comm_if.cpp +++ b/lib/src/main/cpp/master/comm_if.cpp @@ -37,7 +37,8 @@ #define ID_SM_RTK_PLAT_LOGIN 0x0005 #define ID_SM_READ_MAP 0x0006 #define ID_MS_MAP 0x8006 -#define ID_MS_ROAD_MAP 0x8013 +#define ID_MS_ROAD_MAP 0x8014 +#define ID_MS_ROAD_MAP2 0x8013 #define ID_SM_READ_CAR 0x0007 #define ID_MS_CAR 0x8007 @@ -745,6 +746,263 @@ } break; } + case ID_MS_ROAD_MAP2: { + Document doc; + doc.Parse(value); + if (!doc.HasParseError()) { + DEBUG("寮�濮嬭В鏋愯矾鑰冨湴鍥�"); + + vector<PointF> mapPoints; + mapPoints.clear(); + + if (doc.HasMember("points")) { + const Value &s = doc["points"]; + + DEBUG("寰楀埌鎵�鏈夌偣 鍏� %d", s.Size()/2); + + int n = 0; + PointF temp; + + // X-Y鍧愭爣闆嗗悎 + for (Value::ConstValueIterator itr = s.Begin(); + itr != s.End(); ++itr, ++n) { + if ((n % 2) == 0) { + temp.X = (*itr).GetDouble(); + } else { + temp.Y = (*itr).GetDouble(); + mapPoints.push_back(temp); + } + } + } + + road_exam_map map; + + map.roads.clear(); + map.specialAreas.clear(); + map.triggerLines.clear(); + + if (doc.HasMember("road")) { + const Value &a = doc["road"]; + + if (a.IsArray()) { + for (Value::ConstValueIterator itr = a.Begin(); itr != a.End(); ++itr) { + road_t road; + + if (itr->HasMember("id")) { + const Value &s = (*itr)["id"]; + DEBUG("璺痠d %d", s.GetInt()); + road.id = s.GetInt(); + } + if (itr->HasMember("start_line")) { + const Value &a2 = (*itr)["start_line"]; + + PointF p1, p2; + int n = 0; + + for (Value::ConstValueIterator itr2 = a2.Begin(); itr2 != a2.End(); ++itr2, ++n) { + DEBUG("璧峰绾� %d", (*itr2).GetInt()); + if (n == 0) { + p1 = mapPoints[(*itr2).GetInt()]; + } else if (n == 1) { + p2 = mapPoints[(*itr2).GetInt()]; + } + } + + MakeLine(&road.startLine, &p1, &p2); + } + if (itr->HasMember("stop_line")) { + const Value &a2 = (*itr)["stop_line"]; + + PointF p1, p2; + int n = 0; + + for (Value::ConstValueIterator itr2 = a2.Begin(); itr2 != a2.End(); ++itr2, ++n) { + DEBUG("缁撴潫绾� %d", (*itr2).GetInt()); + if (n == 0) { + p1 = mapPoints[(*itr2).GetInt()]; + } else if (n == 1) { + p2 = mapPoints[(*itr2).GetInt()]; + } + } + + MakeLine(&road.stopLine, &p1, &p2); + } + if (itr->HasMember("active")) { + const Value &s = (*itr)["active"]; + DEBUG("璺彛鍔ㄤ綔 %d", s.GetInt()); + + road.active = s.GetInt(); + } + if (itr->HasMember("tts")) { + const Value &s = (*itr)["tts"]; + road.tts = s.GetString(); + } + if (itr->HasMember("stop_flag")) { + const Value &s = (*itr)["stop_flag"]; + road.stopFlag = s.GetInt(); + } + if (itr->HasMember("next_road")) { + const Value &s = (*itr)["next_road"]; + road.targetRoad = s.GetInt(); + } + if (itr->HasMember("left_edge")) { + const Value &a2 = (*itr)["left_edge"]; + DEBUG("宸﹁竟绾�"); + + if (a2.IsArray()) { + for (Value::ConstValueIterator itr2 = a2.Begin(); itr2 != a2.End(); ++itr2) { + edge_t edge; + + if (itr2->HasMember("type")) { + const Value &s = (*itr2)["type"]; + DEBUG("\t绫诲瀷 %d", s.GetInt()); + + edge.character = s.GetInt(); + } + if (itr2->HasMember("line")) { + const Value &a3 = (*itr2)["line"]; + if (a3.IsArray()) { + for (Value::ConstValueIterator itr3 = a3.Begin(); itr3 != a3.End(); ++itr3) { + DEBUG("\t绾跨偣 %d", (*itr3).GetInt()); + edge.points.push_back(mapPoints[(*itr3).GetInt()]); + } + } + } + + road.leftEdge.push_back(edge); + } + } + } + if (itr->HasMember("right_edge")) { + const Value &a2 = (*itr)["right_edge"]; + DEBUG("鍙宠竟绾�"); + if (a2.IsArray()) { + for (Value::ConstValueIterator itr2 = a2.Begin(); itr2 != a2.End(); ++itr2) { + edge_t edge; + + if (itr2->HasMember("type")) { + const Value &s = (*itr2)["type"]; + DEBUG("\t绫诲瀷 %d", s.GetInt()); + edge.character = s.GetInt(); + } + if (itr2->HasMember("line")) { + const Value &a3 = (*itr2)["line"]; + if (a3.IsArray()) { + for (Value::ConstValueIterator itr3 = a3.Begin(); itr3 != a3.End(); ++itr3) { + DEBUG("\t绾跨偣 %d", (*itr3).GetInt()); + edge.points.push_back(mapPoints[(*itr3).GetInt()]); + } + } + } + + road.rightEdge.push_back(edge); + } + } + } + if (itr->HasMember("separate")) { + const Value &a2 = (*itr)["separate"]; + DEBUG("娈垫暟閲� %d", a2.Size()); + + for (Value::ConstValueIterator itr2 = a2.Begin(); itr2 != a2.End(); ++itr2) { + DEBUG("\t绾挎暟閲� %d", (*itr2).Size()); + separate_t sep; + + for (Value::ConstValueIterator itr3 = (*itr2).Begin(); itr3 != (*itr2).End(); ++itr3) { + DEBUG("\t\t鑺傛暟閲� %d", (*itr3).Size()); + vector<segment_t> sline; + + for (Value::ConstValueIterator itr4 = (*itr3).Begin(); itr4 != (*itr3).End(); ++itr4) { + const Value &type = (*itr4)["type"]; + const Value &line = (*itr4)["line"]; + + segment_t seg; + + DEBUG("\t\t\t鑺傜被鍨� = %d", type.GetInt()); + seg.character = type.GetInt(); + + if ((*itr4).HasMember("left_lane_direct")) { + const Value &dir = (*itr4)["left_lane_direct"]; + DEBUG("\t\t\t宸﹁溅閬撴柟鍚� %d", dir.GetInt()); + seg.left_lane_direct = dir.GetInt(); + } else { + seg.left_lane_direct = 0; + } + + if ((*itr4).HasMember("right_lane_direct")) { + const Value &dir = (*itr4)["right_lane_direct"]; + DEBUG("\t\t\t鍙宠溅閬撴柟鍚� %d", dir.GetInt()); + seg.right_lane_direct = dir.GetInt(); + } else { + seg.right_lane_direct = 0; + } + + for (Value::ConstValueIterator itr5 = line.Begin(); itr5 != line.End(); ++itr5) { + DEBUG("\t\t\t鐐� = %d", (*itr5).GetInt()); + + seg.points.push_back(mapPoints[(*itr5).GetInt()]); + } + + sline.push_back(seg); + } + sep.lines.push_back(sline); + } + + road.separate.push_back(sep); + } + } + + map.roads.push_back(road); + } + } + } + + if (doc.HasMember("special_area")) { + const Value &a = doc["special_area"]; + + vector<special_area_t> specialAreas; + + for (Value::ConstValueIterator itr = a.Begin(); + itr != a.End(); ++itr) { + special_area_t specialArea; + + if (itr->HasMember("type")) { + const Value &s = (*itr)["type"]; + specialArea.type = s.GetInt(); + } + if (itr->HasMember("id")) { + const Value &s = (*itr)["id"]; + specialArea.id = s.GetInt(); + } + if (itr->HasMember("road")) { + const Value &s = (*itr)["road"]; + specialArea.road = s.GetInt(); + } + + if (itr->HasMember("area")) { + const Value &a2 = (*itr)["area"]; + + for (Value::ConstValueIterator itr2 = a2.Begin(); + itr2 != a2.End(); ++itr2) { + specialArea.area.push_back(mapPoints[(*itr2).GetInt()]); + } + } + + map.specialAreas.push_back(specialArea); + } + } + if (doc.HasMember("trigger_line")) { + + } + DEBUG("鍦板浘瑙f瀽瀹屾瘯"); + + CleanRoadMap(); + SetRoadMap(map); + } else { + DEBUG("############## 鍦板浘瑙f瀽鍑洪敊###################"); + } + + break; + } case ID_MS_MAP: { Document doc; doc.Parse(value); diff --git a/lib/src/main/cpp/test_common/Geometry.cpp b/lib/src/main/cpp/test_common/Geometry.cpp index 8c90d31..7f8c02c 100644 --- a/lib/src/main/cpp/test_common/Geometry.cpp +++ b/lib/src/main/cpp/test_common/Geometry.cpp @@ -11,6 +11,9 @@ #include <malloc.h> #include <initializer_list> #include <cctype> +#include <iosfwd> +#include <iomanip> +#include <sstream> #include "../jni_log.h" @@ -37,6 +40,13 @@ inline double toDegree(double radians) { return (radians * 180.0) / M_PI; +} + +double round(double number, unsigned int bits) { + stringstream ss; + ss << setiosflags(ios::fixed) << setprecision(bits) << number; + ss >> number; + return number; } void MakeLine(Line *line, const PointF *p1, const PointF *p2) @@ -442,6 +452,7 @@ } } +// 0 - straight, 1 - left, -1 - right, 2 - front, -2 - back int IntersectionOfLine(PointF p, Line line) { PointF p1, p2; @@ -451,7 +462,7 @@ p2.X = line.X2; p2.Y = line.Y2; - IntersectionOfLine(p1, p2, p); + return IntersectionOfLine(p1, p2, p); } /*************************************************************** @@ -514,6 +525,26 @@ return false; } +bool VerticalPointOnLine(PointF point, Line line, PointF &vp) +{ + PointF p1, p2; + + p1.X = line.X1; + p1.Y = line.Y1; + + p2.X = line.X2; + p2.Y = line.Y2; + + PointF pv = GetVerticalPoint(p1, p2, point); + + if (isEqual2(pv.X, MIN(p1.X, p2.X)) || isEqual2(pv.X, MAX(p1.X, p2.X)) || + (pv.X > MIN(p1.X, p2.X) && pv.X < MAX(p1.X, p2.X))) { + vp = pv; + return true; + } + return false; +} + /**************************************************************** * p3 * | 'L' diff --git a/lib/src/main/cpp/test_common/Geometry.h b/lib/src/main/cpp/test_common/Geometry.h index cfe73be..b07dd52 100644 --- a/lib/src/main/cpp/test_common/Geometry.h +++ b/lib/src/main/cpp/test_common/Geometry.h @@ -37,6 +37,7 @@ inline double toDegree(double radians); inline bool isEqual(double a, double b); inline bool isEqual2(double a, double b); +double round(double number, unsigned int bits); void MakeLine(Line *line, const PointF *p1, const PointF *p2); void MakePolygon(Polygon *polygon, std::initializer_list<PointF> point_set); @@ -60,6 +61,7 @@ int IntersectionOfLine(PointF p, Line line); PointF GetVerticalPoint(PointF p1, PointF p2, PointF p3); bool VerticalPointOnLine(PointF point, Line line); +bool VerticalPointOnLine(PointF point, Line line, PointF &vp); PointF Calc3Point(PointF p1, PointF p2, double L, char dir); PointF PointExtend(PointF ori, double length, double yaw); #endif //GUI_GEOMETRY_H diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp index de97518..f712b10 100644 --- a/lib/src/main/cpp/test_items2/road_exam.cpp +++ b/lib/src/main/cpp/test_items2/road_exam.cpp @@ -18,6 +18,7 @@ #include <vector> #include <list> +#include <map> #include <string> #include <cstdlib> @@ -39,6 +40,9 @@ static const int TURN_CHECK_INTERVAL = 500; const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3; const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1; + +static const int FIND_POSITION = -2; +static const int INVALID_POSITION = -1; static bool occurCrashRedLine; static bool occurCrashGreenLine; @@ -76,9 +80,16 @@ static bool handBreakActive = false; static bool reportRPMOver = false; +static int currCarOnRoadIndex; + static const uint32_t TURN_ERROR_COLD_TIME = D_SEC(10); static bool turnError13Cold, turnError14Cold; +static struct car_on_lane { + int road; + int separate; + int lane; +} CarOnLane; static const int MAX_ENGINE_RPM = 2500; static const double START_CAR_MOVE_DISTANCE = 10.0; @@ -90,10 +101,16 @@ static const uint32_t CRASH_DOTTED_LINE_TIMEOUT = D_SEC(10); static const uint32_t TURN_SIGNAL_LAMP_ADVANCE = D_SEC(3); +static const int CRL_NONE = 0; +static const int CRL_SEP_DOTTED = 1; +static const int CRL_SEP_SOLID = 2; +static const int CRL_EDGE_DOTTED = 3; +static const int CRL_EDGE_SOLID = 4; + static const double MAX_SPEED = 40.0 * 1000.0 / 3600.0; static const int SPEED_GEAR_TABLE[][2] = {{0, 20}, {5, 30}, {15, 40}, {25, 10000}, {35, 10000}}; -static void TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime); +static int TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime); static char isTurn(int currYaw, int prevYaw, int &ang); static char CheckCarTurn(LIST_CAR_MODEL &CarModelList); @@ -106,8 +123,12 @@ static void TurnSignalError13ColdTimer(union sigval sig); static void TurnSignalError14ColdTimer(union sigval sig); static void ReportTurnSignalError(int err, const struct RtkTime *rtkTime); +static bool UpdateLane(struct car_on_lane &out, road_t &road, const car_model *car); +static int CrashRoadLine(road_t &road, const car_model *car); +static bool LaneIsSame(struct car_on_lane lane1, struct car_on_lane lane2); +static bool LaneIsValid(struct car_on_lane lane); -void InitRoadExam(void) +void InitRoadExam(road_exam_map &RoadMap) { DEBUG("Start road_exam"); @@ -132,7 +153,7 @@ prevGearNSlide = false; gearNSlideTime = 0; - currExamMapIndex = -1; + currExamMapIndex = FIND_POSITION; startCar = START_CAR_NOT_DO; @@ -143,6 +164,12 @@ checkStartCarSignal = startCarLeftTurnSignal = false; turnError13Cold = turnError14Cold = true; + + currCarOnRoadIndex = FIND_POSITION; + + CarOnLane.road = CarOnLane.separate = CarOnLane.lane = -1; + + InitThroughSomething(RoadMap); } void TerminateRoadExam(void) @@ -188,7 +215,7 @@ } } -static void TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime) +static int TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime) { double moveDistance; @@ -256,6 +283,558 @@ } else { } + + return startCar; +} + +void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) +{ + // 璧锋妫�娴� + TestRoadStartCar(car, speed, moveDirect, rtkTime); + + // 瓒呴�熸娴� + if (moveDirect != 0 && speed > MAX_SPEED) { + if (!occurOverSpeed) { + occurOverSpeed = true; + // 瓒呴�燂紝涓嶅悎鏍� + AddExamFault(10, rtkTime); + } + } else { + occurOverSpeed = false; + } + + // 鍓埞杞︽娴� + if (ReadCarStatus(SECOND_BREAK) == BREAK_ACTIVE) { + // 鍓埞杞﹁俯涓嬶紝涓嶅悎鏍� + if (!occurSecondBreak) { + DEBUG("鍓埞杞﹀姩浣滀簡"); + occurSecondBreak = true; + AddExamFault(17, rtkTime); + } + } else { + occurSecondBreak = false; + } + + // 鎸′綅鍖归厤妫�娴� + bool currGearError = false; + bool currGearNSlide = false; + + switch (ReadCarStatus(GEAR)) { + case GEAR_N: + if (moveDirect != 0) { + // 绌烘。婊戣 + currGearNSlide = true; + } + break; + case GEAR_1: + if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[0][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[0][1]) { + currGearError = true; + } + break; + case GEAR_2: + if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[1][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[1][1]) { + currGearError = true; + } + break; + case GEAR_3: + if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[2][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[2][1]) { + currGearError = true; + } + break; + case GEAR_4: + if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[3][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[3][1]) { + currGearError = true; + } + break; + case GEAR_5: + if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[4][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[4][1]) { + currGearError = true; + } + break; + default:break; + } + // 绌烘。婊戣瓒呮椂 + if (currGearNSlide && prevGearNSlide) { + gearNSlideTime += TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10, + gearNSlideTimePoint.hour, gearNSlideTimePoint.min, gearNSlideTimePoint.sec, gearNSlideTimePoint.msec*10); + } + if (gearNSlideTime > GEAR_N_SLIDE_TIMEOUT) { + // 绌烘。婊戣瓒�5绉掞紝涓嶅悎鏍� + DEBUG("鎸′綅婊戣锛岃秴杩�5绉�"); + AddExamFault(8, rtkTime); + gearNSlideTime = 0; + } + + prevGearNSlide = currGearNSlide; + if (prevGearNSlide) { + Rtk2DriveTimer(gearNSlideTimePoint, rtkTime); + } else { + gearNSlideTime = 0; + } + // 鎸′綅涓嶅尮閰嶈秴鏃� + if (currGearError && prevGearError) { + gearErrorTime += TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10, + gearErrorTimePoint.hour, gearErrorTimePoint.min, gearErrorTimePoint.sec, gearErrorTimePoint.msec*10); + } + if (gearErrorTime > GEAR_ERROR_TIMEOUT) { + // 绱15绉掞紝鎸′綅-杞﹂�熶笉鍖归厤锛屼笉鍚堟牸 + DEBUG("鎸′綅閿欒瓒呰繃15绉�"); + AddExamFault(6, rtkTime); + gearErrorTime = 0; + } + + prevGearError = currGearError; + if (prevGearError) { + Rtk2DriveTimer(gearErrorTimePoint, rtkTime); + } + + // 璧锋鍚庢粦 + if (moveDirect != prevMoveDirect) { + if (moveDirect == 0) { + stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10); + reportStopCarOnRedArea = false; + + DEBUG("鍋滆溅浜� %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss); + } else if (moveDirect == -1 && prevMoveDirect == 0) { + DEBUG("寮�濮嬪悗婊�"); + stopPoint = car->basePoint; + occurSlide = true; + } + prevMoveDirect = moveDirect; + } else if (moveDirect == 0) { + uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10); + + /*if (tp - stopTimepoint >= STOP_CAR_TIME && !reportStopCarOnRedArea && CrashRedArea(RoadMapList, car)) { + // 鍋滆溅瓒�2绉掞紝鍋滃湪绾㈠尯锛屼笉鍚堟牸 + AddExamFault(16, rtkTime); + DEBUG("绂佸仠鍖哄仠杞�"); + reportStopCarOnRedArea = true; + }*/ + } else if (moveDirect == -1) { + if (occurSlide) { + double slideDistance = DistanceOf(stopPoint, car->basePoint); + + if (slideDistance > SLIDE_DISTANCE_THRESHOLD_YELLOW) { + slideNormalDistance = true; + } + + if (slideDistance > SLIDE_DISTANCE_THRESHOLD_RED && !slideLongDistance) { + // 鍚庢粦瓒呰繃30鍘樼背, 涓嶅悎鏍� + AddExamFault(5, rtkTime); + DEBUG("鍚庢粦瓒呰繃30鍘樼背"); + slideLongDistance = true; + } + } + } else { + if (slideNormalDistance) { + // 鍚庢粦锛屾墸10鍒� + AddExamFault(18, rtkTime); + DEBUG("鍚庢粦瓒呰繃10鍘樼背, 浣嗕笉瓒呰繃30鍘樼背"); + } + + slideNormalDistance = false; + slideLongDistance = false; + occurSlide = false; + } + + // 妫�娴嬮�氳繃璺彛銆佷汉琛岄亾绛夊尯鍩熸椂锛岄噴鏀惧埞杞︽垨鍑忛�� + CheckBreakActive(RoadMap, car, CarModelList); + + // 妫�娴嬬寮�姝よ矾娈碉紝鍏ㄨ溅闇�涓嶅湪鑼冨洿鍐� + if (currExamMapIndex >= 0) { + Polygon area; + int n = 0; + + area.num = 0; + + for (int j = 0; j < RoadMap.roads[currExamMapIndex].leftEdge.size(); ++j) { + if (j > 0) { + area.num += RoadMap.roads[currExamMapIndex].leftEdge[j].points.size() - 1; + } else { + area.num += RoadMap.roads[currExamMapIndex].leftEdge[j].points.size(); + } + } + for (int j = 0; j < RoadMap.roads[currExamMapIndex].rightEdge.size(); ++j) { + if (j > 0) { + area.num += RoadMap.roads[currExamMapIndex].rightEdge[j].points.size() - 1; + } else { + area.num += RoadMap.roads[currExamMapIndex].rightEdge[j].points.size(); + } + } + + area.point = (PointF *) malloc(area.num * sizeof(PointF)); + + for (int j = 0; j < RoadMap.roads[currExamMapIndex].leftEdge.size(); ++j) { + for (int k = (j>0?1:0); k < RoadMap.roads[currExamMapIndex].leftEdge[j].points.size(); ++k) { + area.point[n++] = RoadMap.roads[currExamMapIndex].leftEdge[j].points[k]; + } + } + + for (int j = RoadMap.roads[currExamMapIndex].rightEdge.size() - 1; j >= 0; --j) { + if (j == RoadMap.roads[currExamMapIndex].rightEdge.size() - 1) { + for (int k = RoadMap.roads[currExamMapIndex].rightEdge[j].points.size() - 1; k >= 0; --k) { + area.point[n++] = RoadMap.roads[currExamMapIndex].rightEdge[j].points[k]; + } + } else { + for (int k = RoadMap.roads[currExamMapIndex].rightEdge[j].points.size() - 2; k >= 0; --k) { + area.point[n++] = RoadMap.roads[currExamMapIndex].rightEdge[j].points[k]; + } + } + } + + // 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓� + 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, &area) == GM_None) { + DEBUG("绂诲紑璺 id = %d", RoadMap.roads[currExamMapIndex].id); + currExamMapIndex = FIND_POSITION; + } + + free(carBody.point); + free(area.point); + } + if (currExamMapIndex == FIND_POSITION) { + for (int i = 0; i < RoadMap.roads.size(); ++i) { + Polygon area; + int n = 0; + + area.num = 0; + + for (int j = 0; j < RoadMap.roads[i].leftEdge.size(); ++j) { + if (j > 0) { + area.num += RoadMap.roads[i].leftEdge[j].points.size() - 1; + } else { + area.num += RoadMap.roads[i].leftEdge[j].points.size(); + } + } + for (int j = 0; j < RoadMap.roads[i].rightEdge.size(); ++j) { + if (j > 0) { + area.num += RoadMap.roads[i].rightEdge[j].points.size() - 1; + } else { + area.num += RoadMap.roads[i].rightEdge[j].points.size(); + } + } + + area.point = (PointF *) malloc(area.num * sizeof(PointF)); + + for (int j = 0; j < RoadMap.roads[i].leftEdge.size(); ++j) { + for (int k = (j>0?1:0); k < RoadMap.roads[i].leftEdge[j].points.size(); ++k) { + area.point[n++] = RoadMap.roads[i].leftEdge[j].points[k]; + } + } + + for (int j = RoadMap.roads[i].rightEdge.size() - 1; j >= 0; --j) { + if (j == RoadMap.roads[i].rightEdge.size() - 1) { + for (int k = RoadMap.roads[i].rightEdge[j].points.size() - 1; k >= 0; --k) { + area.point[n++] = RoadMap.roads[i].rightEdge[j].points[k]; + } + } else { + for (int k = RoadMap.roads[i].rightEdge[j].points.size() - 2; k >= 0; --k) { + area.point[n++] = RoadMap.roads[i].rightEdge[j].points[k]; + } + } + } + + if (IntersectionOf(car->carXY[car->axial[AXIAL_FRONT]], &area) == GM_Containment) { + currExamMapIndex = i; + DEBUG("杩涘叆閬撹矾 id = %d", RoadMap.roads[i].id); + break; + } + + free(area.point); + } + if (currExamMapIndex < 0) { + currExamMapIndex = INVALID_POSITION; + DEBUG("鎼滃鏈灉"); + } + } else if (currExamMapIndex == INVALID_POSITION) { + for (int i = 0; i < RoadMap.roads.size(); ++i) { + if (CrashTheLine(RoadMap.roads[i].startLine, car, CarModelList)) { + currExamMapIndex = i; + DEBUG("杩涘叆閬撹矾 id = %d", RoadMap.roads[i].id); + break; + } + } + } + + if (currExamMapIndex >= 0) { + int crl = CrashRoadLine(RoadMap.roads[currExamMapIndex], car); + + if (crl == CRL_NONE) { + DEBUG("浠�涔堥兘娌″帇"); + } else if (crl == CRL_SEP_DOTTED) { + DEBUG("鍘嬪垎閬撹櫄绾�"); + } else if (crl == CRL_SEP_SOLID) { + DEBUG("鍘嬪垎閬撳疄绾�"); + } else if (crl == CRL_EDGE_DOTTED) { + DEBUG("鍘嬭竟娌胯櫄绾�"); + } else if (crl == CRL_EDGE_SOLID) { + DEBUG("鍘嬭竟娌垮疄绾�"); + } + + if (crl != CRL_SEP_DOTTED || crl != CRL_SEP_SOLID) { + struct car_on_lane lane; + UpdateLane(lane, RoadMap.roads[currExamMapIndex], car); + if (!LaneIsSame(lane, CarOnLane)) { + if (LaneIsValid(CarOnLane)) { + // 杞﹂亾鍙樻崲 + DEBUG("鍙樻洿杞﹂亾"); + } + CarOnLane = lane; + } + } + } +} + +/************************************************************* + * 妫�娴嬪綋鍓嶈溅閬擄紝浠ヨ溅澶翠腑鐐逛负鍩哄噯 + * @param road + * @param car + */ +static bool UpdateLane(struct car_on_lane &out, road_t &road, const car_model *car) +{ + bool leftExt = false, rightExt = false; + Line leftExtLine, rightExtLine; + struct car_on_lane lane; + + for (int i = 0; i < road.leftEdge.size(); ++i) { + PointF p1, p2; + Line edge; + + p1 = road.leftEdge[i].points[0]; + for (int j = 1; j < road.leftEdge[i].points.size(); ++j) { + p2 = road.leftEdge[i].points[j]; + MakeLine(&edge, &p1, &p2); + + PointF vp; + if (VerticalPointOnLine(car->carXY[car->axial[AXIAL_FRONT]], edge, vp)) { + leftExt = true; + MakeLine(&leftExtLine, &car->carXY[car->axial[AXIAL_FRONT]], &vp); + goto LEFT_EXT_CMP; + } + p1 = p2; + } + } +LEFT_EXT_CMP: + for (int i = 0; i < road.rightEdge.size(); ++i) { + PointF p1, p2; + Line edge; + + p1 = road.rightEdge[i].points[0]; + for (int j = 1; j < road.rightEdge[i].points.size(); ++j) { + p2 = road.rightEdge[i].points[j]; + MakeLine(&edge, &p1, &p2); + + PointF vp; + if (VerticalPointOnLine(car->carXY[car->axial[AXIAL_FRONT]], edge, vp)) { + rightExt = true; + MakeLine(&rightExtLine, &car->carXY[car->axial[AXIAL_FRONT]], &vp); + goto RIGHT_EXT_CMP; + } + p1 = p2; + } + } +RIGHT_EXT_CMP: + + if (!leftExt || !rightExt) { + return false; + } + + bool orthogonalInSegment = false; + + for (int i = 0; i < road.separate.size(); ++i) { // 娈� + PointF p1, p2; + Line sep; + + map<int, int> orthogonal; + + // 涓�娈靛垎閬撶粍鍐咃紝鏈変竴鏉℃浜わ紝灏卞繀椤讳繚璇佸悓缁勭殑鍏ㄩ兘姝d氦锛屽惁鍒欑洿鎺ラ��鍑� + for (int j = 0; j < road.separate[i].lines.size(); ++j) { // 绾跨粍 + bool intersection = false; + + for (int k = 0; !intersection && k < road.separate[i].lines[j].size(); ++k) { // 鑺� + + p1 = road.separate[i].lines[j][k].points[0]; + for (int m = 1; m < road.separate[i].lines[j][k].points.size(); ++m) { + p2 = road.separate[i].lines[j][k].points[m]; + MakeLine(&sep, &p1, &p2); + + if (IntersectionOf(leftExtLine, sep) == GM_Intersection) { + orthogonal.insert(pair<int, int>(j, 1)); + orthogonalInSegment = true; + intersection = true; + DEBUG("鍒嗛亾绾� %d 宸︽浜�", j); + break; + } else if (IntersectionOf(rightExtLine, sep) == GM_Intersection) { + orthogonal.insert(pair<int, int>(j, 2)); + orthogonalInSegment = true; + intersection = true; + DEBUG("鍒嗛亾绾� %d 鍙虫浜�", j); + break; + } + p1 = p2; + } + } + } + DEBUG("鐩爣 %d 褰撳墠 %d", road.separate[i].lines.size(), orthogonal.size()); + + if (orthogonal.size() > 0) { + if (orthogonal.size() == road.separate[i].lines.size()) { + // 寰楀埌褰撳墠鍦ㄧ鍑犱釜杞﹂亾 + int x = 0; + for (x = 0; x < orthogonal.size(); ++x) { + auto itx = orthogonal.find(x); + if (itx != orthogonal.end()) { + if (itx->second != 1) { + lane.road = road.id; + lane.separate = i; + lane.lane = itx->first; + + DEBUG("璺� %d 娈� %d 杞﹂亾 %d", lane.road, lane.separate, lane.lane); + break; + } + } + } + if (x >= orthogonal.size()) { + lane.road = road.id; + lane.separate = i; + lane.lane = orthogonal.size(); + + DEBUG("璺� %d 娈� %d 杞﹂亾 %d", lane.road, lane.separate, lane.lane); + } + out = lane; + return true; + } else { + // 涓嶅畬鍏ㄦ浜わ紝鐩存帴閫�鍑� + } + return false; + } + } + return false; +} + +/************************************************************************* + * 纰拌Е杞﹂亾绾垮瀷 + * @param road + * @param car + */ +static int CrashRoadLine(road_t &road, const car_model *car) +{ + Line frontAxle, rearAxle; + + MakeLine(&frontAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]); + MakeLine(&rearAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]); + + // 妫�鏌ラ亾璺竟缂� + for (int i = 0; i < road.leftEdge.size(); ++i) { + PointF p1, p2; + Line edge; + + p1 = road.leftEdge[i].points[0]; + for (int j = 1; j < road.leftEdge[i].points.size(); ++j) { + p2 = road.leftEdge[i].points[j]; + MakeLine(&edge, &p1, &p2); + + if (IntersectionOf(edge, frontAxle) == GM_Intersection || + IntersectionOf(edge, rearAxle) == GM_Intersection) { + // 鍘嬮亾璺乏杈圭嚎 + if (road.leftEdge[i].character != LINE_SOLID) { + return CRL_EDGE_DOTTED; + } + return CRL_EDGE_SOLID; + } + p1 = p2; + } + } + + for (int i = 0; i < road.rightEdge.size(); ++i) { + PointF p1, p2; + Line edge; + + p1 = road.rightEdge[i].points[0]; + for (int j = 1; j < road.rightEdge[i].points.size(); ++j) { + p2 = road.rightEdge[i].points[j]; + MakeLine(&edge, &p1, &p2); + + if (IntersectionOf(edge, frontAxle) == GM_Intersection || + IntersectionOf(edge, rearAxle) == GM_Intersection) { + // 鍘嬮亾璺彸杈圭嚎 + if (road.rightEdge[i].character != LINE_SOLID) { + return CRL_EDGE_DOTTED; + } + return CRL_EDGE_SOLID; + } + p1 = p2; + } + } + + // 妫�鏌ュ垎閬撶嚎 + for (int i = 0; i < road.separate.size(); ++i) { + // 娈� + PointF p1, p2; + Line sep; + + for (int j = 0; j < road.separate[i].lines.size(); ++j) { + // 绾� + for (int k = 0; k < road.separate[i].lines[j].size(); ++k) { + // 鑺� + p1 = road.separate[i].lines[j][k].points[0]; + + for (int m = 1; m < road.separate[i].lines[j][k].points.size(); ++m) { + p2 = road.separate[i].lines[j][k].points[m]; + MakeLine(&sep, &p1, &p2); + + if (IntersectionOf(sep, frontAxle) == GM_Intersection || + IntersectionOf(sep, rearAxle) == GM_Intersection) { + // 妫�鏌ラ亾璺垎闅旂嚎绫诲瀷 + if (road.separate[i].lines[j][k].character == LINE_DOTTED) { + // 鍘嬭櫄绾� + return CRL_SEP_DOTTED; + } else if (road.separate[i].lines[j][k].character == LINE_SOLID) { + // 鍘嬪疄绾� + return CRL_SEP_SOLID; + } else if (road.separate[i].lines[j][k].character == LINE_HALF_SOLID_LEFT) { + if (LaneIsValid(CarOnLane) && CarOnLane.lane <= j) { + return CRL_SEP_SOLID; + } + return CRL_SEP_DOTTED; + } else if (road.separate[i].lines[j][k].character == LINE_HALF_SOLID_RIGHT) { + if (LaneIsValid(CarOnLane) && CarOnLane.lane > j) { + return CRL_SEP_SOLID; + } + return CRL_SEP_DOTTED; + } + } + + p1 = p2; + } + } + } + } + + return CRL_NONE; +} + +static bool LaneIsSame(struct car_on_lane lane1, struct car_on_lane lane2) +{ + if (lane1.road == lane2.road && lane1.separate == lane2.separate && lane1.lane == lane2.lane) { + return true; + } + return false; +} + +static bool LaneIsValid(struct car_on_lane lane) +{ + if (lane.road >= 0 && lane.separate >= 0 && lane.lane >= 0) { + return true; + } + return false; } void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) @@ -974,6 +1553,28 @@ return map_idx; } +bool CrashTheLine(Line line, const car_model *car, LIST_CAR_MODEL &CarModelList) +{ + if (CarModelList.size() < 2) + return false; + + list<car_model *>::iterator iter = CarModelList.begin(); + + Line trace; + PointF p1, p2; + + p1 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->axial[AXIAL_FRONT]]; + ++iter; + p2 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->axial[AXIAL_FRONT]]; + MakeLine(&trace, &p1, &p2); + + if (IntersectionOf(trace, line) == GM_Intersection && + IntersectionOfLine(p1, line) == -1) { + return true; + } + return false; +} + static int FindMapIndexById(int id, LIST_ROAD_MAP &RoadMapList) { for (int i = 0; i < RoadMapList.size(); ++i) { @@ -984,12 +1585,63 @@ return -1; } -#if 1 +/********************************************************************* + * 璁$畻鏌愮偣鍒伴亾璺乏杈圭嚎鐨勬渶杩戝瀭鐐� + * @param edge + * @param road + * @param point + * @return + */ +PointF GetSELine(vector<edge_t> &edge, PointF point) +{ + PointF p1, p2; + PointF px; + + vector<PointF> vps; + + Line line; + + for (int i = 0; i < edge.size(); ++i) { + p1 = edge[i].points[0]; + for (int j = 1; j < edge[i].points.size(); ++j) { + p2 = edge[i].points[j]; + MakeLine(&line, &p1, &p2); + + PointF vp; + if (VerticalPointOnLine(point, line, vp)) { + vps.push_back(vp); + } + + p1 = p2; + } + } + + if (vps.size() == 0) { + if (DistanceOf(point, edge[0].points[0]) < DistanceOf(point, edge[edge.size() - 1].points[edge[edge.size() - 1].points.size() - 1])) { + px = GetVerticalPoint(edge[0].points[0], edge[0].points[1], point); + } else { + px = GetVerticalPoint(edge[edge.size() - 1].points[edge[edge.size() - 1].points.size() - 2], edge[edge.size() - 1].points[edge[edge.size() - 1].points.size() - 1], point); + } + } else if (vps.size() == 1) { + px = vps[0]; + } else { + px = vps[0]; + for (int i = 1; i < vps.size(); ++i) { + if (DistanceOf(point, vps[i]) < DistanceOf(point, px)) { + px = vps[i]; + } + } + } + + return px; +} + +#if 0 typedef struct { int road; int segment; - int track; + int lane; } CarOnTrackInfo_t; static CarOnTrackInfo_t CarOnTrackInfo; @@ -1001,7 +1653,7 @@ if (newCarOnTrackInfo.road == CarOnTrackInfo.road && newCarOnTrackInfo.segment == CarOnTrackInfo.segment && - newCarOnTrackInfo.track != CarOnTrackInfo.track) { + newCarOnTrackInfo.lane != CarOnTrackInfo.lane) { } @@ -1145,7 +1797,7 @@ static void DetectSeparate(int currIndex, struct road_exam2_map &map, const car_model *car) { int segment; - int track = -1; + int lane = -1; CarOnTrackInfo_t newInfo; @@ -1190,7 +1842,7 @@ vrecord.push_back(v); - track = separate_line_num;// + lane = separate_line_num;// } @@ -1215,14 +1867,14 @@ if (rel != -1) { newInfo.road = currIndex; newInfo.segment = i; - newInfo.track = x; + newInfo.lane = x; break; } } newInfo.road = currIndex; newInfo.segment = i; - newInfo.track = vrecord.size(); + newInfo.lane = vrecord.size(); break; } diff --git a/lib/src/main/cpp/test_items2/road_exam.h b/lib/src/main/cpp/test_items2/road_exam.h index 015a29e..57a4f5c 100644 --- a/lib/src/main/cpp/test_items2/road_exam.h +++ b/lib/src/main/cpp/test_items2/road_exam.h @@ -27,10 +27,14 @@ }; void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime); -void InitRoadExam(void); +void InitRoadExam(road_exam_map &RoadMap); void TerminateRoadExam(void); +void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime); 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); bool CrashSonRedLine(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList); +bool CrashTheLine(Line line, const car_model *car, LIST_CAR_MODEL &CarModelList); +PointF GetSELine(std::vector<edge_t> &edge, PointF point); + #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 index 9461842..d9349e9 100644 --- a/lib/src/main/cpp/test_items2/through_something.cpp +++ b/lib/src/main/cpp/test_items2/through_something.cpp @@ -2,6 +2,9 @@ // Created by YY on 2020/3/20. // +#include <map> +#include <algorithm> + #include "through_something.h" #include "../driver_test.h" #include "../test_common/Geometry.h" @@ -12,12 +15,17 @@ #define DEBUG(fmt, args...) LOGD("<through_something> <%s>: " fmt, __func__, ##args) +using namespace std; + static const double LASTEST_BREAK_POINT = 30.0; static const double DISTANCE_STOP_CAR_TO_STOP_LINE = 5.0; +static const double PASS_SCHOOL_MAX_SPEED = 30.0; // kmh static int breakActive; static int stopActive; static bool crashRedLine; + +static map<int, bool> breakRecord; // 璁板綍杞﹁締椹惰繃鍚勪釜鍋滄绾垮墠鍒硅溅鍔ㄤ綔 void StartThroughExam(int index, LIST_ROAD_MAP &RoadMapList) { @@ -33,6 +41,20 @@ breakActive = 0; stopActive = 0; crashRedLine = false; +} + +void InitThroughSomething(road_exam_map &RoadMap) +{ + breakRecord.clear(); + // 涓烘瘡涓�涓埞杞︽娴嬬偣 + for (int i = 0; i < RoadMap.specialAreas.size(); ++i) { + if (RoadMap.specialAreas[i].type == ZEBRA_CROSSING || RoadMap.specialAreas[i].type == BUS_STATION_AREA) { + breakRecord.insert(pair<int, bool>(RoadMap.specialAreas[i].id, false)); + } + } + for (int i = 0; i < RoadMap.roads.size(); ++i) { + breakRecord.insert(pair<int, bool>(RoadMap.roads[i].id, false)); + } } int ExecuteThroughExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car, @@ -133,36 +155,57 @@ return index; } -static void ThroughZebraCrossing(struct road_exam2_map &map, const car_model *car) +void CheckBreakActive(road_exam_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList) { - for (int i = 0; i < map.specialAreas.size(); i++) { - if (map.specialAreas[i].character == 'zebra') { - double distance = 0.0; + int BreakDone = ReadCarStatus(BREAK); + for (int i = 0; i < map.specialAreas.size(); i++) { + if (map.specialAreas[i].type == ZEBRA_CROSSING || map.specialAreas[i].type == BUS_STATION_AREA) { double yaw = YawOf(car->carXY[ car->axial[AXIAL_FRONT] ], car->carXY[ car->axial[AXIAL_REAR] ]); PointF extPoint = PointExtend(car->carXY[ car->axial[AXIAL_FRONT] ], LASTEST_BREAK_POINT, yaw); Line extLine; MakeLine(&extLine, &car->carXY[ car->axial[AXIAL_FRONT] ], &extPoint); - if (IntersectionOf(extLine, map.specialAreas[i].startLine) == GM_Intersection && - IntersectionOfLine(car->carXY[ car->axial[AXIAL_FRONT] ], map.specialAreas[i].startLine) == 1 ) { - jj = true; - if (ReadCarStatus(BREAK) == BREAK_ACTIVE) { - breakActive = 1; - } - } else { - jj = false; + // 璁$畻鐐瑰埌宸︿晶璺竟绾跨殑鍨傜偣 + int road = 0; + for (road = 0; road < map.roads.size(); ++road) { + if (map.roads[road].id == map.specialAreas[i].road) + break; } + PointF vPoint = GetSELine(map.roads[road].leftEdge, map.specialAreas[i].area[0]); + Line startLine; + DEBUG("璁$畻鍨傜偣 (%f, %f)", vPoint.X, vPoint.Y); + MakeLine(&startLine, &map.specialAreas[i].area[0], &vPoint); - - if (distance < LASTEST_BREAK_POINT) { - if (ReadCarStatus(BREAK) == BREAK_ACTIVE) { - breakActive = 1; + // 杞﹀ご鍜屾枒椹嚎璺濈涓嶈冻30绫� + if (IntersectionOf(extLine, startLine) == GM_Intersection && + IntersectionOfLine(car->carXY[ car->axial[AXIAL_FRONT] ], startLine) == 1 ) { + DEBUG("杩涘叆鍑忛�熷尯"); + if (BreakDone == BREAK_ACTIVE) { + auto itx = breakRecord.find(map.specialAreas[i].id); + if (itx != breakRecord.end()) { + itx->second = true; + } } } + + // 璺ㄧ嚎鍚庯紝妫�鏌ュ埞杞﹀姩浣� + if (CrashTheLine(startLine, car, CarModelList)) { + auto itx = breakRecord.find(map.specialAreas[i].id); + if (itx != breakRecord.end()) { + if (itx->second == false) { + // 涓嶆寜瑙勫畾鍑忛�燂紝涓嶅悎鏍� + DEBUG("涓嶆寜瑙勫畾鍑忛��"); + } + itx->second = false; + } + } + } else if (map.specialAreas[i].type == SCHOOL_AREA) { + } } } + diff --git a/lib/src/main/cpp/test_items2/through_something.h b/lib/src/main/cpp/test_items2/through_something.h index a6967e9..dc3ee53 100644 --- a/lib/src/main/cpp/test_items2/through_something.h +++ b/lib/src/main/cpp/test_items2/through_something.h @@ -8,6 +8,8 @@ #include "../driver_test.h" void StartThroughExam(int index, LIST_ROAD_MAP &RoadMapList); +void InitThroughSomething(road_exam_map &RoadMap); +void CheckBreakActive(road_exam_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList); int ExecuteThroughExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime); diff --git a/lib/src/main/java/com/anyun/exam/lib/RemoteService.java b/lib/src/main/java/com/anyun/exam/lib/RemoteService.java index a349d85..e89893c 100644 --- a/lib/src/main/java/com/anyun/exam/lib/RemoteService.java +++ b/lib/src/main/java/com/anyun/exam/lib/RemoteService.java @@ -93,7 +93,7 @@ new Thread(new TestThread()).start(); - PlayRing(this); +// PlayRing(this); } public void PlayRing(Context context) { -- Gitblit v1.8.0