本文详细介绍了使用大陆毫米波雷达ARS408-21XX进行硬件链接、代码解析及ROS集成的过程。作者分享了学习资源，包括基于ROS的开源代码，并提供了简化版的解析程序，实现了障碍物坐标及速度的发布，同时利用rviz进行可视化。此外，还给出了配置和检查雷达的函数，以及can帧的解析函数。
前言
从我个人的学习成长历程来看，从0到1确实很难。我个人在对这款毫米波雷达的学习的过程中也比较痛苦，资料缺乏，自己的能力也不是很厉害，而且我们这边的实验室之前也没有师兄们搞过，一个人在探索，所有学习资料都从github和其他优秀的博主中获取。这边推荐我在学习这款大陆毫米波雷达时，经常看的一些好的博客，这边感谢他们的知识分享，同时也感谢其他帮助过我的前辈：
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:"<<obj_x<<", "<<"y:"<<obj_y<<", "<<"Vx:"<<obj_Vx;
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<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
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<pro_can::sensorData>("sensorRawData", 10);
//rviz可视化话题
ros::NodeHandle nh;
ros::Publisher markerArrayPub = nh.advertise<visualization_msgs::MarkerArray>("visualization_marker_radar", 10);
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模式的数据解析，实现了障碍物相对于毫米波雷达的坐标发布，以及横纵向速度的发布，并简单做了筛选，仅此而已。后期将继续上传更新优化后的解析、筛选程序。
2257
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
