Navigation导航包的学习实践-（二）传感器数据流

在ROS上发布传感器数据流

• 参考ROS_WIKI

---

1. 发布LaserScans-模拟激光数据

目前导航功能包集只接受使用sensor_msgs/LaserScan或sensor_msgs/PointCloud消息类型发布的传感器数据
在开始模拟发送激光数据钱，需要首先知道两种消息类型：

• Header - 该消息类型被用于包含有时间戳的消息，例如sensor_msgs/LaserScan和 sensor_msgs/PointCloud。消息定义如下：

uint32 seq //对应一个标识符，随着消息被发布，它会自动增加

time stamp //时间戳，以激光扫描为例，stamp可能对应每次扫描开始的时间

string frame_id //以激光扫描为例，它将是激光数据所在帧（坐标系）

• LaserScan - 存储激光信息，激光扫描仪获取的数据可以格式化为这种消息类型。消息定义如下：

Header header
float32 angle_min        # scan的开始角度 [弧度]
float32 angle_max        # scan的结束角度 [弧度]
float32 angle_increment  # 测量的角度间的距离 [弧度]
float32 time_increment   # 测量间的时间 [秒]
float32 scan_time        # 扫描间的时间 [秒]
float32 range_min        # 最小的测量距离 [米]
float32 range_max        # 最大的测量距离 [米]
float32[] ranges         # 测量的距离数据 [米] (注意: 值 < range_min 或 > range_max 应当被丢弃)
float32[] intensities    # 强度数据 [device-specific units]

下面开始编写代码模拟激光雷达发布消息。

#include 
#include 

int main(int argc, char** argv){
  ros::init(argc, argv, "laser_scan_publisher");

  ros::NodeHandle n;
  ros::Publisher scan_pub = n.advertise("scan", 50);

  unsigned int num_readings = 100;
  double laser_frequency = 40;
  double ranges[num_readings];
  double intensities[num_readings];

  int count = 0;
  ros::Rate r(1.0);
  while(n.ok()){
    //generate some fake data for our laser scan
    for(unsigned int i = 0; i < num_readings; ++i){
      ranges[i] = count;
      intensities[i] = 100 + count;
    }
    ros::Time scan_time = ros::Time::now();

    //populate the LaserScan message
    sensor_msgs::LaserScan scan;
    scan.header.stamp = scan_time;
    scan.header.frame_id = "laser_frame";
    scan.angle_min = -1.57;
    scan.angle_max = 1.57;
    scan.angle_increment = 3.14 / num_readings;
    scan.time_increment = (1 / laser_frequency) / (num_readings);
    scan.range_min = 0.0;
    scan.range_max = 100.0;

    scan.ranges.resize(num_readings);
    scan.intensities.resize(num_readings);
    for(unsigned int i = 0; i < num_readings; ++i){
      scan.ranges[i] = ranges[i];
      scan.intensities[i] = intensities[i];
    }

    scan_pub.publish(scan);
    ++count;
    r.sleep();
  }
}

这里就是用随机数填充LaserScan消息，其中scan.ranges[]与scan.intensities[]是激光雷达填充的，这里用随机数填充，填充值每秒钟加1。

ros::Rate r(1.0);设置了定时器延时1s，r.sleep();进行等待。

2. 发布点云 PointClouds

需要用到消息sensor_msgs/PointCloud，首先看一下其定义：

Header header
geometry_msgs/Point32[] points  #三维点的坐标数组
ChannelFloat32[] channels       #每个通道中的元素值的个数必须与点云数组元素个数相同

下面编写代码发布点云消息：

#include 
#include 

int main(int argc, char** argv){
  ros::init(argc, argv, "point_cloud_publisher");

  ros::NodeHandle n;
  ros::Publisher cloud_pub = n.advertise("cloud", 50);

  unsigned int num_points = 100;
2. 
  int count = 0;
  ros::Rate r(1.0);
  while(n.ok()){
    sensor_msgs::PointCloud cloud;
    cloud.header.stamp = ros::Time::now();
    cloud.header.frame_id = "sensor_frame";

    cloud.points.resize(num_points);//设置点云的数量

    //we'll also add an intensity channel to the cloud
    cloud.channels.resize(1);
    cloud.channels[0].name = "intensities";
    cloud.channels[0].values.resize(num_points);//增加强度通道 "intensity" ，并设置其大小，使与点云数量相匹配

    //generate some fake data for our point cloud
    for(unsigned int i = 0; i < num_points; ++i){
      cloud.points[i].x = 1 + count;
      cloud.points[i].y = 2 + count;
      cloud.points[i].z = 3 + count;
      cloud.channels[0].values[i] = 100 + count;//使用虚拟数据填充 PointCloud 消息。同时，使用虚拟数据填充 intensity 信道。
    }

    cloud_pub.publish(cloud);
    ++count;
    r.sleep();
  }
}