实现多层udp通信

lievech
于 2021-04-02 11:43:28 发布
阅读量176
点赞数
分类专栏： udp 文章标签： udp
本文链接：https://blog.csdn.net/lievech/article/details/115395076
版权
udp 专栏收录该内容
2 篇文章 0 订阅
订阅专栏
该代码实现了一个ROS节点，用于接收雷达数据并基于距离信息通过UDP进行多层通信。当检测到不同距离范围内的目标时，节点会发送不同的CAN数据，触发不同级别的警告或执行紧急刹车操作。主要涉及了ROS订阅、UDP套接字编程以及距离判断逻辑。
摘要由CSDN通过智能技术生成
实现多层udp通信


```cpp
//中控+pcw指示灯+cheju+baojing+shache

#include <cv_bridge/cv_bridge.h>
#include <glog/logging.h>
#include <ros/ros.h>

#include <algorithm>
#include <string>
#include <vector>
#include <iostream>
#include <math.h>
#include "aebs/RadarData.h"
#include "aebs/RadarPoint.h"
#include <stdio.h>
#include <sys/time.h>

#include <cmath>

#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <unistd.h>
#include <errno.h>
#include <string.h>
#include "arpa/inet.h"

//Boost
#include <boost/algorithm/string.hpp>
#include <boost/timer.hpp>
#include <boost/math/special_functions/round.hpp>
#include "boost/asio.hpp"
#include <boost/thread/thread.hpp>

#define SERV_PORT_1  7500 //7400
#define dest_port  4003

#define dest_port1  4001



bool switchFlag=true;
using namespace std;


class lhn_AEBS_class
{
public:
    string ip;
    int center_sock_fd;
    struct sockaddr_in center_addr_serv;
    struct sockaddr_in addr_client_center;
    int center_len;


    char *dest_ip;
    int sock_fd;
    struct sockaddr_in addr_serv;

    int ECU_sock_fd;
    struct sockaddr_in ECU_addr_serv;

    int voice_sock_fd;
    struct sockaddr_in voice_addr_serv;

    int d_sock_fd;
    struct sockaddr_in d_addr_serv;

    lhn_AEBS_class();
    ~lhn_AEBS_class();
    ros::NodeHandle nh_;
    ros::Subscriber sub_2;
    void poseMessageReceived(const aebs::RadarDataConstPtr &msg);
};
lhn_AEBS_class::lhn_AEBS_class()
{


//    ip = "192.168.0.55";
//    int center_sock_fd = socket(AF_INET, SOCK_DGRAM, 0);
//    if(center_sock_fd < 0)
//    {
//        perror("socket");
//        exit(1);
//    }
//    /* 设置address */
//    memset(&center_addr_serv, 0, sizeof(struct sockaddr_in));
//    center_addr_serv.sin_family = AF_INET;
//    center_addr_serv.sin_port = htons(SERV_PORT_1);
//    center_addr_serv.sin_addr.s_addr = inet_addr(ip.c_str());
//    center_len = sizeof(center_addr_serv);




    dest_ip = (char*)"192.168.0.178";
    sock_fd = socket(AF_INET, SOCK_DGRAM, 0);
    if (sock_fd < 0) {
        perror("socket");
        exit(1);
    }
    memset(&addr_serv, 0, sizeof(addr_serv));
    addr_serv.sin_family = AF_INET;
    addr_serv.sin_addr.s_addr = inet_addr(dest_ip);
    addr_serv.sin_port = htons(dest_port);
//
//
//
    dest_ip = (char*)"192.168.0.178";
    ECU_sock_fd = socket(AF_INET, SOCK_DGRAM, 0);
    if (ECU_sock_fd < 0) {
        perror("socket");
        exit(1);
    }
    memset(&ECU_addr_serv, 0, sizeof(ECU_addr_serv));
    ECU_addr_serv.sin_family = AF_INET;
    ECU_addr_serv.sin_addr.s_addr = inet_addr(dest_ip);
    ECU_addr_serv.sin_port = htons(dest_port1);
//
//
//
//    dest_ip = (char*)"192.168.0.178";
//    voice_sock_fd = socket(AF_INET, SOCK_DGRAM, 0);
//    if (voice_sock_fd < 0) {
//        perror("socket");
//        exit(1);
//    }
//    /* 设置address */
//    memset(&voice_addr_serv, 0, sizeof(voice_addr_serv));
//    voice_addr_serv.sin_family = AF_INET;
//    voice_addr_serv.sin_addr.s_addr = inet_addr(dest_ip);
//    voice_addr_serv.sin_port = htons(dest_port);
//
//
//
//    dest_ip = (char*)"192.168.0.178";
//    d_sock_fd = socket(AF_INET, SOCK_DGRAM, 0);
//    if (d_sock_fd < 0) {
//        perror("socket");
//        exit(1);
//    }
//    /* 设置address */
//    memset(&d_addr_serv, 0, sizeof(d_addr_serv));
//    d_addr_serv.sin_family = AF_INET;
//    d_addr_serv.sin_addr.s_addr = inet_addr(dest_ip);
//    d_addr_serv.sin_port = htons(dest_port);




    sub_2 = nh_.subscribe<aebs::RadarData>("radardata", 1, boost::bind(&lhn_AEBS_class::poseMessageReceived,this,_1));
}
lhn_AEBS_class::~lhn_AEBS_class()
{

}

//subECUData_ = nodehandle_.subscribe<chang_an_msgs::Report>("ecudatareport", 1, boost::bind(&PostProcess::ECUDataHandler,this,_1));

void lhn_AEBS_class::poseMessageReceived(const aebs::RadarDataConstPtr &msg)
{

//    unsigned char center_recv_buf[13];
//    int recv_num_center = recvfrom(center_sock_fd, center_recv_buf, sizeof(center_recv_buf), 0, (struct sockaddr *) &addr_client_center,(socklen_t *) &center_len);
//
//
//
//    if (recv_num_center < 0) {
//        perror("recvfrom error:");
//        exit(1);
//    }
//
//    else if (center_recv_buf[0] == 1) {

        double Position_y, Position_y_min = 1000, car_id = 0;
        for (int i = 0; i < 32; ++i) {
            //判断条件有问题
            //cout<<'333333333'<<endl;
            if (msg->delphi_detection_array[i].x > (-0.5) && msg->delphi_detection_array[i].x < (0.5)) {
                Position_y = msg->delphi_detection_array[i].y;
                //cout<<"y = "<<Position_y<<endl;
                //if (Position_y < Position_y_min && Position_y != 0.) {
                //Position_y_min = Position_y;
                double d;
                d = Position_y;
    //                    cout << "d = " << d << endl;
                car_id = i;


    //            d=10;

                unsigned char zjm_can_data[13];
                zjm_can_data[0] = 0x08;
                zjm_can_data[1] = 0x00;
                zjm_can_data[2] = 0x00;
                zjm_can_data[3] = 0x07;
                zjm_can_data[4] = 0x32;
                zjm_can_data[5] = 0x00;
                zjm_can_data[6] = 0x00;
                zjm_can_data[7] = 0x00;
                zjm_can_data[8] = 0x00;
                zjm_can_data[9] = 0x00;
                zjm_can_data[11] = 0x00;
                zjm_can_data[12] = 0x00;


                unsigned char ECU_data[13];
                ECU_data[0] = 0x08;
                ECU_data[1] = 0x10;
                ECU_data[2] = 0xF8;
                ECU_data[3] = 0xE3;
                ECU_data[4] = 0xF3;
                ECU_data[5] = 0x00;
                ECU_data[6] = 0x00;
                ECU_data[7] = 0x00;
                ECU_data[8] = 0x00;
                ECU_data[9] = 0x00;
                ECU_data[10] = 0x00;
                ECU_data[11] = 0x00;
                ECU_data[12] = 0x00;
    //


                if (d > 15.0 && d < 25.0) {
                    zjm_can_data[10] = 0x00;
                    int send_num = sendto(sock_fd, zjm_can_data, sizeof(zjm_can_data), 0, (struct sockaddr *) &addr_serv,
                                          sizeof(addr_serv));

                    zjm_can_data[3] = 0x07;
                    zjm_can_data[4] = 0x37;
                    zjm_can_data[5] = 0x01;
                    zjm_can_data[7] = 0x01;
                    int send_num1 = sendto(sock_fd, zjm_can_data, sizeof(zjm_can_data), MSG_DONTWAIT,
                                            (struct sockaddr *) &addr_serv, sizeof(addr_serv));
                    cout << "安全" << d << endl;
                    if (send_num < 0) {
                        cout << "send fail" << endl;
                    }
                }


                else if (d > 6.5 && d <= 15.0) {
                    zjm_can_data[10] = 0x01;
                    int send_num = sendto(sock_fd, zjm_can_data, sizeof(zjm_can_data), 0, (struct sockaddr *) &addr_serv,
                                          sizeof(addr_serv));

                    zjm_can_data[3] = 0x07;
                    zjm_can_data[4] = 0x37;
                    zjm_can_data[5] = 0x01;
                    zjm_can_data[7] = 0x02;
                    int send_num2 = sendto(sock_fd, zjm_can_data, sizeof(zjm_can_data), MSG_DONTWAIT,
                                            (struct sockaddr *) &addr_serv, sizeof(addr_serv));
                    cout << "一级警告" << d << endl;
                    if (send_num < 0) {
                        cout << "send fail" << endl;
                    }

                }

                else if (d > 3.6 && d <= 6.5) {
                    zjm_can_data[10] = 0x01 << 6;
                    int send_num = sendto(sock_fd, zjm_can_data, sizeof(zjm_can_data), 0, (struct sockaddr *) &addr_serv,
                                          sizeof(addr_serv));


                    zjm_can_data[3] = 0x07;
                    zjm_can_data[4] = 0x34;
                    zjm_can_data[5] = 0x00;
                    int send_num_voice1 = sendto(sock_fd, zjm_can_data, sizeof(zjm_can_data), MSG_DONTWAIT,
                                                (struct sockaddr *) &addr_serv, sizeof(addr_serv));


                    zjm_can_data[3] = 0x07;
                    zjm_can_data[4] = 0x37;
                    zjm_can_data[5] = 0x01;
                    zjm_can_data[7] = 0x03;
                    int send_num_d = sendto(sock_fd, zjm_can_data, sizeof(zjm_can_data), MSG_DONTWAIT,
                                            (struct sockaddr *) &addr_serv, sizeof(addr_serv));
                    cout << "二级警告" << d << endl;
                    if (send_num < 0) {
                        cout << "send fail" << endl;
                    }
                }


                else if (d > 0.2 && d <= 3.6) {
                    zjm_can_data[10] = 0x01 << 6;
                    int send_num = sendto(sock_fd, zjm_can_data, sizeof(zjm_can_data), 0, (struct sockaddr *) &addr_serv,
                                          sizeof(addr_serv));

                    int send_num_ecu = sendto(ECU_sock_fd, ECU_data, sizeof(ECU_data), MSG_DONTWAIT,
                                              (struct sockaddr *) &ECU_addr_serv, sizeof(ECU_addr_serv));


                    zjm_can_data[3] = 0x07;
                    zjm_can_data[4] = 0x34;
                    zjm_can_data[5] = 0x01;
                    int send_num_voice = sendto(sock_fd, zjm_can_data, sizeof(zjm_can_data), MSG_DONTWAIT,
                                                (struct sockaddr *) &addr_serv, sizeof(addr_serv));

                    zjm_can_data[3] = 0x07;
                    zjm_can_data[4] = 0x37;
                    zjm_can_data[5] = 0x01;
                    zjm_can_data[7] = 0x04;
                    int send_num3 = sendto(sock_fd, zjm_can_data, sizeof(zjm_can_data), MSG_DONTWAIT,
                                           (struct sockaddr *) &addr_serv, sizeof(addr_serv));
                    cout << "紧急刹车" << d << endl;
                    if (send_num < 0) {
                        cout << "send fail" << endl;
                    }

                }


//                else(d>) {
//                    zjm_can_data[10] = 0x00;
//                    int send_num = sendto(sock_fd, zjm_can_data, sizeof(zjm_can_data), 0, (struct sockaddr *) &addr_serv,
//                                          sizeof(addr_serv));
//
//    //                d_data[7] = 0x00;
//    //                int send_num_d = sendto(d_sock_fd, d_data, sizeof(d_data), MSG_DONTWAIT,
//    //                                        (struct sockaddr *) &d_addr_serv, sizeof(d_addr_serv));
//                    if (send_num < 0) {
//                        cout << "send " << endl;
//                    }
//               }
            }

        }

 }


int main(int argc, char** argv) {


    ros::init(argc, argv, "AEBS2");

    //ros::NodeHandle nh;

//    ros::Subscriber sub_2 = nh.subscribe("/radardata", 1, &lhn_AEBS_class::poseMessageReceived);
    lhn_AEBS_class lhn;
    ros::spin();

    //return 0;

}