//
|
// Created by YY on 2023/4/20.
|
//
|
|
#include "teach.h"
|
#include "../jni_log.h"
|
#include "../driver_test.h"
|
#include "../native-lib.h"
|
#include <tuple>
|
#include <vector>
|
#include <algorithm>
|
|
#define DEBUG(fmt, args...) LOGD("<teach> <%s>: " fmt, __func__, ##args)
|
|
typedef struct {
|
int id;
|
std::string tts;
|
int stage;
|
int refer; // 车身某点，某某事件
|
PointF point; // 采集点经过坐标变换后得到
|
} point_tip_t;
|
|
typedef struct {
|
int id;
|
std::string tts;
|
int stage;
|
int refer; // 车身某点，某某事件
|
double distance;
|
Line line; // 采集点经过坐标变换后得到
|
} line_tip_t;
|
|
typedef struct {
|
int item;
|
std::vector<point_tip_t> tips;
|
} teach_tips_t;
|
|
typedef struct {
|
|
};
|
|
teach_tips_t all;
|
|
void testAll(void)
|
{
|
all.item = MAP_TYPE_PARK_BUTTOM;
|
|
point_tip_t p1 = {
|
.id = 1,
|
.tts = "提示一",
|
.stage = 2,
|
.refer = 1,
|
.point = {.X = 4.396647, .Y = 4.967977}
|
};
|
|
point_tip_t p2 = {
|
.id = 2,
|
.tts = "提示二",
|
.stage = 2,
|
.refer = 1,
|
.point = {.X = 1.352043, .Y = 3.434146}
|
};
|
|
point_tip_t p3 = {
|
.id = 3,
|
.tts = "提示三",
|
.stage = 2,
|
.refer = 1,
|
.point = {.X = 1.042866, .Y = 0.124397}
|
};
|
|
point_tip_t p4 = {
|
.id = 4,
|
.tts = "提示四",
|
.stage = 2,
|
.refer = 1,
|
.point = {.X = 1.037644, .Y = -4.675115}
|
};
|
|
all.tips.push_back(p1);
|
all.tips.push_back(p2);
|
all.tips.push_back(p3);
|
all.tips.push_back(p4);
|
}
|
|
class Foobar {
|
public:
|
int id;
|
double distance;
|
|
Foobar(int id, double distance): id(id), distance(distance) {}
|
~Foobar() = default;
|
|
bool operator<(const Foobar &bar)
|
{
|
if (this->distance < bar.distance) {
|
return true;
|
}
|
return false;
|
}
|
|
bool static decrease(const Foobar &foo1, const Foobar &foo2)
|
{
|
if (foo1.distance > foo2.distance)
|
{
|
return false;
|
}
|
return true;
|
}
|
|
};
|
|
std::vector<Foobar> status;
|
|
// 当前项目阶段发生变化时，产生的点位提示
|
void LoadStageTips(prime_t &prime)
|
{
|
DEBUG("LoadStageTips %d", prime.examing_area.stage);
|
|
std::vector<Foobar>().swap(status);
|
|
// 以2号点(左侧车库入口角)为原点
|
double dx = std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[2].X;
|
double dy = std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[2].Y;
|
|
// 以2---->3线为-Y轴，旋转
|
double angle = YawOf(std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[2], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[3]);
|
|
if (angle > 180) {
|
angle = angle - 180;
|
} else {
|
angle = angle + 180;
|
}
|
|
// 记录车辆位置经过坐标变换后的值
|
PointF car = {.X = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->axial[AXIAL_FRONT]].X - dx,
|
.Y = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->axial[AXIAL_FRONT]].Y - dy};
|
|
car = rotatePoint(car, {.X = 0,.Y = 0}, angle);
|
|
// 需按照距离远近递增排序
|
for (int i = 0; i < all.tips.size(); ++i) {
|
if (all.tips[i].stage == prime.examing_area.stage) {
|
DEBUG("Add %d: %f", i, DistanceOf(car, all.tips[i].point));
|
status.push_back(Foobar(i, DistanceOf(car, all.tips[i].point)));
|
}
|
}
|
if (status.size() > 0) {
|
std::sort(status.begin(), status.end(), Foobar::decrease);
|
|
for (auto st: status) {
|
DEBUG("%d: %f", st.id, st.distance);
|
}
|
}
|
}
|
|
void teach(prime_t &prime)
|
{
|
if(status.size() == 0)
|
return;
|
|
PointF car;
|
PointF &teach_point = all.tips[status.front().id].point;
|
|
switch (prime.examing_area.type) {
|
case MAP_TYPE_PARK_BUTTOM: {
|
// 以2号点(左侧车库入口角)为原点
|
double dx = std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[2].X;
|
double dy = std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[2].Y;
|
|
// 以2---->3线为-Y轴，旋转
|
double angle = YawOf(std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[2], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[3]);
|
|
if (angle > 180) {
|
angle = angle - 180;
|
} else {
|
angle = angle + 180;
|
}
|
|
// 记录车辆位置经过坐标变换后的值
|
car = {.X = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->axial[AXIAL_FRONT]].X - dx,
|
.Y = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->axial[AXIAL_FRONT]].Y - dy};
|
|
car = rotatePoint(car, {.X = 0,.Y = 0}, angle);
|
|
break;
|
}
|
case MAP_TYPE_PARK_EDGE:
|
break;
|
case MAP_TYPE_RIGHT_CORNER:
|
break;
|
case MAP_TYPE_UPHILL:
|
break;
|
case MAP_TYPE_CURVE:
|
break;
|
default:break;
|
}
|
|
double dis = DistanceOf(car, teach_point);
|
|
DEBUG("XY(%f,%f, dis = %f)", car.X, car.Y, dis);
|
|
if (dis < status.front().distance) {
|
status.front().distance = dis;
|
} else if (dis > status.front().distance && dis - status.front().distance > 0.05) {
|
// 提示
|
PlayTTS(all.tips[status.front().id].tts, nullptr);
|
status.erase(status.begin());
|
}
|
}
|
