462 lines
14 KiB
C++
462 lines
14 KiB
C++
#include "pl.hpp"
|
||
|
||
#include <math.h>
|
||
#include <string.h>
|
||
#include <sys/stat.h>
|
||
#include <unistd.h>
|
||
|
||
#include <iostream>
|
||
|
||
#include "logger/logger.h"
|
||
|
||
using namespace std;
|
||
|
||
#define WRC_MAX_POINTS_IN_PATH 2000
|
||
|
||
#define Lat_Origin 32.045062 // 地面平面坐标系原点的纬度
|
||
#define Lon_Origin 118.845200366 // 地面平面坐标系原点的经度
|
||
#define Re 6378137
|
||
#define Rn 6356755
|
||
#define deg_rad (0.01745329252) // Transfer from angle degree to rad
|
||
#define R_LATI (6378137)
|
||
#define R_LONT (5407872) // 这个需要根据所在区域的纬度进行换算：R_LONT = R_LATI*cos(所在地的纬度转化为弧度)
|
||
|
||
float nl_radius;
|
||
int nl_within_radius;
|
||
double x = 0.0;
|
||
double y = 1.0;
|
||
int pl_speed = 0;
|
||
|
||
int GpsPointNum = 0;
|
||
WRC_GPS_Point GPSPointQueue[WRC_MAX_POINTS_IN_PATH];
|
||
|
||
WRC_MC_NAV_INFO g_NavInfo;
|
||
struct_rtk g_rtk;
|
||
|
||
static int g_last_road_pos = -1; // 跟踪上一次的最近点索引，用于单调性约束
|
||
|
||
int mode = 0;
|
||
int last_mode = 0;
|
||
|
||
Direction drive_mode = Direction::STRAIGHT_D;
|
||
TaskStatus task_status = TaskStatus::PENDING;
|
||
int thread_exit_flag = 0; // 线程退出标志，0=继续运行，1=需要退出
|
||
|
||
// 返回单位：m
|
||
double ntzx_GPS_length(double lonti1, double lati1, double lonti2, double lati2)
|
||
{
|
||
double x, y, length;
|
||
x = (R_LONT) * (lonti2 - lonti1) * deg_rad; // 弧长公式
|
||
y = (R_LATI) * (lati2 - lati1) * deg_rad;
|
||
length = sqrt(x * x + y * y);
|
||
return length;
|
||
}
|
||
|
||
// Cur_navAngle指定y轴正方向.Cur_lonti，Cur_lati为原点；x_diff,y_diff是Dest_lonti,Dest_lati在该坐标系的直角坐标:m
|
||
int ntzx_GPS_posit(double Cur_navAngle, double Cur_lonti, double Cur_lati, double Dest_lonti, double Dest_lati,
|
||
double* x_diff, double* y_diff)
|
||
{
|
||
double Navi_rad, x, y;
|
||
float k1, k2, k3, k4; // 旋转矩阵的四个参数，对应cos(theta),-sin(theta),sin(theta),cos(theta);
|
||
Navi_rad = Cur_navAngle * deg_rad; // 角度转化为弧度
|
||
// 以当前位置为原点，以正北为y正轴，以正东为x正轴；
|
||
x = (Dest_lonti - Cur_lonti) * deg_rad * R_LONT;
|
||
y = (Dest_lati - Cur_lati) * deg_rad * R_LATI;
|
||
k1 = cos(Navi_rad);
|
||
k2 = (-1) * sin(Navi_rad);
|
||
k3 = (-1) * k2;
|
||
k4 = k1;
|
||
// 以当前航向角作为旋转角
|
||
*x_diff = x * k1 + y * k2;
|
||
*y_diff = x * k3 + y * k4;
|
||
|
||
return 0;
|
||
}
|
||
|
||
// Find_Nearest_Point
|
||
// 线性前向搜索 + 严格单调性约束：拒绝向后跳动，防止交叉点索引混乱
|
||
int Road_Planning_Find_Nearest_Point(int start_index, WRC_MC_NAV_INFO Cur_GPS_info, WRC_GPS_Point* Road_info)
|
||
{
|
||
if (start_index < 0)
|
||
{
|
||
return -1;
|
||
}
|
||
|
||
// 钳位 start_index 到有效范围
|
||
if (start_index >= GpsPointNum)
|
||
{
|
||
start_index = GpsPointNum - 1;
|
||
}
|
||
|
||
int Nearest_point_index = start_index; // 初始化为起始索引
|
||
double min_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree,
|
||
Road_info[start_index].LongitudeInfo, Road_info[start_index].LatitudeInfo);
|
||
|
||
// 线性前向搜索（不环绕），到达路径末尾停止
|
||
for (int i = 1; i < GpsPointNum; i++)
|
||
{
|
||
int current_index = start_index + i;
|
||
|
||
// 到达路径末尾，停止搜索
|
||
if (current_index >= GpsPointNum)
|
||
{
|
||
break;
|
||
}
|
||
|
||
double current_distance =
|
||
ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree,
|
||
Road_info[current_index].LongitudeInfo, Road_info[current_index].LatitudeInfo);
|
||
|
||
if (current_distance < min_distance)
|
||
{
|
||
// === 航向角过滤：排除交叉路径上航向不匹配的段 ===
|
||
double heading_diff = std::fmod(std::fabs(Cur_GPS_info.Yaw_rad - Road_info[current_index].Heading), 360.0);
|
||
if (heading_diff > 180.0) heading_diff = 360.0 - heading_diff;
|
||
|
||
if (heading_diff > 45.0)
|
||
{
|
||
continue; // 航向不匹配，跳过此交叉段候选点
|
||
}
|
||
|
||
min_distance = current_distance;
|
||
Nearest_point_index = current_index; // 更新最近点的索引
|
||
|
||
// 如果距离小于 0.8 m，提前退出
|
||
if (min_distance < 0.8)
|
||
{
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
// === 严格单调性约束 ===
|
||
// 车辆只往前走，不允许回头，因此索引不允许向后跳动。
|
||
// 当路径存在空间交叉点时，几何最近点可能跳回另一段路径，
|
||
// 此处强制拒绝任何向后跳动。
|
||
if (g_last_road_pos >= 0 && Nearest_point_index < g_last_road_pos)
|
||
{
|
||
// 候选点在上次位置之前，拒绝它，保持前进
|
||
if (g_last_road_pos + 1 < GpsPointNum)
|
||
{
|
||
Nearest_point_index = g_last_road_pos + 1;
|
||
}
|
||
else
|
||
{
|
||
Nearest_point_index = g_last_road_pos;
|
||
}
|
||
|
||
LOG_WARN("单调约束: 拒绝向后跳 (候选=%d, 上次=%d), 保持前进", Nearest_point_index, g_last_road_pos);
|
||
}
|
||
|
||
g_last_road_pos = Nearest_point_index;
|
||
return Nearest_point_index; // 返回最近点的索引
|
||
}
|
||
/*
|
||
int Road_Planning_Find_Nearest_Point(int start_index, WRC_MC_NAV_INFO Cur_GPS_info, WRC_GPS_Point *Road_info)
|
||
{
|
||
|
||
if (start_index < 0)
|
||
{
|
||
return -1;
|
||
}
|
||
int Nearest_point_index = 0;
|
||
double aft_distance = 0, pre_distance = 0;
|
||
pre_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree,
|
||
Road_info[start_index].LongitudeInfo, Road_info[start_index].LatitudeInfo);
|
||
|
||
int start = start_index + 1;
|
||
for (int i = start; i < GpsPointNum; i++)
|
||
{
|
||
|
||
aft_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree,
|
||
Road_info[i].LongitudeInfo, Road_info[i].LatitudeInfo);
|
||
// printf("i: %d , aft_distance: %lf \t",i,aft_distance);
|
||
if (aft_distance < pre_distance)
|
||
{
|
||
pre_distance = aft_distance;
|
||
continue;
|
||
}
|
||
else
|
||
{
|
||
Nearest_point_index = i - 1;
|
||
|
||
return Nearest_point_index;
|
||
}
|
||
}
|
||
return GpsPointNum - 1;
|
||
}
|
||
*/
|
||
|
||
// Find_Aim_Point
|
||
int Road_Planning_Find_Aim_Point(int start_index, int dest_num)
|
||
{
|
||
int dest_index = start_index;
|
||
if (start_index + dest_num <= (GpsPointNum - 1))
|
||
{
|
||
dest_index = start_index + dest_num;
|
||
}
|
||
else
|
||
{
|
||
dest_index = (GpsPointNum - 1);
|
||
}
|
||
|
||
return (dest_index);
|
||
}
|
||
|
||
int star_find_nearest_point(double lon, double lat, WRC_GPS_Point* Road_info)
|
||
{
|
||
double dis = 10000;
|
||
int point = 0;
|
||
for (int i = 0; i < GpsPointNum; i++)
|
||
{
|
||
double dis_temp = ntzx_GPS_length(lon, lat, Road_info[i].LongitudeInfo, Road_info[i].LatitudeInfo);
|
||
if (dis_temp < dis)
|
||
{
|
||
dis = dis_temp;
|
||
point = i;
|
||
}
|
||
LOG_DEBUG("i: %d , dis_temp: %lf", i, dis_temp);
|
||
}
|
||
return point;
|
||
}
|
||
|
||
// 判断车辆是否在左转、右转还是直行
|
||
Direction straight_or_turn(double cur_direction, double des_direction, int threshold)
|
||
{
|
||
double diff = std::fmod(std::fabs(cur_direction - des_direction), 360.0);
|
||
|
||
if (diff > 180) diff = 360 - diff;
|
||
|
||
// std::cout << "diff : " << diff << std::endl;
|
||
|
||
if (diff < threshold)
|
||
{
|
||
return Direction::STRAIGHT_D;
|
||
}
|
||
else
|
||
{
|
||
// 计算角度差的符号来确定转向方向
|
||
double angle_diff = std::fmod(des_direction - cur_direction + 360.0, 360.0);
|
||
|
||
if (angle_diff > 180)
|
||
return Direction::LEFT_TURN;
|
||
else
|
||
return Direction::RIGHT_TURN;
|
||
}
|
||
}
|
||
|
||
// 加载路径文件的函数
|
||
bool load_path_file()
|
||
{
|
||
LOG_INFO("开始加载路径文件: gps_load_now.txt");
|
||
memset(GPSPointQueue, 0, sizeof(GPSPointQueue));
|
||
|
||
FILE* fp_gps = fopen("gps_load_now.txt", "r");
|
||
if (NULL == fp_gps)
|
||
{
|
||
LOG_ERROR("打开路径文件失败: gps_load_now.txt");
|
||
return false;
|
||
}
|
||
LOG_INFO("成功打开路径文件");
|
||
|
||
char mystring[200];
|
||
memset(mystring, '\0', 200);
|
||
int point_num = 0;
|
||
int i = 0;
|
||
while (fgets(mystring, 200, fp_gps))
|
||
{
|
||
i++;
|
||
|
||
string line_string(mystring);
|
||
point_num = i / 4;
|
||
|
||
if (i % 4 == 1)
|
||
{
|
||
GPSPointQueue[point_num].LatitudeInfo = atof(line_string.c_str());
|
||
}
|
||
else if (i % 4 == 2)
|
||
{
|
||
GPSPointQueue[point_num].LongitudeInfo = atof(line_string.c_str());
|
||
}
|
||
else if (i % 4 == 3)
|
||
{
|
||
GPSPointQueue[point_num].Heading = atof(line_string.c_str());
|
||
}
|
||
}
|
||
|
||
GpsPointNum = i / 4;
|
||
LOG_INFO("路径文件加载完成，共 %d 个GPS点", GpsPointNum);
|
||
fclose(fp_gps);
|
||
|
||
return true;
|
||
}
|
||
|
||
void PL_ProcThread()
|
||
{
|
||
usleep(70000);
|
||
thread_exit_flag = 0; // 重置退出标志
|
||
LOG_INFO("PL_ProcThread 线程启动");
|
||
|
||
// 用于检测路径文件更新的变量
|
||
struct stat file_stat;
|
||
static time_t last_file_mtime = 0;
|
||
bool need_reload_path = false;
|
||
|
||
int road_pos = 0;
|
||
int des_pos = 3;
|
||
int direction_pos = 20;
|
||
auto next_time = std::chrono::steady_clock::now();
|
||
|
||
// 初始化时获取文件时间
|
||
if (stat("gps_load_now.txt", &file_stat) == 0)
|
||
{
|
||
last_file_mtime = file_stat.st_mtime;
|
||
LOG_INFO("初始获取路径文件mtime: %ld", last_file_mtime);
|
||
}
|
||
else
|
||
{
|
||
LOG_WARN("无法获取路径文件状态，文件可能不存在");
|
||
}
|
||
|
||
while (!thread_exit_flag) // 检查线程退出标志
|
||
{
|
||
next_time += std::chrono::milliseconds(50);
|
||
|
||
// 检查路径文件是否更新
|
||
if (stat("gps_load_now.txt", &file_stat) == 0)
|
||
{
|
||
if (file_stat.st_mtime != last_file_mtime)
|
||
{
|
||
LOG_INFO("检测到路径文件更新 (mtime: %ld -> %ld)，准备重新加载...", last_file_mtime,
|
||
file_stat.st_mtime);
|
||
last_file_mtime = file_stat.st_mtime;
|
||
need_reload_path = true;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
LOG_WARN_THROTTLE(5000, "检查路径文件状态失败");
|
||
}
|
||
|
||
// 如果需要重新加载路径
|
||
if (need_reload_path)
|
||
{
|
||
LOG_INFO("开始重新加载路径文件...");
|
||
if (load_path_file())
|
||
{
|
||
LOG_INFO("路径文件重新加载成功，重置路径索引");
|
||
road_pos = 0;
|
||
des_pos = 3;
|
||
direction_pos = 20;
|
||
g_last_road_pos = -1; // 重置单调性约束，允许在新路径上自由搜索
|
||
task_status = TaskStatus::RUNNING;
|
||
need_reload_path = false;
|
||
LOG_INFO("#################### 开始执行新路径! ####################");
|
||
}
|
||
else
|
||
{
|
||
LOG_ERROR("路径文件重新加载失败");
|
||
need_reload_path = false;
|
||
}
|
||
}
|
||
|
||
// 检查 GPS 点数，确保至少有几个点才开始寻迹
|
||
if (GpsPointNum < 5)
|
||
{
|
||
// 如果点数不够，等待文件更新
|
||
x = 0.0;
|
||
y = 1.0;
|
||
pl_speed = 0;
|
||
// 不设置 COMPLETED 状态
|
||
LOG_INFO_THROTTLE(1000, "等待 GPS 路径点加载完成，当前点数: %d", GpsPointNum);
|
||
usleep(10000);
|
||
continue;
|
||
}
|
||
|
||
g_NavInfo.Latitude_degree = g_rtk.lat;
|
||
g_NavInfo.Longitude_degree = g_rtk.lon;
|
||
g_NavInfo.Yaw_rad = g_rtk.direction;
|
||
// double head_raw = g_NavInfo.Yaw_rad;
|
||
|
||
road_pos = Road_Planning_Find_Nearest_Point(des_pos - 3, g_NavInfo, GPSPointQueue);
|
||
des_pos = Road_Planning_Find_Aim_Point(road_pos, 3); // 找到当前点后第 3 个点，作为目标点
|
||
direction_pos = Road_Planning_Find_Aim_Point(road_pos, 6); // 找到当前点后第 6 个点，判断直行/转弯
|
||
|
||
drive_mode = straight_or_turn(g_rtk.direction, GPSPointQueue[direction_pos].Heading, 12);
|
||
|
||
// 新增距离判断逻辑
|
||
double current_lon = g_rtk.lon;
|
||
double current_lat = g_rtk.lat;
|
||
double current_head = g_rtk.direction;
|
||
double target_lon = GPSPointQueue[des_pos].LongitudeInfo;
|
||
double target_lat = GPSPointQueue[des_pos].LatitudeInfo;
|
||
double target_head = GPSPointQueue[des_pos].Heading;
|
||
|
||
LOG_INFO_THROTTLE(1000, "循迹状态: 当前点=%d/%d, 目标点=%d", road_pos, GpsPointNum - 1, des_pos);
|
||
LOG_INFO_THROTTLE(1000, "当前位置: %.6f, %.6f, 朝向: %.1f", current_lon, current_lat, current_head);
|
||
LOG_INFO_THROTTLE(1000, "目标位置: %.6f, %.6f, 朝向: %.1f", target_lon, target_lat, target_head);
|
||
|
||
// 计算当前定位点与目标点距离
|
||
double distance = ntzx_GPS_length(current_lon, current_lat, target_lon, target_lat);
|
||
int nl_within_head = (abs(target_head - current_head) < 30) ? 1 : 0;
|
||
|
||
nl_within_radius = ((distance <= nl_radius) && nl_within_head) ? 1 : 0;
|
||
|
||
if (road_pos >= (GpsPointNum - 1))
|
||
{
|
||
LOG_INFO("到达路径终点: 当前点=%d, 总点数=%d", road_pos, GpsPointNum);
|
||
x = 0.0;
|
||
y = 1.0;
|
||
pl_speed = 0;
|
||
task_status = TaskStatus::PATH_FINISHED; // 单路径完成
|
||
LOG_INFO("设置任务状态为 PATH_FINISHED");
|
||
|
||
// 短暂休眠，确保车辆停稳
|
||
LOG_INFO("车辆停稳中...");
|
||
sleep(3);
|
||
continue;
|
||
}
|
||
else if (des_pos != -1)
|
||
{
|
||
double x_cm_tmp = 0;
|
||
double y_cm_tmp = 0;
|
||
|
||
ntzx_GPS_posit(g_rtk.direction, g_rtk.lon, g_rtk.lat, GPSPointQueue[des_pos].LongitudeInfo,
|
||
GPSPointQueue[des_pos].LatitudeInfo, &x_cm_tmp, &y_cm_tmp);
|
||
|
||
x = x_cm_tmp;
|
||
y = y_cm_tmp;
|
||
pl_speed = 800;
|
||
if (task_status != TaskStatus::RUNNING)
|
||
{
|
||
task_status = TaskStatus::RUNNING;
|
||
LOG_INFO("设置任务状态为 RUNNING");
|
||
}
|
||
}
|
||
|
||
// 避免忙等待，增加延迟让 CPU 能够处理其他任务
|
||
auto now = std::chrono::steady_clock::now();
|
||
if (now > next_time) next_time = now;
|
||
std::this_thread::sleep_until(next_time);
|
||
}
|
||
|
||
LOG_INFO("PL_ProcThread 线程退出");
|
||
return;
|
||
}
|
||
|
||
void* pl_thread(void* args)
|
||
{
|
||
(void)args;
|
||
sleep(3);
|
||
|
||
if (!load_path_file())
|
||
{
|
||
return NULL;
|
||
}
|
||
|
||
LOG_INFO("#################### auto drive on! ####################");
|
||
|
||
PL_ProcThread();
|
||
return NULL;
|
||
}
|