hokuyo_node代码分析
来源:互联网 发布:php curl 存储cookie 编辑:程序博客网 时间:2024/06/06 05:39
前一篇文章《sensor_msgs/LaserScan Message》说道hokuyo_node包发布的数据有inf和nan这两种数据,由于李太白同学需要用这些距离数据进行相关研究,但是不知道inf和nan这两种数据是在什么情况下得出来的,百度了下发现没有hokuyo_node包的代码分析,只有怎么应用这个包。好吧,看来又到了写博客的时间了。既然没有,自己写个吧。
1 hokuyo URG04-LX 简介
URG-04LX是用于区域扫描的激光传感器。传感器的光源是波长为785nm的红外激光器,具有激光等级1的安全性。扫描区域为240度,最大半径4000mm(683步)。俯仰角为0.36o。激光束直径在2000mm时小于20mm,在4000mm时最大发散角为40mm。
距离测量的原理是基于相位差的计算,由此可以在物体的颜色和反射率的影响最小的情况下获得稳定的测量。
1.1 产品规格:
这个精度可以看出不是很好,但还是要7000~1w左右。。。小于1m精度10mm,1~4m就是距离的百分之一了。。。
1.2 参考质量
上面的图来自hokuyo urg-04lx雷达的说明文档,文档里有对方向进行了说明:
备注
2 hokuyo_node package 代码分析
2.1 hokuyo.h
hokuyo.h头文件中定义了如下的数据结构体:
//! A struct for returning configuration from the Hokuyo struct LaserConfig { //! Start angle for the laser scan [rad]. 0 is forward and angles are measured clockwise when viewing hokuyo from the top. float min_angle; // 扫描的起始角度位置,如-2.07, //! Stop angle for the laser scan [rad]. 0 is forward and angles are measured clockwise when viewing hokuyo from the top. float max_angle; //! Scan resolution [rad]. float ang_increment; //! Scan resoltuion [s] float time_increment; //! Time between scans float scan_time; //! Minimum range [m] float min_range; //! Maximum range [m] float max_range; //! Range Resolution [m] float range_res; };... //! A struct for returning laser readings from the Hokuyo struct LaserScan { //! Array of ranges std::vector<float> ranges; //储存距离数据的数组 //! Array of intensities std::vector<float> intensities; //! Self reported time stamp in nanoseconds uint64_t self_time_stamp; //! System time when first range was measured in nanoseconds uint64_t system_time_stamp; //! Configuration of scan LaserConfig config; };
2.2 hokuyo_node.cpp
将scan的数据转成ROS中的数据格式:
int publishScan(const hokuyo::LaserScan &scan) { //ROS_DEBUG("publishScan"); scan_msg_.angle_min = scan.config.min_angle; scan_msg_.angle_max = scan.config.max_angle; scan_msg_.angle_increment = scan.config.ang_increment; scan_msg_.time_increment = scan.config.time_increment; scan_msg_.scan_time = scan.config.scan_time; scan_msg_.range_min = scan.config.min_range; scan_msg_.range_max = scan.config.max_range; scan_msg_.ranges = scan.ranges; scan_msg_.intensities = scan.intensities; scan_msg_.header.stamp = ros::Time().fromNSec((uint64_t)scan.system_time_stamp) + ros::Duration(driver_.config_.time_offset); scan_msg_.header.frame_id = driver_.config_.frame_id; desired_freq_ = (1. / scan.config.scan_time); scan_pub_.publish(scan_msg_); //ROS_DEBUG("publishScan done"); return(0); }
这个文件主要负责ROS的格式化与通讯,做了一些配置上的检查(如 如果参数服务器中设置的最小角度小于该雷达的最小角度,程序对将雷达的最小角度复制给该参数)。
2.3 hokuyo.cpp
这个文件首先定义了通讯的一些必要方法。如检查端口打开了没,如果打开了就关上端口。典型的串口通讯规则。
之后定义了一些功能方法,如
void //将雷达设置成SCIP2模式的方法hokuyo::Laser::setToSCIP2(){if (!portOpen()) HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");const char * cmd = "SCIP2.0";char buf[100];laserWrite(cmd); laserWrite("\n");laserReadline(buf, 100, 1000);ROS_DEBUG("Laser comm protocol changed to %s \n", buf);//printf ("Laser comm protocol changed to %s \n", buf);}
///////////////////////////////////////////////////////////////////////////////void hokuyo::Laser::reset () //向雷达发送TM2,使雷达复位{if (!portOpen()) HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); laserFlush(); try { sendCmd("TM2", 1000); } catch (hokuyo::Exception &e) {} // Ignore. If the laser was scanning TM2 would fail try { sendCmd("RS", 1000); last_time_ = 0; // RS resets the hokuyo clock. wrapped_ = 0; // RS resets the hokuyo clock. } catch (hokuyo::Exception &e) {} // Ignore. If the command coincided with a scan we might get garbage. laserFlush(); sendCmd("RS", 1000); // This one should just work.}
inthokuyo::Laser::laserOn() { int res = sendCmd("BM",1000); if (res == 1) HOKUYO_EXCEPT(hokuyo::Exception, "Unable to control laser due to malfunction."); return res;}发送BM,打开雷达。(此处我有个疑问,每次我将雷达通电的时候会听到雷达的电机在转的声音,不知道这个打开是不是打开激光的开关,哪位学者可以告知的话李某不胜感激!)
经过了这么多东西,终于找到了我要找的INF和nan了:
读取数据的方法:
voidhokuyo::Laser::readData(hokuyo::LaserScan& scan, bool has_intensity, int timeout){ scan.ranges.clear(); scan.intensities.clear(); int data_size = 3; if (has_intensity) data_size = 6; char buf[100]; int ind = 0; scan.self_time_stamp = readTime(timeout); int bytes; int range; float intensity; for (;;) { bytes = laserReadline(&buf[ind], 100 - ind, timeout); if (bytes == 1) // This is \n\n so we should be done return; if (!checkSum(&buf[ind], bytes)) HOKUYO_EXCEPT(hokuyo::CorruptedDataException, "Checksum failed on data read."); bytes += ind - 2; // Read as many ranges as we can get if(dmax_ > 20000){ // Check error codes for the UTM 30LX (it is the only one with the long max range and has different error codes) for (int j = 0; j < bytes - (bytes % data_size); j+=data_size) {if (scan.ranges.size() < MAX_READINGS){ range = (((buf[j]-0x30) << 12) | ((buf[j+1]-0x30) << 6) | (buf[j+2]-0x30)); switch (range) // See the SCIP2.0 reference on page 12, Table 4 { case 1: // No Object in Range scan.ranges.push_back(std::numeric_limits<float>::infinity()); break; case 2: // Object is too near (Internal Error) scan.ranges.push_back(-std::numeric_limits<float>::infinity()); break; case 3: // Measurement Error (May be due to interference) scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 4: // Object out of range (at the near end) ///< @todo, Should this be an Infinity Instead? scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 5: // Other errors scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; default: scan.ranges.push_back(((float)range)/1000.0); } if (has_intensity) { intensity = (((buf[j+3]-0x30) << 12) | ((buf[j+4]-0x30) << 6) | (buf[j+5]-0x30)); scan.intensities.push_back(intensity); }}else{ HOKUYO_EXCEPT(hokuyo::CorruptedDataException, "Got more readings than expected");} } } else { // Check error codes for all other lasers (URG-04LX UBG-04LX-F01 UHG-08LX) for (int j = 0; j < bytes - (bytes % data_size); j+=data_size) {if (scan.ranges.size() < MAX_READINGS){ range = (((buf[j]-0x30) << 12) | ((buf[j+1]-0x30) << 6) | (buf[j+2]-0x30)); switch (range) // See the SCIP2.0 reference on page 12, Table 3 { case 0: // Detected object is possibly at 22m scan.ranges.push_back(std::numeric_limits<float>::infinity()); break; case 1: // Reflected light has low intensity scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 2: // Reflected light has low intensity scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 6: // Detected object is possibly at 5.7m scan.ranges.push_back(std::numeric_limits<float>::infinity()); break; case 7: // Distance data on the preceding and succeeding steps have errors scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 8: // Intensity difference of two waves scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 9: // The same step had error in the last two scan scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 10: // Others scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 11: // Others scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 12: // Others scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 13: // Others scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 14: // Others scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 15: // Others scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 16: // Others scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 17: // Others scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 18: // Error reading due to strong reflective object scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; case 19: // Non-Measurable step scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN()); break; default: scan.ranges.push_back(((float)range)/1000.0); } if (has_intensity) { intensity = (((buf[j+3]-0x30) << 12) | ((buf[j+4]-0x30) << 6) | (buf[j+5]-0x30)); scan.intensities.push_back(intensity); }}else{ HOKUYO_EXCEPT(hokuyo::CorruptedDataException, "Got more readings than expected");} } } // Shuffle remaining bytes to front of buffer to get them on the next loop ind = 0; for (int j = bytes - (bytes % data_size); j < bytes ; j++) buf[ind++] = buf[j]; }}我用的是URG-04LX的,so...19个case
总结
总结一下,距离超过测量范围时候的两种情况是INF,其余由于强度或其他原因导致的数据错误会输出。
目的达到!写完收工去吃饭。
- hokuyo_node代码分析
- ROS中Hokuyo_node使用
- 分析代码
- 代码分析
- 分析代码
- 代码分析
- 分析代码
- 代码分析
- 代码分析
- 代码分析
- butterknife源码分析:代码分析
- Bittorrent 精彩代码分析
- Duwamish代码分析篇
- 井字棋游戏 代码分析
- VC6启动代码分析
- Duwamish代码分析篇
- 定时器代码分析
- Duwamish -- 代码分析篇
- 华为Eudemon1000防火墙-详细配置
- 配置监听(系统启动和关闭时运行的程序)
- JS 页面加载触发事件 document.ready和window.onload的区别
- 习题7.6
- js数组实现队列和栈
- hokuyo_node代码分析
- Promise异步编程
- Android截屏的几种实现
- window上gcc安装
- 一个例子
- XSS的原理
- Linux环境部署轻量化配置中心
- 台大李宏毅2017机器学习国语课程(更新)
- USG防火墙 配置域内NAT+NAT Server双出口