相机和毫米波雷达数据融合4--解析模块代码编写

最新推荐文章于 2024-07-22 20:24:36 发布

原创

最新推荐文章于 2024-07-22 20:24:36 发布 · 2.7k 阅读

19 ·

CC 4.0 BY-SA版权

文章标签：

#c++ #socket #自动驾驶 #linux

这篇博客介绍了如何在ROS环境下实现相机和毫米波雷达数据的融合，以及通过CAN总线进行通信。首先，文章阐述了流程框架，包括数据融合的四个步骤。接着，详细讲解了`can_ros_class`类的设计，该类用于处理CAN信号的收发，并结合Simulink生成的代码进行信号解析。`Can0RosClass`包含ROS节点的初始化、订阅者和发布者的创建，以及解析和发送CAN消息的回调函数。`Can0MsgSend_CommandMsg`和`Can0MsgReceive`函数分别处理发送和接收CAN消息。博客最后提供了相关源文件的结构和内容，以及`CMakeLists.txt`和`package.xml`文件的作用。整个实现涉及到了ROS节点、消息类型、CAN信号的解析和转换等关键环节。

相机和毫米波雷达数据融合1–流程框架

相机和毫米波雷达数据融合2–SocketCan编写

相机和毫米波雷达数据融合3–Simulink解析Can信号

Simulink代码介绍

代码生成的文件介绍

在这里插入图片描述
这一共6个文件是你所需要移到你的Ros功能包下的。把头文件放在这个include文件夹中，cpp文件放在src里。

在这里插入图片描述
然后路径记得包含，检查一下有没有错误。

can_ros_class介绍

前面我们有介绍can_vehicle_class，用这个类我们可以实现can信号的收发。但是在收之后，发之前，我们需要利用simulink生成的代码来进行转换，而且里面的控制信息，和反馈信息应该是以ROS的msg存在于ROS网络中的，所以我们写一个can_ros_class类来完成这个。（这两个类能写一起吗？问问评论区c++大佬）下面以can0为例介绍这个。

can_ros_class.h

#ifndef CAN0_ROS_CLASS_H
#define CAN0_ROS_CLASS_H

#include <ros/ros.h>
#include "can0_node_test/Can0_to_msg.h"
#include "can0_node_test/Can0_to_param.h"
#include "can_vehicle_class.h"
#include "dbc_trans.h"

class Can0RosClass
{
   
   
private:
    ros::NodeHandle nh_;
    ros::Subscriber subscriber_;

    void InitSubscribers(CanVehicleClass& can_name,camera_dbcModelClass& rtObj);
    void InitPublishers();

public:
    static camera_dbcModelClass rtObj;
    Can0RosClass(ros::NodeHandle& nodehandle,char* name);
    CanVehicleClass can0_;
    ros::Publisher publisher_;
};

void Can0MsgSend_CommandMsg(const can0_node_test::Can0_to_msgConstPtr &can0_param,CanVehicleClass& can_name, camera_dbcModelClass& rtObj);
void Can0MsgReceive(Can0RosClass& Can0RosClass,ros::Publisher& pub,camera_dbcModelClass& rtObj);
#endif

这个类包含一个ros句柄（用来创建publisher和subscriber）；一个publisher用来把解析后的到本车状态信息发出去；一个subscriber用来接收从其他节点传进来控制信息，然后调用相应的回调函数；一个静态类rtobj（simulink生成代码里的类，用来进行信号解析）；一个CanVehicleClass（用来实现socketcan的功能）。一个publisher初始化函数；一个subscriber初始化函数，还有一个类构造函数。

之后还定义了两个函数分别是发送函数和接收函数。

can_ros_class.cpp

头文件包含

#include "can0_t/can0_ros_class.h"
#include "can0_t/dbc_trans.h"
#include "can0_node_test/Can0_to_msg.h"
#include "can0_node_test/Can0_to_param.h"

注意这里的头文件包含，有些头文件在头文件中的头文件已经被包含了，不用重复包含。第三个和第四个头文件在之后会详述。

Can0RosClass::Can0RosClass(params)

这是该类的构造函数。

Can0RosClass::Can0RosClass(ros::NodeHandle& nodehandle, char* name):nh_(nodehandle),can0_(name)
{
   
   
    ROS_INFO("in class constructor of Can0RosClass");
    InitSubscribers(this->can0_,this->rtObj);
    InitPublishers();
}

