hokuyo_node代码分析
前一篇文章《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 ranges; //储存距离数据的数组
//! Array of intensities
std::vector 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.
}
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;
}经过了这么多东西，终于找到了我要找的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 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::infinity());
break;
case 2: // Object is too near (Internal Error)
scan.ranges.push_back(-std::numeric_limits::infinity());
break;
case 3: // Measurement Error (May be due to interference)
scan.ranges.push_back(std::numeric_limits::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::quiet_NaN());
break;
case 5: // Other errors
scan.ranges.push_back(std::numeric_limits::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::infinity());
break;
case 1: // Reflected light has low intensity
scan.ranges.push_back(std::numeric_limits::quiet_NaN());
break;
case 2: // Reflected light has low intensity
scan.ranges.push_back(std::numeric_limits::quiet_NaN());
break;
case 6: // Detected object is possibly at 5.7m
scan.ranges.push_back(std::numeric_limits::infinity());
break;
case 7: // Distance data on the preceding and succeeding steps have errors
scan.ranges.push_back(std::numeric_limits::quiet_NaN());
break;
case 8: // Intensity difference of two waves
scan.ranges.push_back(std::numeric_limits::quiet_NaN());
break;
case 9: // The same step had error in the last two scan
scan.ranges.push_back(std::numeric_limits::quiet_NaN());
break;
case 10: // Others
scan.ranges.push_back(std::numeric_limits::quiet_NaN());
break;
case 11: // Others
scan.ranges.push_back(std::numeric_limits::quiet_NaN());
break;
case 12: // Others
scan.ranges.push_back(std::numeric_limits::quiet_NaN());
break;
case 13: // Others
scan.ranges.push_back(std::numeric_limits::quiet_NaN());
break;
case 14: // Others
scan.ranges.push_back(std::numeric_limits::quiet_NaN());
break;
case 15: // Others
scan.ranges.push_back(std::numeric_limits::quiet_NaN());
break;
case 16: // Others
scan.ranges.push_back(std::numeric_limits::quiet_NaN());
break;
case 17: // Others
scan.ranges.push_back(std::numeric_limits::quiet_NaN());
break;
case 18: // Error reading due to strong reflective object
scan.ranges.push_back(std::numeric_limits::quiet_NaN());
break;
case 19: // Non-Measurable step
scan.ranges.push_back(std::numeric_limits::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];
}
}
总结
总结一下，距离超过测量范围时候的两种情况是INF，其余由于强度或其他原因导致的数据错误会输出。
目的达到！写完收工去吃饭。