自动驾驶之路径规划（PL）和底盘进行socket通信

最新推荐文章于 2024-11-07 14:05:19 发布

范猛男

最新推荐文章于 2024-11-07 14:05:19 发布

阅读量365

点赞数

分类专栏：自动驾驶文章标签：自动驾驶人工智能机器学习

本文链接：https://blog.csdn.net/weixin_48657767/article/details/123175648

版权

自动驾驶专栏收录该内容

21 篇文章 8 订阅

订阅专栏

一、首先在PL中得到mc_Param
mc_Param返回的是PL路径规划过后的转向角、速度、经纬度信息。

str_ntzx_pl_to_mc  Get_MC_Param(int dest_index, str_inv_to_pl inv_to_pl,str_ntzx_vehicle_waypoint* g_road_info_pl)
{   
    pthread_t g_SendTOMCHandle;
    static int angle_Compensation=0,angle_Compensation_last=0,angle_Compensation_llast=0;
    int angle_Now = inv_to_pl.courseAngle;
    float ki_fixed;
    double point_x,point_y;
    int angle_Dest = g_road_info_pl->waypoint_info[dest_index].courseAngle;
    //将修正角度始终控制在+/-180度。
    if(angle_Dest - angle_Now >180){
        angle_Now += 360;
    }
    if(angle_Dest - angle_Now <-180){
        angle_Dest += 360;
    }
        //航向角的补偿
    angle_Compensation = (angle_Dest - angle_Now);
        //转向时候补偿角系数变小
    if(abs(angle_Compensation_llast-angle_Compensation_last)>=1 && abs(angle_Compensation_last-angle_Compensation)>=1){
        ki_fixed = COMPENSATE_TURN;
        printf("进入转弯 ！\n");
    }else{
        ki_fixed = COMPENSATE_HEADING;
    }
    ntzx_GPS_posit(inv_to_pl.courseAngle,inv_to_pl.lon,inv_to_pl.lat,g_road_info_pl->waypoint_info[dest_index].Longitude_degree,
    g_road_info_pl->waypoint_info[dest_index].Latitude_degree,&point_x,&point_y);
    //printf("目标点相对位置x：%f,y:%f\n",point_x,point_y);

    mc_Param.courseAngle = (int)(atan(point_x / point_y) *RAD2DEG)+(int)(ki_fixed*angle_Compensation); //单位是角度
    printf("转向角度计算：%d\n",mc_Param.courseAngle);
    mc_Param.drvCmd = NTZX_MC_ACTION_GO_FORWARD;
    mc_Param.earthCoordX_cm = 0;
    mc_Param.earthCoordY_cm = 0;
    if (ki_fixed== COMPENSATE_TURN)
    {
        mc_Param.speed_cm_ps = TURN_SPEED;
    }  
        mc_Param.speed_cm_ps = HEADING_SPEED;

    angle_Compensation_llast = angle_Compensation_last;
    angle_Compensation_last = abs(g_pl_to_mc.courseAngle);  //angle_Compensation;
 
    pthread_create(&g_SendTOMCHandle,NULL,ThreadToMC,NULL);
    pthread_join(g_SendTOMCHandle,NULL);

    return mc_Param;
}

二、以上创建一个ThreadToMC线程
ThreadToMC线程用于获取PL数据然后建立socket通信。
首先定义一个PL_TO_MC的结构体对象PLResultToMC_temp，然后建立一个线程锁将以上规划的四大数据给PLResultToMC_temp

pthread_mutex_lock(&g_MutexSendToMC);
        memset(&PLResultToMC_temp,0,sizeof(WRC_PL_TO_MC));
        memcpy(&PLResultToMC_temp,&mc_Param,sizeof(WRC_PL_TO_MC));
        pthread_mutex_unlock(&g_MutexSendToMC);

三、把IP数据包的地址给WRC_PL_TO_MC，并返回WRC_PL_TO_MC大小