这个构造函数中完成了对can0_vehicle_class和Ros结点的构造，而前者在构造时就会完成Can_Init函数，也就是和相应的can口绑定。之后会创建相应的subscriber和publisher。

Can0RosClass::InitSubscribers（params）

void Can0RosClass::InitSubscribers(CanVehicleClass& can_name,camera_dbcModelClass& rtObj)
{
   
   
    ROS_INFO("Initializing Subscribers");
    subscriber_=nh_.subscribe<can0_node_test::Can0_to_msg>("Can0_to_msg",1,boost::bind(&Can0MsgSend_CommandMsg,_1,can0_,rtObj));
}

传入的参数是实现socketcan功能的CanVehicleClass类，还有实现信号解析的simulink代码生成的类。传入这两个参数是因为绑定的回调函数Can0MsgSend_CommandMsg需要这两个参数，后者把控制参数转换成can信号，前者把can信号发送出去。这里不太明白的话，可以去搜一下多参数回调函数ROS。

Can0RosClass::InitPublishers()

void Can0RosClass::InitPublishers()
{
   
   
    ROS_INFO("Initializing Publishers");
    publisher_=nh_.advertise<can0_node_test::Can0_to_param>("Can0_to_param",1,true);
}

这个函数就是创建一个publisher，他会用来发布底盘的状态信号。

Can0_to_msg

我们来看看subscriber发送的msg格式是什么样的

float64 Gear_shift_req
float64 Steering_Pos_req
float64 Drive_Tq_Req
float64 Brake_Tq_Req
float64 Parking_Cmd

里面定义了需要的档位，需要的转向角度，需要的驱动力矩，需要的制动力矩，需要的制动指令。所以这个msg是从算法结点传进这个结点的，然后会调用回调函数，将这些信号打包成can信号发送出去。

Can0_to_param

再看看publisher发送出去的msg格式是什么样的

Header header
float64 C1_Host_velocity_vcu
float64 C1_Steering_angle
float64 C1_Gear_pos
float64 C1_Torque_fbk
float64 C3_Gear_Enb_fbk
float64 C3_Steering_Enb_fbk
float64 C3_Drive_Enb_fbk
float64 C3_Brake_Enb_fbk
float64 C3_Parking_Enb_fbk

定义了一个headr（里面有time stamp 后面时间同步需要用到），然后就是一系列的反馈信号。所以这么msg是这个结点将can信号解析之后，将信息赋值给这个msg得到的。他将被publish到其他结点去。

Can0MsgSend_CommandMsg（params）

void Can0MsgSend_CommandMsg(const can0_node_test::Can0_to_msgConstPtr &can0_param, CanVehicleClass& can_name, camera_dbcModelClass& rtObj)
{
   
                               
        can_name.send_msg_vec.clear();
        //档位
        if(can0_param->Gear_shift_req > 0)
        {
   
   
            rtObj.rtU.Can0_Input_param.Gear_A1_Param1.Gear_Enable_control = 1;
            rtObj.rtU.Can0_Input_param.Gear_A1_Param1.Gear_Shift_Req = can0_param->Gear_shift_req;//01：驻车，02：空挡，03：前进，04：后退，05：无效
            rtObj.rtU.Can0_Input_param.Gear_A1_Param1.IPC_Mode_Shift = 0;
            rtObj.rtU.Can0_Input_param.Gear_A1_Param1.IPC_Stop_Eme = 0;
        }
        else
        {
   
   
            rtObj.rtU.Can0_Input_param.Gear_A1_Param1.Gear_Enable_control = 0;
            rtObj.rtU.Can0_Input_param.Gear_A1_Param1.Gear_Shift_Req = 0;
            rtObj.rtU.Can0_Input_param.Gear_A1_Param1.IPC_Mode_Shift = 0;
            rtObj.rtU.Can0_Input_param.Gear_A1_Param1.IPC_Stop_Eme = 0;
        }
        //转向
        if(can0_param->Steering_Pos_req > 0)
        {
   
   
            rtObj.rtU.Can0_Input_param.Steering_A2_Param1.Steering_Pos_Req = can0_param->Steering_Pos_req;//前轮转角-28到28
            rtObj.rtU.Can0_Input_param.Steering_A2_Param1.Steering_Enb = 1;
        }
        else
        {
   
   
            rtObj.rtU.Can0_Input_param.Steering_A2_Param1.Steering_Pos_Req = 0;
            rtObj.rtU.Can0_Input_param.Steering_A2_Param1.Steering_Enb = 0;
        }
        //驱动
        if(can0_param->Drive_Tq_Req