网页和网站区别是什么,中国十大装修公司,wordpress编辑器 插件,.net网站开发技术简介之前遇到过“镭神32线激光雷达ROS下运行fromRosMsg()报错 Failed to find match for field “intensity“ 问题”#xff0c; 当时确定了是镭神C32雷达缺少相应字段#xff0c;并记录博客【学习记录】镭神32线激光雷达ROS下运行fromRosMsg()报错 Failed to find match for fi…之前遇到过“镭神32线激光雷达ROS下运行fromRosMsg()报错 Failed to find match for field “intensity“ 问题” 当时确定了是镭神C32雷达缺少相应字段并记录博客【学习记录】镭神32线激光雷达ROS下运行fromRosMsg()报错 Failed to find match for field “intensity“ 问题。
这次写了一个ros的节点代码接受原始雷达数据并转化为相应格式。 完整代码https://github.com/LarryDong/lslidar_PointXYZ2PointXYZIR 说明是另写了一个节点接受雷达发出的原始数据再赋予ring字段的信息然后再发布带有这个字段的点云。 但原始并没有包括intensity字段这个信息是丢失的所以intensity我就瞎补了一个0至少保证了格式正确。
基本原理
计算每个点对应的角度看距离激光雷达定义的哪条ring最接近。 如何判断最接近计算定义的两条ring平均值如果在左右两个平均值之间则认为是这个ring。 由于镭神32线雷达有两种角度模式左边这种均匀分布直接将角度近似取整就可以比较简单。但右侧这种不均匀分布的就需要按照手册给出的角度信息去解算到底属于哪根。
代码说明
首先列出雷达定义的角度并计算与上一个/下一个线束的平均值。
const vectorfloat g_ring_angle {-18, -15, -12, -10, -8, -7, -6, -5,-4, -3.33, -3, -2.66, -2.33, -2, -1.66, -1.33,-1, -0.66, -0.33, 0, 0.33, 0.66, 1, 1.33, 1.66, 2, 3, 4, 6, 8, 11, 14}; // ring angles defined by leishen
vectorfloat g_angle_range; // define a range between each ring angle.void initRingAngleRange(void){// calculate angle rangeassert(RING_NUMBERg_ring_angle.size());g_angle_range.push_back(-100); // assign a very large valuefor(int i0; iRING_NUMBER-1; i){float middle_value (g_ring_angle[i] g_ring_angle[i 1]) / 2; // calculate the average value between two ring.g_angle_range.push_back(middle_value);}g_angle_range.push_back(100);
}然后计算实际角度并赋予线束id即可
void lidarCallback(const sensor_msgs::PointCloud2ConstPtr msg_pc){pcl::PointCloudpcl::PointXYZ pc;pcl::PointCloudmyPointXYZIR pc_new;pcl::fromROSMsg(*msg_pc, pc);// convert to PointXYZIR.pc_new.points.reserve(pc.points.size());myPointXYZIR pt_new;for(const pcl::PointXYZ p : pc.points){float angle atan(p.z / sqrt(p.x * p.x p.y * p.y)) * 180 / M_PI;if(std::isnan(angle)) // remove nan point.continue;// int scanID int(angle 17); // 对于1°分辨率的雷达直接用这行指令就可以了不需要计算下面的不均匀分布角度。// for 0.33 degree mode.int scanID -1;for(int i0; iRING_NUMBER; i){if(angle g_angle_range[i] angle g_angle_range[i1]){scanID i;break;}}pt_new.x p.x;pt_new.y p.y;pt_new.z p.z;pt_new.intensity 0; // intensity is not used.pt_new.ring scanID;pc_new.points.push_back(pt_new);}sensor_msgs::PointCloud2 msg_pc_new;pcl::toROSMsg(pc_new, msg_pc_new);msg_pc_new.header.frame_id laser_link;msg_pc_new.header.stamp msg_pc-header.stamp;g_pub_pc.publish(msg_pc_new);ROS_INFO(Published new pointcloud.);}踩坑记录
一开始以为直接将雷达设置为1°模式扫描就可以了结果发现精度不正常。问了客服才知道1°模式和0.33°模式不能调整。所以无奈只能再重写这个转换代码。但还行算的挺对。rviz中PointCloud2的intensity选项点击后会出现一个channel还包括XYZ以及intensity等一开始没搞明白啥意思说明如下 About RViz: If you open a PointCloud2 display and select the “Intensity” color transformer, you can select a channel to display. This doesn’t have to be intensity, it can actually be any channel of your point cloud. If you leave “autocompute intensity bounds” checked, it will compute the min max for each point cloud separately and scale the color spectrum to that range. If you disable the check box, you can enter min max intensity manually (good if the min/max varies a lot between point clouds and you want the colors to be consistent between point clouds).