nIPDataSize_MC = WRC_PL_Encode_IP_Data(&PLResultToMC_temp, &pIPDataToMC);

int WRC_PL_Encode_IP_Data(WRC_PL_TO_MC *pRes, void **pIPData)
{
	*pIPData = pRes;
	return sizeof(WRC_PL_TO_MC);
}

四、PL开始通过socket发送给底盘

nSendToMCSize = WRC_IP_send_to("MC", pIPDataToMC, nIPDataSize_MC);

int WRC_IP_send_to(const char* pModuleName, const void* pData, const unsigned int nBytes)
{
if(strcmp(pModuleName,“MC”)0)
{
if(isMCSocketInitfalse)
{
int ret=initialize(pModuleName);
if(ret<0)
{
printf(“PL : create m_UDPMCSocket failed!\n”);
return -1;
}
}
int ret=sendto(m_UDPMCSocket,pData,nBytes,0, (struct sockaddr*)&toaddr, sizeof(struct sockaddr)); //返回成功发送的字节数
if(ret==-1)
{
printf(“PL:%d\n”,errno);
}
return ret;
}
// if(strcmp(pModuleName,“MO”)0)
// {
// if(isMoSocketInit0)
// {
// int ret=initialize(pModuleName);
// if(ret<0)
// {
// LOG(“PL :create m_TCPMOSocket failed!\n”);
// return -2;
// }
// }
// int nsendSize=send(m_TCPMOSocket,pData,nBytes,0);
// return nsendSize;
// }
}
五、PL初始化与底盘建立连接

int initialize(const char *pModuleName)
{
if(strcmp(pModuleName,"MC")==0)
    {
        m_TCPMCSocket=socket(AF_INET,SOCK_STREAM,0);
        if(m_TCPMCSocket<0)
        {
            printf("PL : 创建与MC通信的套接字失败 !\n");
            return -3;
        }

        //发送缓冲区
        int nSendBuf=32*1024;//设置为32K
        setsockopt(m_TCPMCSocket, SOL_SOCKET, SO_SNDBUF, (const char*)&nSendBuf,sizeof(int));

        struct sockaddr_in server_sockaddr;
        memset(&server_sockaddr,0,sizeof(struct sockaddr_in));
        server_sockaddr.sin_family = AF_INET;
        server_sockaddr.sin_port = htons(8887);//服务器端口号
        server_sockaddr.sin_addr.s_addr = inet_addr("127.0.0.1");//服务器IP地址 //INADDR_ANY

        //step3、建立连接
        int ret = connect(m_TCPMCSocket, (struct sockaddr *)&server_sockaddr, sizeof(struct sockaddr *));
        if(ret==-1)
        {
            printf("PL : 与MC通信的套接字连接失败 Connect Failed !\n");
            close(m_TCPMCSocket);
            return -4;
        }
        //signal(SIGPIPE, sig_pipe2);   //若为SIG_IGN，SIGPIPE将交给系统处理
        isMCSocketInit=true;

        m_UDPMCSocket=socket(AF_INET,SOCK_DGRAM,0);
        if(m_UDPMCSocket<0)
        {
            printf("PL : 创建与MC通信的套接字失败 !\n");
            return -3;
        }

        // //发送缓冲区
        // int nSendBuf=32*1024;//设置为32K
        // setsockopt(m_UDPMCSocket, SOL_SOCKET, SO_SNDBUF, (const char*)&nSendBuf,sizeof(int));

        // memset(&toaddr,0,sizeof(struct sockaddr_in));
        // toaddr->sin_family = AF_INET;
        // toaddr->sin_port = htons(50003);//服务器端口号
        // toaddr->sin_addr.s_addr = inet_addr("127.0.0.1");//服务器IP地址 

        // //signal(SIGPIPE, sig_pipe2);   //若为SIG_IGN，SIGPIPE将交给系统处理
        // isMCSocketInit=true;
    }
    return 0;
}