裁剪rplidar的扫描数据

对于rplidar 雷达，特点是360度扫描结果，无死角。而有时候，我们没需要用到360度，只需要270度或者更少，这时需要在代码里对数据进行裁剪。

rplidar扫描角度的定义，如下图：

A1型号：

A2型号：

通常我们选取是laser 正前方的扇型数据，从上图可以看出

例如正面180度扇型数据，那么选取的度数为0~90，270~359的数据

270度面的扇型数据，选取度数为0~135， 225~359的数据

回到代码中

1.下载rplidar node的源码https://github.com/robopeak/rplidar_ros

2.打开node.cpp文件，简单看下逻辑

op_result = drv->grabScanData(nodes, count);

抓取一个完整的0-360度的扫描数据

op_result = drv->ascendScanData(nodes, count); //nodes[0] 代表0度角的扫描数据，即nodes[90]代表90度角的扫描数据

按照扫描角度升序方式将数据排序这样我们需要的数据就是nodes[0]~nodes[135]，nodes[225]~nodes[359]。

然后用publish_scan将数据发布出去，

我所采取的方式是修改publish_scan函数，即将0～360的数据都传入该函数，这样有没有设置angle_compensate都不影响发布的数据。

默认是angle_compensate = true，即传入的数据是已经优化过的。我也试过外面进行角度裁剪，不过看扫描的结果，优化后的数据都堆到一起了，十分不准确。

如果想在main函数中进行裁剪，一定要搞清楚角度优化的原理。搞清楚下面变量的作用和优化算法，待更新～～

