文章目录
前言一、硬件链接二、代码如何使用三、大陆毫米波雷达ARS408-21XX解析代码说明总结
前言
从我个人的学习成长历程来看，从0到1确实很难。我个人在对这款毫米波雷达的学习的过程中也比较痛苦，资料缺乏，自己的能力也不是很厉害，而且我们这边的实验室之前也没有师兄们搞过，一个人在探索，所有学习资料都从github和其他优秀的博主中获取。这边推荐我在学习这款大陆毫米波雷达时，经常看的一些好的博客，这边感谢他们的知识分享，同时也感谢其他帮助过我的前辈：
博客1博客2博客3
Tips：大陆毫米波雷达ASR408-21XX的相关资料包见如下的链接: ARS408-21XX说明资料
Tips：github上也已经有了关于这款ARS408-21XX毫米波雷达的开源代码（基于ROS，主要语言是C/C++），其功能是实现了数据解析和发送，我这边已经下载好了，链接如下: ARS408雷达github开源代码
但是，因为我个人的能力有限而且时间不是很充裕（暑假得将matlab下大论文的代码转成C++语言和弄大论文数据），所以我个人还没我完全吃透这个代码，因此我在使用这款代码时，我借鉴了这个开源代码，自己写了一个ARS408-21XX毫米波雷达解析程序，比较简单，但足够我使用了，我这边仅使用毫米波雷达的object目标模式，没使用cluster点云。最后还基于rviz做了个简单目标可视化，方便对radar进行空间标定,这边提供了该代码的下载链接: 本人撰写的关于ARS408-21XX解析代码（仅供参考）
一、硬件链接
我这边使用的can卡是peakcan，关于如何将其部署到自己的电脑上，参考我的这篇博客： peakcan驱动安装
二、代码如何使用
当完成硬件连接之后，并下载了我这边提供的仅供参考的radar解析程序后，进入代码的工作空间，使用catkin_make进行编译（该代码基于ROS，使用了C语言）。 打开两个终端，一个启动roscore，一个进入radar_byqjj_ws后，执行如下指令
$ source devel/setup.bash
$ rosrun pro_can radarCan_solve
若出现如下警告：说找不到can0设备，这边建议您，先执行Ctrl+z退出该程序，然后稍微等待几秒，再重新执行“rosrun pro_can radarCan_solve”指令，因为有时候电脑搜索该can卡比较慢，所以需要稍微等待一小会，若还不行，请重试这个步骤。 图1
当程序成功运行起来之后，再打开一个新的终端，执行以下指令，打开rviz：
$ rosrun rviz rviz
然后点击rviz左上角的flie，选择open config，然后选择我代码包里面提供的sensor_obj_visional.rviz配置文件，如下图所示 图2
效果如下图所示： 图3 周围环境如下： 图4
三、大陆毫米波雷达ARS408-21XX解析代码说明
提示：大陆毫米波雷达ASR408-21XX解析代码如下所示，因为我这边用的是object模式，所以我只写了object的解析代码。
#define down "sudo ip link set down can0" //关闭CAN0
#define command "sudo ip link set can0 type can bitrate 500000" //大陆ars408_radar的波特率为500Kbps
#define up "sudo ip link set up can0" //打开CAN0
/* 用于设置 ：can_frame*/
#define CAN_EFF_FLAG 0x80000000U //扩展 帧的标识
#define CAN_RTR_FLAG 0x40000000U //远程 帧的标识
#define CAN_ERR_FLAG 0x20000000U //错误 帧的标识，用于错误检查
/* 用于设置：can_filter*/
#define CAN_SFF_MASK 0x000007FFU /* standard frame format (SFF) */
#define CAN_EFF_MASK 0x1FFFFFFFU /* extended frame format (EFF) */
#define CAN_ERR_MASK 0x1FFFFFFFU /* omit EFF, RTR, ERR flags */
using namespace std;
Radar_408_60b radar_60b = {0,0,0,0,0,0,0};//dbc结构体
Radar_408_200 radar_200 = {0,0,0,0,0,0,0};//dbc结构体
Radar_408_202 radar_202 = {0,0,0,0,0,0};//dbc结构体
pro_can::sensorData sensor_data;//实例化的msg对象
bool once1 = true;
/*rviz可视化函数*/
void visualization(float & obj_x, float & obj_y, float & obj_Vx, int obj_ID, ros::Publisher & markerArrayPub){
visualization_msgs::MarkerArray marker_array;
//********************图形数组*********************//
visualization_msgs::Marker marker_1;
if (once1) {
marker_1.action = visualization_msgs::Marker::DELETEALL;
once1 = false;
}
else{
marker_1.action = visualization_msgs::Marker::ADD;
}
marker_1.header.frame_id = "sensor_frame";
marker_1.header.stamp = ros::Time::now();
marker_1.id = obj_ID;
marker_1.type = visualization_msgs::Marker::CUBE;//设置目标物体的形状
//设置物体的大小：
marker_1.scale.x = 0.5;
marker_1.scale.y = 0.5;
marker_1.scale.z = 0.5;
//物体的颜色
marker_1.color.r = 2;//radar红色
marker_1.color.g = 0;
marker_1.color.b = 0;
//设置透明程度 0：透明 1：不透明
marker_1.color.a = 1;
//物体的坐标(物体中心为质点)
marker_1.pose.position.x = obj_x;
marker_1.pose.position.y = obj_y;
marker_1.pose.position.z = 0.25;
//物体显示在rviz的时间
marker_1.lifetime = ros::Duration(0.01);//存在0.01s
marker_array.markers.push_back(marker_1);
//********************文字数组*********************//
visualization_msgs::Marker marker_2;
marker_2.action = visualization_msgs::Marker::ADD;
marker_2.header.frame_id = "sensor_frame";
marker_2.header.stamp = ros::Time::now();
marker_2.id = obj_ID + 100;
marker_2.type = visualization_msgs::Marker::TEXT_VIEW_FACING;//设置目标物为文本格式
//设置字体的大小：
marker_2.scale.x = 0.5;
marker_2.scale.y = 0.5;
marker_2.scale.z = 0.5;
//字体的颜色
marker_2.color.r = 2;//radar红色
marker_2.color.g = 0;
marker_2.color.b = 0;
//设置字体透明程度 0：透明 1：不透明
marker_2.color.a = 0.4;
//字体的坐标
marker_2.pose.position.x = obj_x;
marker_2.pose.position.y = obj_y;
marker_2.pose.position.z = 0.75;
//字体显示在rviz的时间
marker_2.lifetime = ros::Duration(0.01);//存在0.01s
//设置---字体显示的内容
ostringstream str;
str<<"Radar "<<"x:"< marker_2.text=str.str(); marker_array.markers.push_back(marker_2); //***************发送至rviz markerArrayPub.publish(marker_array); } ////&&&&&&&&&&&&&&&&&&&&&&&&&&&&-------------------------------------&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&& ///*can解析函数*/ void Radar_can2value(can_frame & frame, ros::Publisher & pub, ros::Publisher & markerArrayPub){ if (frame.can_id == 0x60b){ //*************字节段按大端（或小端方式）拼接 radar_60b.DATA0.D = frame.data[0]; radar_60b.DATA12.D = (frame.data[1] << 8) + frame.data[2]; radar_60b.DATA23.D = (frame.data[2] << 8) + frame.data[3]; radar_60b.DATA45.D = (frame.data[4] << 8) + frame.data[5]; radar_60b.DATA56.D = (frame.data[5] << 8) + frame.data[6]; float lat_distance = radar_60b.DATA23.bit.obj_lat*0.2 - 204.6;//横向距离（radar右手坐标系）,统一用右手坐标系 float long_distance= radar_60b.DATA12.bit.obj_long*0.2 - 500;//纵向距离 //*************筛选 if ( fabs(lat_distance) > 4.0){//横向绝对值大于4，表示若该障碍物的横向距离的绝对值在4.0米之外，则该帧不要 return ; } else if(long_distance > 60.0){//大于60米,表示若该障碍物的纵向距离大于60m，则该帧不要 return ; } else { //解析 sensor_data.sensorType = 1;//------1表示radar sensor_data.obj_ID = radar_60b.DATA0.bit.obj_id; sensor_data.X = radar_60b.DATA12.bit.obj_long*0.2 - 500; sensor_data.Y = radar_60b.DATA23.bit.obj_lat*0.2 - 204.6; sensor_data.Vx = radar_60b.DATA45.bit.obj_vlong*0.25 - 128; sensor_data.Vy = radar_60b.DATA56.bit.obj_vlat*0.25 - 64; //--------获取系统时间戳 uint64_t sys_time=std::chrono::duration_cast int32_t time_second = sys_time / 1000000; int32_t time_nsecs = sys_time % 1000000 * 1000; double timestamp_ = (double)time_second+1e-9*(double)time_nsecs; sensor_data.timestamp = timestamp_;//单位为：s //--------rviz可视化 int obj_ID = sensor_data.obj_ID; visualization(sensor_data.X, sensor_data.Y, sensor_data.Vx, obj_ID, markerArrayPub); //--------发往数据链队列 pub.publish(sensor_data); printf("Topic(sensorRawData)-----radar:目标ID%d------纵向距离%f-----纵向速度%f\n", sensor_data.obj_ID, sensor_data.X, sensor_data.Vx); } } return; } /*radar配置函数*/ bool radar_cfg(int & skt){ struct can_frame frame[2]; //struct Radar_408_200 radar_200; //radar配置使能 radar_200.DATA0.bit.RadarCfg_MaxDistance_Valid = 1;//允许配置最大距离 radar_200.DATA0.bit.RadarCfg_SensorID_Valid = 1;//允许配置radarID radar_200.DATA0.bit.RadarCfg_RadarPower_Valid = 1;//允许配置radar功率 radar_200.DATA0.bit.RadarCfg_OutputType_Valid = 1;//允许配置radar输出模式 radar_200.DATA0.bit.RadarCfg_SendQuality_Valid = 1;//允许配置radar输出cluster和object的质量信息 radar_200.DATA0.bit.RadarCfg_SendExtInfo_Valid = 1;//允许配置radar输出object的扩展信息 radar_200.DATA0.bit.RadarCfg_SortIndex_Valid = 1;//Object目标列表的当前排序索引值配置 radar_200.DATA0.bit.RadarCfg_StoreInNVM_Valid = 1;//使能；+++++++++++++++++++++++++++++++++ //radar相关参数配置 //最大距离设置 radar_200.DATA12.bit.RadarCfg_MaxDistance = ((150 - 0)/2);//150为真实物理值：150m radar_200.DATA12.bit.reserved =0; //保留位 radar_200.DATA3.bit.reserved =0; //设置radarID，输出类型，radar功率 radar_200.DATA4.bit.RadarCfg_sensorID = (0-0)/1;//radarID：为0 radar_200.DATA4.bit.RadarCfg_OutputType = (1-0)/1;//object模式 radar_200.DATA4.bit.RadarCfg_RadarPower = (1-0)/1;//标准发射功率 // radar_200.DATA5.bit.RadarCfg_CtrlRelay_Valid = 0; radar_200.DATA5.bit.RadarCfg_CtrlRelay = 0; radar_200.DATA5.bit.RadarCfg_SendQuality = 1; radar_200.DATA5.bit.RadarCfg_SendExtInfo = 1; radar_200.DATA5.bit.RadarCfg_SortIndex = 1;//按距离排序输出 radar_200.DATA5.bit.RadarCfg_StoreInNVM = 1;//使能；+++++++++++++++++++++++++++++++++ // radar_200.DATA6.bit.RadarCfg_RCS_threshold_Valid = 1; radar_200.DATA6.bit.RadarCfg_RCS_threshold = 0;//标准灵敏度 radar_200.DATA6.bit.RadarCfg_InvalidClusters_Valid = 0; radar_200.DATA6.bit.reserved = 0; // radar_200.DATA7.bit.RadarCfg_InvalidClusters = 0; //radar基本属性配置 frame[0].can_id = 0x200;//这样赋值默认标准帧。如果为扩展帧，那么 frame.can_id = CAN_EFF_FLAG | 0x123; frame[0].can_dlc = 8; //数据长度为 8个字节 frame[0].data[0] = radar_200.DATA0.D; frame[0].data[2] = radar_200.DATA12.D; frame[0].data[1] = radar_200.DATA12.D >> 8; frame[0].data[3] = radar_200.DATA3.D; frame[0].data[4] = radar_200.DATA4.D; frame[0].data[5] = radar_200.DATA5.D; frame[0].data[6] = radar_200.DATA6.D; frame[0].data[7] = radar_200.DATA7.D; //radar过滤器配置---------- frame[1].can_id = 0x202;//这样赋值默认标准帧 frame[1].can_dlc = 8; //数据长度为 8个字节 //过滤器使能 radar_202.DATA0.bit.reserved = 0; radar_202.DATA0.bit.FilterCfg_Valid = 1;//使能过滤器 radar_202.DATA0.bit.FilterCfg_Active = 1;//激活过滤器 radar_202.DATA0.bit.FilterCfg_Index = 0x9;//筛选y方向的距离用过滤条件为:0x9(长度为12bit),0x5为 radar_202.DATA0.bit.FilterCfg_Type = 1;//object过滤器 //最小距离设置 radar_202.DATA12.bit.FilterCfg_Min_X =(-1.8 + 409.5)/0.2;//车头右边1.8米以外的目标筛选掉 radar_202.DATA12.bit.reserved = 0; //最大距离设置 radar_202.DATA34.bit.FilterCfg_Max_X =(1.8 + 409.5)/0.2;//车头左边1.8米以外的目标筛选掉 radar_202.DATA34.bit.reserved = 0; radar_202.DATA5.bit.reserved = 0; radar_202.DATA6.bit.reserved = 0; radar_202.DATA7.bit.reserved = 0; frame[1].data[0] = radar_202.DATA0.D; frame[1].data[2] = radar_202.DATA12.D; frame[1].data[1] = radar_202.DATA12.D >> 8; frame[1].data[4] = radar_202.DATA34.D; frame[1].data[3] = radar_202.DATA34.D >> 8; frame[1].data[5] = radar_202.DATA5.D; frame[1].data[6] = radar_202.DATA6.D; frame[1].data[7] = radar_202.DATA7.D; int nbytes = write(skt, &frame[0], sizeof(frame[0])); //发送数据,第三个参数表示：需要发送的字节数 if(nbytes != sizeof(frame[0])){ //如果 nbytes 不等于帧长度，就说明发送失败 printf("Error\n!"); return false; }//配置失败，返回false int nbytes2_ = write(skt, &frame[1], sizeof(frame[1])); //发送数据,第三个参数表示：需要发送的字节数 if(nbytes != sizeof(frame[1])){ //如果 nbytes 不等于帧长度，就说明发送失败 printf("Error\n!"); return false; }//配置失败，返回false return true; } /*检查radar配置的函数*/ bool check_radar_cfg(int & skt){ struct can_frame recFrame; int nbytes = read(skt, &recFrame, sizeof(recFrame)); if(nbytes != sizeof(recFrame)){ //如果 nbytes 不等于帧长度，就说明发送失败 printf("Error\n!"); return false; } //该帧不是所需要的帧，返回false if(recFrame.can_id == 0x201){ struct Radar_408_201 radar_201; radar_201.DATA0.D = recFrame.data[0]; radar_201.DATA12.D = (recFrame.data[1] << 8) + recFrame.data[2]; radar_201.DATA34.D = (recFrame.data[3] << 8) + recFrame.data[4]; radar_201.DATA5.D = recFrame.data[5]; radar_201.DATA6.D = recFrame.data[6]; radar_201.DATA7.D = recFrame.data[7]; printf("当前radar的ID为：%d\n",radar_201.DATA34.bit.RadarState_SensorID); //radar功率 if(radar_201.DATA34.bit.RadarState_RadarPowerCfg == 0){ printf("当前radar的发射功率：std\n"); } else if(radar_201.DATA34.bit.RadarState_RadarPowerCfg == 1){ printf("当前radar的发射功率：3db\n"); } else if(radar_201.DATA34.bit.RadarState_RadarPowerCfg == 2){ printf("当前radar的发射功率：6db\n"); } else if(radar_201.DATA34.bit.RadarState_RadarPowerCfg == 3){ printf("当前radar的发射功率：9db\n"); } //radar输出模式 if(radar_201.DATA5.bit.RadarState_OutputTypeCfg == 0){ printf("当前radar的输出模式：不输出目标\n"); } else if(radar_201.DATA5.bit.RadarState_OutputTypeCfg == 1){ printf("当前radar的输出模式：object模式\n"); } else if(radar_201.DATA5.bit.RadarState_OutputTypeCfg == 2){ printf("当前radar的输出模式：cluster模式\n"); } sleep(1); return true; } else { return false; } } //***************************************功能函数******************************************************// void process_1(int & can_skt, struct ifreq ifr, struct sockaddr_can addr){ can_skt = socket(PF_CAN, SOCK_RAW, CAN_RAW);//创建 SocketCAN 套接字 strcpy(ifr.ifr_name, "can0" );//指定 can0 设备------------更换其他路 can，改动此 ioctl(can_skt, SIOCGIFINDEX, &ifr); addr.can_family = AF_CAN; addr.can_ifindex = ifr.ifr_ifindex; while(bind(can_skt, (struct sockaddr *)&addr, sizeof(addr)) < 0){ if (bind(can_skt, (struct sockaddr *)&addr, sizeof(addr)) > 0){ break; } printf("继续bind\n"); sleep(1); } printf("Bind success.\n"); return; } void process_2(int & can_skt){ while(radar_cfg(can_skt)==false){ if (radar_cfg(can_skt)==true){ break; } printf("Radar继续配置.\n"); sleep(1); } printf("Radar配置完成.\n"); return; } void process_3(int & can_skt){ while(check_radar_cfg(can_skt)==false){ if (check_radar_cfg(can_skt)==true){ break; } printf("Radar继续查看.\n"); sleep(1); } return; } void process_5(int can_skt, ros::Publisher pub, ros::Publisher markerArrayPub){ printf("主线程继续"); struct can_frame recFrame; int bytes; while (true)//一帧一帧的读取 { bytes = read(can_skt, &recFrame, sizeof(recFrame)); //接收总线上的报文保存在recFrame中 if (bytes != sizeof(recFrame)) { printf("rec Error\n!"); break; } else { Radar_can2value(recFrame,pub,markerArrayPub); } //usleep(10000); } } int main(int argc, char **argv) { //向ros系统注册节点 ros::init(argc, argv, "radarCan_solve"); //数据链话题 ros::NodeHandle n; ros::Publisher pub = n.advertise //rviz可视化话题 ros::NodeHandle nh; ros::Publisher markerArrayPub = nh.advertise system(down);//0路给的是：radar和vision system(command); system(up); //*******1******创建用于can收发的socket_can文件描述符 int can_skt; struct ifreq ifr; struct sockaddr_can addr; //-------------- process_1(can_skt, ifr, addr); //*******1******创建socket_can文件描述符号 //******2******radar_配置 process_2(can_skt); //******2******radar_配置 //******3******查看radar_配置 //process_3(can_skt); //******3******查看radar_配置 //******5*******解析can帧并发送实际物理值 process_5(can_skt, pub, markerArrayPub); //******5*******解析can帧并发送实际物理值 close(can_skt); //******4*******解析can帧并发送实际物理值 return 0; } 总结 查看该代码可知，此代码只是提供了object模式的数据解析，实现了障碍物相对于毫米波雷达的坐标发布，以及横纵向速度的发布，并简单做了筛选，仅此而已。后期将继续上传更新优化后的解析、筛选程序。