裁剪rplidar的扫描数据
来源:互联网 发布:淘宝水印在线制作 编辑:程序博客网 时间:2024/06/08 00:03
对于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]; } }}
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(); else scan_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(); else scan_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); } /**/就可以实现。
阅读全文
0 0
- 裁剪rplidar的扫描数据
- SLAMTEC rplidar a1的使用
- MRPT关于rplidar激光雷达的错误
- MRPT关于rplidar激光雷达的错误
- ubuntu下rplidar转速慢的问题
- Arcengine+C#实现矢量数据的裁剪
- Oracle 扫描数据的方法
- oracle的数据扫描方式
- 自定义各种裁剪框、扫描框
- rplidar A2 在机器人上的安装说明
- rplidar使用hector_slam的hector_mapping geotiff创建地图方法
- rplidar使用hector_slam的hector_mapping geotiff创建地图方法(2)
- Tk1上使用rpLidar,并解决找不到 ttyUSB0的问题!
- 存取Oracle当中扫描数据的方法
- Qt获取激光扫描枪的数据
- 扫描枪的数据获取问题
- 条码扫描枪的数据接收处理
- yuv420数据快速裁剪
- 付款操作
- 必知的6条PCB设计原则
- 复习C
- SharedPreferences的工具类
- 修改Ubuntu16.04出现的ifconfig显示问题
- 裁剪rplidar的扫描数据
- 三个问题
- 数字识别
- A literature review and classification of recommender systems research
- ldm/stm与栈的处理
- hdu 5938 Four Operations
- 最符合这个时代的读书方式,每天只需要一块钱
- 资本大鳄彼得•蒂尔与同性男友完婚了,他是如何不走寻常路?
- vim 常用指令及其配制