const int angle_compensate_multiple = 1;
int angle_compensate_offset = 0;
rplidar_response_measurement_node_t angle_compensate_nodes[angle_compensate_nodes_count];
memset(angle_compensate_nodes, 0, angle_compensate_nodes_count*sizeof(rplidar_response_measurement_node_t));
int i = 0, j = 0;
for( ; i < count; i++ ) {if (nodes[i].distance_q2 != 0) {float angle = (float)((nodes[i].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f);int angle_value = (int)(angle * angle_compensate_multiple);if ((angle_value - angle_compensate_offset) < 0) angle_compensate_offset = angle_value;for (j = 0; j < angle_compensate_multiple; j++) {angle_compensate_nodes[angle_value-angle_compensate_offset+j] = nodes[i];}}
}

查看publish_scan函数:

void publish_scan(ros::Publisher *pub,rplidar_response_measurement_node_t *nodes, //扫描数据size_t node_count, ros::Time start, //扫描数据的个数，开始时间double scan_time, bool inverted,    //扫描时长，是否是倒置的float angle_min, float angle_max,   //最小角度和最大角度std::string frame_id)
{static int scan_count = 0;sensor_msgs::LaserScan scan_msg;scan_msg.header.stamp = start;scan_msg.header.frame_id = frame_id;scan_count++;bool reversed = (angle_max > angle_min);  //将最小和最大角度进行修正，是数据从小到大。if ( reversed ) {scan_msg.angle_min =  M_PI - angle_max;scan_msg.angle_max =  M_PI - angle_min;} else {scan_msg.angle_min =  M_PI - angle_min;scan_msg.angle_max =  M_PI - angle_max;}scan_msg.angle_increment =(scan_msg.angle_max - scan_msg.angle_min) / (double)(node_count-1);scan_msg.scan_time = scan_time;scan_msg.time_increment = scan_time / (double)(node_count-1);scan_msg.range_min = 0.15;scan_msg.range_max = 8.0;scan_msg.intensities.resize(node_count);scan_msg.ranges.resize(node_count);bool reverse_data = (!inverted && reversed) || (inverted && !reversed);if (!reverse_data) {for (size_t i = 0; i < node_count; i++) {float read_value = (float) nodes[i].distance_q2/4.0f/1000;if (read_value == 0.0)scan_msg.ranges[i] = std::numeric_limits<float>::infinity(); elsescan_msg.ranges[i] = read_value;  //这里可以看出，scan_msg.range与node[i]是一一对应的。scan_msg.intensities[i] = (float) (nodes[i].sync_quality >> 2);}} else {for (size_t i = 0; i < node_count; i++) {float read_value = (float)nodes[i].distance_q2/4.0f/1000;if (read_value == 0.0)scan_msg.ranges[node_count-1-i] = std::numeric_limits<float>::infinity();elsescan_msg.ranges[node_count-1-i] = read_value;scan_msg.intensities[node_count-1-i] = (float) (nodes[i].sync_quality >> 2);}}pub->publish(scan_msg);
}

修改后的代码：

void publish_scan(ros::Publisher *pub,rplidar_response_measurement_node_t *nodes,size_t node_count, ros::Time start,double scan_time, bool inverted,float angle_min, float angle_max,std::string frame_id)
{static int scan_count = 0;sensor_msgs::LaserScan scan_msg;scan_msg.header.stamp = start;scan_msg.header.frame_id = frame_id;scan_count++;bool reversed = (angle_max > angle_min); if ( reversed ) {scan_msg.angle_min =  M_PI - angle_max;scan_msg.angle_max =  M_PI - angle_min;} else {scan_msg.angle_min =  M_PI - angle_min;scan_msg.angle_max =  M_PI - angle_max;}scan_msg.angle_increment =(scan_msg.angle_max - scan_msg.angle_min) / (double)(node_count-1);scan_msg.scan_time = scan_time;scan_msg.time_increment = scan_time / (double)(node_count-1);scan_msg.range_min = 0.15;scan_msg.range_max = 8.0;scan_msg.intensities.resize(node_count);scan_msg.ranges.resize(node_count);bool reverse_data = (!inverted && reversed) || (inverted && !reversed); //修改后的代码reverse_data就没有用处了。/* 将rplidar放到hokuyo的位置，角度信息见上面的图如下0度/前270度/左   rplidar的方向  90度/右    180度/后kobuki接收到 LaserScan scan_msg.ranges数据对应的角度信息180度/前270度/左   kobuki的方向  90度/右    0度/后要把 0～90度对应的node数据映射到 180～90度的scan_msg.ranges中要把 90～180度对应的node数据映射到 90～0度的scan_msg.ranges中要把 180～270度对应的node数据映射到 359～270度的scan_msg.ranges中要把 270～359度对应的node数据映射到 270～180度的scan_msg.ranges中*/const size_t degree_90 = 90; //固定值，算法需要const size_t degree_270 = 270; //固定值，算法需要const size_t left_degrees = 225; // 裁剪的范围 保留数据225～359. const size_t right_degrees = 135; // 裁剪的范围 保留数据0～135. //先全部置inf，注意：如果初始化是0,则表示范围内无障碍，故不能置0。inf表示无数据 for (size_t i = 0; i < node_count; i++){ scan_msg.ranges[i] = std::numeric_limits<float>::infinity(); } //将数据分别对应设置进去 for (size_t i = 0; i < node_count; i++){float read_value = (float) nodes[i].distance_q2/4.0f/1000; if (i < right_degrees){ if (read_value == 0.0) scan_msg.ranges[2*degree_90 - i] = std::numeric_limits<float>::infinity(); else scan_msg.ranges[2*degree_90 - i] = read_value; scan_msg.intensities[2*degree_90 - i] = (float) (nodes[i].sync_quality >> 2); } else if (i > left_degrees) { if (read_value == 0.0) scan_msg.ranges[2*degree_270 - i] = std::numeric_limits<float>::infinity(); else scan_msg.ranges[2*degree_270 - i] = read_value; scan_msg.intensities[2*degree_270 - i] = (float) (nodes[i].sync_quality >> 2); } else { //do nothing; } } //发布出去 pub->publish(scan_msg);
}

讲需要裁剪的角度放到launch文件中，当作参数传入，比在代码中修改好很多

例如：在rplidar.launch文件中加入

<param name="cut_angle"    type="bool"   value="true"/>
<param name="right_degrees"    type="int"   value="90"/>
<param name="left_degrees"    type="int"   value="270"/>

然后在main函数中增加

    /**/nh_private.param<bool>("cut_angle", cut_angle, false);if (cut_angle){nh_private.param<int>("left_degrees", left_degrees, 180);nh_private.param<int>("right_degrees", right_degrees, 180);}/**/

就可以实现。

裁剪rplidar的扫描数据相关推荐

Meta宣布将关闭面部识别系统删除超10亿用户面部扫描数据
11月3日消息,据国外媒体报道,当地时间周二,Meta(原名Facebook)宣布,它将在在未来几周关闭已经运行了10年的面部识别系统,并删除超过10亿用户的个人面部扫描数据. 外媒称,Meta做出这 ...
【转】Oracle当中扫描数据的方法
本文将对oracle当中扫描数据的存取方法进行介绍. 1) 全表扫描(Full Table Scans, FTS) 为实现全表扫描,Oracle读取表中所有的行,并检查每一行是否满足语句的WHERE限 ...
人脸扫描建模_一种基于三维扫描数据的人脸建模方法
一种基于三维扫描数据的人脸建模方法黄炎辉 1 , 樊养余 1 , 董卫军 2 [摘要] 三维扫描仪可以准确获取人脸的几何形状与纹理,但原始的人脸扫描数据仅为一张连续曲面,不符合实际的人脸结构,无 ...
如何掌握二维码的扫描数据？
产品推广.获取用户画像.品牌宣传.市场营销.获客引流等,每一个环节都需要真实精准的数据支持.众所周知,随着流量竞争愈加激烈,数据增长成为衡量企业进步指标.社交达人影响力的最直观体现. 大家为获取真实有 ...
python监听扫码枪扫描数据
背景:当扫码枪扫描达到输入的数量N时,自动调用打印机打印N个数据的二维码. 实现:因为需要一直监听扫码枪扫描数据的状态,因此创建线程.但是在调试的时候发现,扫码枪扫描的数据并不是一次性出来的,而是一个 ...
SOLIDWORKS 3D CAD 2019新增功能详解（一）：利用网格和扫描数据的新设计功能
三维设计SolidWorks 3D CAD软件除了设计.模拟.成本估算.可制造性检查.CAM.可持续设计和数据管理等多种功能以外,还包含适用于钣金,焊件,曲面,模具,产品配置,DFM和CAM的专业工具 ...
oracle全表扫过程讲解,CSS_浅谈存取Oracle当中扫描数据的方法，1) 全表扫描(Full Table Scans, FTS) - phpStudy...
浅谈存取Oracle当中扫描数据的方法 1) 全表扫描(Full Table Scans, FTS) 为实现全表扫描,Oracle读取表中所有的行,并检查每一行是否满足语句的WHERE限制条件一个多块 ...
Android 无 EditText 情况下接受扫码枪扫描数据
2019年04月12日更新,根据评论区反馈,可能不是很好用(但是我当时用的时候就是这么实现的),可以选择性尝试 Android 无 EditText 情况下接受扫码枪扫描数据简单无脑! 去下载个 J ...
GEE|在GEE对Sentinel-2、Landsat7、Landsat8进行批量大气校正、去云，并进行均值融合、裁剪、显示和数据下载
前言: 由于我要使用S2在2016-2020年时序数据进行作物分类,但GEE仅提供2018年后的S2 L2A数据('COPERNICUS/S2_SR'),因此需要进行大气校正,同时由于需要的数据量大, ...

裁剪rplidar的扫描数据

裁剪rplidar的扫描数据相关推荐

最新文章

热门文章