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<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);
}将雷达设置成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;
}

发送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 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，其余由于强度或其他原因导致的数据错误会输出。

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