在ROS上发布传感器流

传感器流

传感器流信息对于导航栈来说是必要的，如果缺少，机器人就是盲目导航，撞上障碍物。有许多的传感器可以给导航栈提供信息：激光扫描仪、摄像头、声呐传感器、红外线传感器以及撞击传感器等等。但是目前的导航栈仅仅支持传感器发布的sensor_msgs/LaserScan或者sensor_msgs/PointCloud消息类型。

ROS消息头(Header)

sensor_msgs/LaserScan和sensor_msgs/PointCloud消息类型，和其他任何发布至ROS的消息一样，包含着tf frame和time相关的信息。为了标准化这些信息的发布，这些消息类型中都包含Header消息类型字段。

Header消息类型有3个字段。seq字段对应于标识符，每发布一次消息，它便自动增加。stamp字段存储着和消息有关的时间信息。例如，对于激光扫描仪，它对应的是每次扫描的时间。frame_id字段存储和消息有关的tf frame信息。对于激光扫描仪，应当设为激光扫描仪所在的坐标系。具体描述如下：

#Standard metadata for higher-level flow data types#sequence ID: consecutively increasing ID uint32 seq#Two-integer timestamp that is expressed as:# * stamp.secs: seconds (stamp_secs) since epoch# * stamp.nsecs: nanoseconds since stamp_secs# time-handling sugar is provided by the client librarytime stamp#Frame this data is associated with# 0: no frame# 1: global framestring frame_id

在ROS上发布LaserScans

LaserScan消息

ROS在sensor_msgs包中提供了一个特殊的消息类型，叫LaserScan，用来存储扫描信息。LaserScan消息类型让我们非常方便的存储几乎任何一种激光扫描仪扫描的信息。下面是LaserScan消息类型的规格说明：

# Laser scans angles are measured counter clockwise, with 0 facing forward# (along the x-axis) of the device frame#Header headerfloat32 angle_min        # start angle of the scan [rad]float32 angle_max        # end angle of the scan [rad]float32 angle_increment  # angular distance between measurements [rad]float32 time_increment   # time between measurements [seconds]float32 scan_time        # time between scans [seconds]float32 range_min        # minimum range value [m]float32 range_max        # maximum range value [m]float32[] ranges         # range data [m] (Note: values < range_min or > range_max should be discarded)float32[] intensities    # intensity data [device-specific units]

编码实现发布LaserScan消息

创建源码包

编码前需要创建一个实现发布激光扫描数据的功能包：

cd %TOP_DIR_YOUR_CATKIN_WS%/srccatkin_create_pkg robot_laser_scan_publisher roscpp tf sensor_msgs

添加cpp文件

进入刚创建的robot_laser_scan_publisher包下的src目录，创建laser_scan_publisher.cpp文件，将下面的代码粘贴进去：

#include <ros/ros.h>#include <sensor_msgs/LaserScan.h>int main(int argc, char** argv){  ros::init(argc, argv, "laser_scan_publisher");  ros::NodeHandle n;  ros::Publisher scan_pub = n.advertise<sensor_msgs::LaserScan>("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();  }}

我们将其分解，一步一步来看。

   2 #include <sensor_msgs/LaserScan.h>

这里，我们包含sensor_msgs/LaserScan消息。

   8   ros::Publisher scan_pub = n.advertise<sensor_msgs::LaserScan>("scan", 50);

这行代码创建了一个ros::Publisher对象，之后用来发布LaserScan消息。

  10   unsigned int num_readings = 100;  11   double laser_frequency = 40;  12   double ranges[num_readings];  13   double intensities[num_readings];

这里，我们仅仅模拟用来填充scan的数据。一个实际的应用应该从laser传感器的驱动程序中获取。

  18     //generate some fake data for our laser scan  19     for(unsigned int i = 0; i < num_readings; ++i){  20       ranges[i] = count;  21       intensities[i] = 100 + count;  22     }  23     ros::Time scan_time = ros::Time::now();

填充模拟的laser数据，它的值每秒递增。

  25     //populate the LaserScan message  26     sensor_msgs::LaserScan scan;  27     scan.header.stamp = scan_time;  28     scan.header.frame_id = "laser_frame";  29     scan.angle_min = -1.57;  30     scan.angle_max = 1.57;  31     scan.angle_increment = 3.14 / num_readings;  32     scan.time_increment = (1 / laser_frequency) / (num_readings);  33     scan.range_min = 0.0;  34     scan.range_max = 100.0;  35   36     scan.ranges.resize(num_readings);  37     scan.intensities.resize(num_readings);  38     for(unsigned int i = 0; i < num_readings; ++i){  39       scan.ranges[i] = ranges[i];  40       scan.intensities[i] = intensities[i];  41     }

创建一个sensor_msgs::LaserScan消息，并用之前准备好的模拟数据填入。

  43     scan_pub.publish(scan);

将模拟的LaserScan消息发布至ROS。

构建代码

我们最终想让上面所写的功能包在ROS上以节点的形式存在，需要再添加一些额外的东西。打开robot_laser_scan_publisher包下的CMakeList.txt文件，在文件末尾添加下面的代码：

add_executable(laser_scan_publisher src/laser_scan_publisher.cpp)target_link_libraries(laser_scan_publisher ${catkin_LIBRARIES})

构建节点

上面的工作完成之后，剩下的就是使用catkin_make命令编译构建节点了：

cd %TOP_DIR_YOUR_CATKIN_WS%catkin_make

运行和测试

• 开启第一个终端，启动roscore

• 开启第二个终端，运行robot_laser_scan_publisher软件包

rosrun robot_laser_scan_publisher laser_scan_publisher

• 开启第三个终端，查看当前运行的节点

rosnode list

在第三个终端应该会显示下面两个节点，

/laser_scan_publisher/rosout

还可以进一步查看ROS当前存在的话题：

rostopic list

应该会显示下面的几个话题：

/rosout/rosout_agg/scan

话题/scan就是节点/laser_scan_publisher发布的话题。查看话题的内容：

rostopic echo /scan

会看见类似如下结果：

我们会发现，图片中的内容和代码中的内容相合。

参考网站： http://wiki.ros.org/navigation/Tutorials/RobotSetup/Sensors