文章目录
- 前言
- 一、硬件链接
- 二、代码如何使用
- 三、大陆毫米波雷达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"
#define command "sudo ip link set can0 type can bitrate 500000"
#define up "sudo ip link set up can0"
#define CAN_EFF_FLAG 0x80000000U
#define CAN_RTR_FLAG 0x40000000U
#define CAN_ERR_FLAG 0x20000000U
#define CAN_SFF_MASK 0x000007FFU
#define CAN_EFF_MASK 0x1FFFFFFFU
#define CAN_ERR_MASK 0x1FFFFFFFU
using namespace std;
Radar_408_60b radar_60b = {0,0,0,0,0,0,0};
Radar_408_200 radar_200 = {0,0,0,0,0,0,0};
Radar_408_202 radar_202 = {0,0,0,0,0,0};
pro_can::sensorData sensor_data;
bool once1 = true;
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;
marker_1.color.g = 0;
marker_1.color.b = 0;
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;
marker_1.lifetime = ros::Duration(0.01);
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;
marker_2.color.g = 0;
marker_2.color.b = 0;
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;
marker_2.lifetime = ros::Duration(0.01);
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);
markerArrayPub.publish(marker_array);
}
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;
float long_distance= radar_60b.DATA12.bit.obj_long*0.2 - 500;
if ( fabs(lat_distance) > 4.0){
return ;
}
else if(long_distance > 60.0){
return ;
}
else {
sensor_data.sensorType = 1;
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_;
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];
radar_200.DATA0.bit.RadarCfg_MaxDistance_Valid = 1;
radar_200.DATA0.bit.RadarCfg_SensorID_Valid = 1;
radar_200.DATA0.bit.RadarCfg_RadarPower_Valid = 1;
radar_200.DATA0.bit.RadarCfg_OutputType_Valid = 1;
radar_200.DATA0.bit.RadarCfg_SendQuality_Valid = 1;
radar_200.DATA0.bit.RadarCfg_SendExtInfo_Valid = 1;
radar_200.DATA0.bit.RadarCfg_SortIndex_Valid = 1;
radar_200.DATA0.bit.RadarCfg_StoreInNVM_Valid = 1;
radar_200.DATA12.bit.RadarCfg_MaxDistance = ((150 - 0)/2);
radar_200.DATA12.bit.reserved =0;
radar_200.DATA3.bit.reserved =0;
radar_200.DATA4.bit.RadarCfg_sensorID = (0-0)/1;
radar_200.DATA4.bit.RadarCfg_OutputType = (1-0)/1;
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;
frame[0].can_id = 0x200;
frame[0].can_dlc = 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;
frame[1].can_id = 0x202;
frame[1].can_dlc = 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;
radar_202.DATA0.bit.FilterCfg_Type = 1;
radar_202.DATA12.bit.FilterCfg_Min_X =(-1.8 + 409.5)/0.2;
radar_202.DATA12.bit.reserved = 0;
radar_202.DATA34.bit.FilterCfg_Max_X =(1.8 + 409.5)/0.2;
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])){
printf("Error\n!");
return false;
}
int nbytes2_ = write(skt, &frame[1], sizeof(frame[1]));
if(nbytes != sizeof(frame[1])){
printf("Error\n!");
return false;
}
return true;
}
bool check_radar_cfg(int & skt){
struct can_frame recFrame;
int nbytes = read(skt, &recFrame, sizeof(recFrame));
if(nbytes != sizeof(recFrame)){
printf("Error\n!");
return 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);
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");
}
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);
strcpy(ifr.ifr_name, "can0" );
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));
if (bytes != sizeof(recFrame))
{
printf("rec Error\n!");
break;
}
else
{
Radar_can2value(recFrame,pub,markerArrayPub);
}
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "radarCan_solve");
ros::NodeHandle n;
ros::Publisher pub = n.advertise<pro_can::sensorData>("sensorRawData", 10);
ros::NodeHandle nh;
ros::Publisher markerArrayPub = nh.advertise<visualization_msgs::MarkerArray>("visualization_marker_radar", 10);
system(down);
system(command);
system(up);
int can_skt;
struct ifreq ifr;
struct sockaddr_can addr;
process_1(can_skt, ifr, addr);
process_2(can_skt);
process_5(can_skt, pub, markerArrayPub);
close(can_skt);
return 0;
}
总结
查看该代码可知,此代码只是提供了object模式的数据解析,实现了障碍物相对于毫米波雷达的坐标发布,以及横纵向速度的发布,并简单做了筛选,仅此而已。后期将继续上传更新优化后的解析、筛选程序。
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)