无人小巴自动寻迹（二）底盘和pl(路径规划）程序解读（sv2惯导+激光雷达）

底盘和pl程序:
底盘main.cpp中包含RecvFromCAN、RecvFromPL、Process、SendToCAN、RecvFromIMU、AnalysisIMU、SendCarState七个线程。

1.RecvFromCAN线程：
void *RecvFromCAN_pthread(void *arg)

vcu_vstate1_st它是一个关于车辆各种信息状态的结构，首先是结构体vcu_vstate1_st和vcu_dstate_st进行memset初始化
memset(&g_vcu_vstate1, 0, sizeof(vcu_vstate1_st));
memset(&g_vcu_dstate, 0, sizeof(vcu_dstate_st));

然后为了获取到vcu_vstate1_st和vcu_dstate_st结构数据需要与底盘相匹配can口建立通信，所以下一步can通信的初始化和can填充结构体数据的数据接受。


将底盘frame.data数据传输到中间g_vcu_vstate1中。
2.RecvFromIMU线程：
这个线程的功能类似于人工采集路径点的过程，思路是一样的，即提取sv2惯导GPS对自动搜索有用的数据：经纬度、速度、转向角。

和g_speed

3.SendCarState线程：
这个线程用于将上个线程中提取的经纬度、转向角放到该线程建立的gMC_state、gMC_NAV、gMC_DRIVE在三个结构中。
第一步:初始化三个结构。
memset(&gMC_state, 0, sizeof(gMC_state));
memset(&gMC_NAV, 0, sizeof(gMC_NAV));
memset(&gMC_DRIVE, 0, sizeof(gMC_DRIVE));

这里的速度不是惯导提取的速度，而是在使用之前can通信传输给vcu_vstate1_st中的speed.并将赋给了gMC_NAV结构体中的Speed_cm_ps，gMC_DRIVE也有速度。
第二步：然后通过UDP_send_data()函数和底盘pl建立路径规划socketUDP主要是通信gMC_NAV该结构发送给pl程序，发送函数是sendto函数。
sendbytes = sendto(brdcFd, &gMC_NAV, sizeof(gMC_NAV), 0, (struct sockaddr *)&Addr, sizeof(struct sockaddr));
4.pl程序中的WRC_UDPRECV线程：
该线程用于传输底盘gMC_NAV中的数据进行接受。
创建数组buffTEMP，并与底盘建立socketUDP通信。

这个地方pl程序既充当socket客户端，也充当socket的服务端，ret判断是否接收到底盘发送的数据是一个标志位。-1代表发送失败，0代表发送超时，其他值时代表发送成功recvfrom存储数据的函数buffTEMP最后通过线程锁传递给中间g_temp_NAV_info.pRecvBuff中。
5.WRC_UDPPROC处理线程：
该线程用于处理传输的数据。NAV（导航）、DRIVE（驱动）、STATE经过一系列的传输，三者的数据最终传输到pParam中。

6.pl_thread线程：
将保存在gps_load.txt人工收集的路径点包括经纬度、速度和转向角的传输g_pMoRoadToPL中，并统计point_num。
第一步：创建g_pMoRoadToPL结构体和初始化
memset(&g_pMoRoadToPL,0,sizeof(WRC_MO_Road_To_PL));
第二步：读取gps_load.txt保存在中间的经纬度、速度和转角信息mystring接受这个数组lat_s、lon_s、sp_s接受相应位置的经纬度和速度。最后传递给结构体g_pMoRoadToPL.GPSPointQueue[point_num]中。


7.PL_ProcThread线程：
这个线程是整个搜索的核心。搜索的两个关键点(寻找最小路径点和目标点)。
第一步：创建结构体g_PLResultToMC并初始化。
memset(&g_PLResultToMC,0,sizeof(WRC_PL_TO_MC));
第二步：定义了两个求最小路径点和目标点的函数Road_Planning_Find_Nearest_Point和Road_Planning_Find_Aim_Point。
road_pos = Road_Planning_Find_Nearest_Point(des_pos-5,pParam->g_NavInfo,g_pMoRoadToPL.GPSPointQueue);
des_pos = Road_Planning_Find_Aim_Point(road_pos,5,pParam->g_NavInfo,g_pMoRoadToPL.GPSPointQueue);
一是寻求最小路径点的函数：
输入的参数分别是：起点下标、车辆实时位置经纬度（一点）、人工采集路径点经纬度（全部）。
实时无人车位置坐标与人工采集的第一个要求距离（pre_distance),然后从第二点到人工采集的所有点，分别要求它们的距离(得到aft_distance),然后通过大小比较算法，最终得到离无人车位置最近的人工路径点。返回最小路径点的下标并给予road_pos。找到最近的路径点。


接着是寻找目标点的函数：
输入参数分别是指起点的下标、到达目标点的数量（从起点到前5点的位置设置为目标点）、车辆的实时位置经纬度（一点）、人工采集路径点经纬度（全部）。
首先定义两个double类型的x_diff、y_diff，表示车辆需要转向目标点的角度（x轴、y轴）。若添加起点dest_num在收集路径点达到最后一点之前，让步dest_index = start_index dest_num，找到目标点。如果不满意，加上起点dest_num如果下标大于最后一点，则将目标点设置为最后一点。
然后车辆需要在目标点行驶。x_diff、y_diff，用到ntzx_GPS_posit计算得到这个函数。最后也得到了。des_pos目标点下标。


找到第一个目标点后，再找到第二个目标点。。。直到最后。找到最后一个目标点时，赋予它g_PLResultToMC中速为零停车。当不是最后一点时，继续给结构体g_PLResultToMC（x,y角度和速度)。

8.ThreadToMC线程：
创建了一个PLResultToMC_temp在上述步骤中获得的结构体（x,y)并将速度信息传输给结构。

9.RecvFromPL_pthread线程：
回到底盘代码，首先建立底盘和pl的udp通信，然后将pl那边的数据(包括（x,y）轴的信息和速度信息)最终保存在gWRC_PL_TO_MC中。


10 Process_thread线程跳转到direction_control、speed_control控制，对方向和速度做优化的处理。
11 最后通过CAN_send.cpp给出底盘最终速度等信息can的frame.data让车动起来。