前一篇文章《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 Hokuyostruct 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 scansfloat 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 Hokuyostruct LaserScan{//! Array of rangesstd::vector<float> ranges; //储存距离数据的数组//! Array of intensitiesstd::vector<float> intensities;  //! Self reported time stamp in nanosecondsuint64_t self_time_stamp;//! System time when first range was measured in nanosecondsuint64_t system_time_stamp;//! Configuration of scanLaserConfig 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);
}将雷达设置成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 failtry{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.
}

int
hokuyo::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了：

读取数据的方法：

void
hokuyo::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 donereturn;if (!checkSum(&buf[ind], bytes))HOKUYO_EXCEPT(hokuyo::CorruptedDataException, "Checksum failed on data read.");bytes += ind - 2;// Read as many ranges as we can getif(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 errorsscan.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 22mscan.ranges.push_back(std::numeric_limits<float>::infinity());break;case 1: // Reflected light has low intensityscan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());break;case 2: // Reflected light has low intensityscan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());break;case 6: // Detected object is possibly at 5.7mscan.ranges.push_back(std::numeric_limits<float>::infinity());break;case 7: // Distance data on the preceding and succeeding steps have errorsscan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());break;case 8: // Intensity difference of two wavesscan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());break;case 9: // The same step had error in the last two scanscan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());break;case 10: // Othersscan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());break;case 11: // Othersscan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());break;case 12: // Othersscan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());break;case 13: // Othersscan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());break;case 14: // Othersscan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());break;case 15: // Othersscan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());break;case 16: // Othersscan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());break;case 17: // Othersscan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());break;case 18: // Error reading due to strong reflective objectscan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());break;case 19: // Non-Measurable stepscan.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 loopind = 0;for (int j = bytes - (bytes % data_size); j < bytes ; j++)buf[ind++] = buf[j];}
}

我用的是URG-04LX的,so...19个case

总结

总结一下，距离超过测量范围时候的两种情况是INF，其余由于强度或其他原因导致的数据错误会输出。

目的达到！写完收工去吃饭。

hokuyo_node代码分析相关推荐

20145236《网络攻防》Exp4 恶意代码分析
20145236<网络攻防>Exp4 恶意代码分析一.基础问题回答如果在工作中怀疑一台主机上有恶意代码,但只是猜想,所有想监控下系统一天天的到底在干些什么.请设计下你想监控的操作有哪些 ...
C#中类的继承 override virtual new的作用以及代码分析
继承中override virtual new的作用 virtual 父类中需要注明允许重写的方法: override 子类中必须显示声明该方法是重写的父类中的方法: new 子类中忽略父类的已存在的 ...
2017.4.18 静态代码分析工具sonarqube+sonar-runner的安装配置及使用
配置成功后的代码分析页面: 可以看到对复杂度.语法使用.重复度等等都做了分析,具体到了每一个方法和每一句代码. 四种使用方式: sonarqube + sonar-runner sonarqube + ...
lighttpd1.4.18代码分析
lighttpd1.4.18代码分析(八)--状态机(2)CON_STATE_READ状态 posted @ 2008-09-24 10:50 那谁阅读(2225) | 评论 (1) 编辑 lig ...
Device Tree（三）：代码分析
2019独角兽企业重金招聘Python工程师标准>>> 一.前言 Device Tree总共有三篇,分别是: 1.为何要引入Device Tree,这个机制是用来解决什么问题的?(请 ...
使用Hadoop和ELK进行业务代码分析！分分钟捉到Bug!
大数据是计算领域的新高地,它有望提供一种方法来应对二十一世纪不断增长的数据生成.越来越多的大数据爱好者正在涌现,越来越多的公司正在采用各种大数据平台,并希望提出以客户为中心的解决方案,帮助他们在竞争激 ...
20145328 《网络对抗技术》恶意代码分析
20145328 <网络对抗技术>恶意代码分析 ------看到这句话说明还没写完-------- 实践内容: 使用schtasks指令监控系统运行使用sysmon工具监控系统运行使用 ...
starGAN原理代码分析
下载: git clone https://github.com/yunjey/StarGAN.git 1 cd StarGAN/ 1 下载celebA训练数据: bash download.sh 1 ...
tensorflow笔记：多层CNN代码分析
tensorflow笔记系列: (一) tensorflow笔记:流程,概念和简单代码注释 (二) tensorflow笔记:多层CNN代码分析 (三) tensorflow笔记:多层LSTM代 ...
ARM裸机篇---启动代码分析
ARM裸机篇---启动代码分析先搞清楚启动代码和Bootloader的区别,启动代码是指CPU复位后到进入C语言的main函数之前需要执行的那段汇编代码. 下面的代码先暂且这样吧,没啥注释的,时间关 ...

hokuyo_node代码分析