//
|
// Created by YY on 2020/4/3.
|
//
|
|
#include <cstdlib>
|
#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 "park_edge.h"
|
#include "driving_curve.h"
|
#include "turn_a90.h"
|
#include "../utils/xconvert.h"
|
#include "../common/apptimer.h"
|
#include "../test_common/odo_graph.h"
|
#include "../test_common/Geometry.h"
|
#include "../common/observer.h"
|
|
#define DEBUG(fmt, args...) LOGD("<area_exam> <%s>: " fmt, __func__, ##args)
|
|
ilovers::Observer<std::function<void(move_status_t)>> CarMoveEvent;
|
|
static bool ProximityArea(Line &base_line, Line &line);
|
static bool CrossingStartLine(Line &trace, Line &start_line);
|
static void ProximityReminders(prime_t &prime);
|
static void DetectCarMove(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;
|
}
|
DetectCarMove(prime);
|
}
|
|
static void DetectCarMove(prime_t &prime)
|
{
|
static move_status_t prevMove = STOP;
|
if (prime.pMotion->move != prevMove) {
|
// Notify
|
CarMoveEvent.Notify(prime.pMotion->move);
|
prevMove = prime.pMotion->move;
|
}
|
}
|
|
void RegisterCarMoveObserver(void (*ptr)(move_status_t))
|
{
|
CarMoveEvent.Connect(ptr);
|
}
|
|
void UnregisterCarMoveObserver(int handle)
|
{
|
CarMoveEvent.Disconnect(handle);
|
}
|
|
void EnterMap(prime_t &prime)
|
{
|
if (prime.curr_exam_map.type != 0) {
|
return;
|
}
|
|
if (prime.prev_modeling_index == -1 || prime.curr_modeling_index == -1) {
|
return;
|
}
|
|
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 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]);
|
if (CrossingStartLine(left_trace, start_line)) {
|
prime.curr_exam_map.type = MAP_TYPE_PARK_BUTTOM;
|
prime.curr_exam_map.map_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]);
|
if (CrossingStartLine(left_trace, start_line)) {
|
prime.curr_exam_map.type = MAP_TYPE_PARK_BUTTOM;
|
prime.curr_exam_map.map_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]);
|
if (CrossingStartLine(left_trace, start_line)) {
|
prime.curr_exam_map.type = MAP_TYPE_PARK_EDGE;
|
prime.curr_exam_map.map_idx = i;
|
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;
|
}
|
}
|
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;
|
}
|
}
|
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;
|
}
|
}
|
}
|
|
// 车轮驶过线，且车头位于右侧
|
static bool CrossingStartLine(Line &trace, Line &start_line)
|
{
|
PointF head = {.X = trace.X1, .Y = trace.Y1};
|
|
if (IntersectionOf(trace, start_line) == GM_Intersection
|
&& IntersectionOfLine(head, start_line) == RELATION_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) {
|
return;
|
}
|
|
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);
|
Line car_head_line;
|
MAKE_LINE(car_head_line, car_head, car_head_trend);
|
|
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 < 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 < 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 < 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 < 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]);
|
}
|
}
|
|
// 判断是否接近场地的起始线
|
// 车头趋势线是否和入口线相交
|
// 和入口线的夹角
|
static bool ProximityArea(Line &base_line, Line &line)
|
{
|
PointF head = {.X = line.X1, .Y = line.Y1};
|
|
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;
|
}
|
}
|
|
return false;
|
}
|
|
void TerminateAreaExam(void)
|
{
|
CurrExamMapIndex = -1;
|
}
|
|
void InitAreaExam(void)
|
{
|
CurrExamMapIndex = -1;
|
ResetOdo();
|
}
|
