ROS探索总结(二十)——发布导航需要的传感器信息

25259
18
2016年2月28日 21时30分

      在导航过程中,传感器的信息至关重要,这些传感器可以是激光雷达、摄像机、声纳、红外线、碰撞开关,但是归根结底,导航功能包要求机器人必须发布sensor_msgs/LaserScansensor_msgs/PointCloud格式的传感器信息,本篇将详细介绍如何使用代码发布所需要的消息。

1ROS的消息头信息

    无论是 sensor_msgs/LaserScan还是sensor_msgs/PointCloud ,都和ROStf帧信息等时间相关的消息一样,带标准格式的头信息。

#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 library
time stamp

#Frame this data is associated with
# 0: no frame
# 1: global frame
string frame_id

       以上是标准头信息的主要部分。seq是消息的顺序标识,不需要手动设置,发布节点在发布消息时,会自动累加。stamp 是消息中与数据相关联的时间戳,例如激光数据中,时间戳对应激光数据的采集时间点。frame_id 是消息中与数据相关联的参考系id,例如在在激光数据中,frame_id对应激光数据采集的参考系

2、如何发布激光扫描消息

2.1、激光消息的结构

      针对激光雷达,ROSsensor_msgs 包中定义了专用了数据结构来存储激光消息的相关信息,成为LaserScanLaserScan消息的格式化定义,为虚拟的激光雷达数据采集提供了方便,在我们讨论如何使用他之前,来看看该消息的结构是什么样的:

#
# Laser scans angles are measured counter clockwise, with 0 facing forward
# (along the x-axis) of the device frame
#

Header header
float32 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]

      备注中已经详细介绍了每个参数的意义。

2.2、通过代码发布LaserScan消息

      使用ROS发布LaserScan格式的激光消息非常简洁,下边是一个简单的例程:

#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();
  }
}

      我们将代码分解以便于分析:

#include <sensor_msgs/LaserScan.h>

      首先我们需要先包含Laserscan的数据结构。

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

      创建一个发布者,以便于后边发布针对scan主题的Laserscan消息。

  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_msgs::LaserScan数据类型的变量scan,把我们之前伪造的数据填入格式化的消息结构中。

   scan_pub.publish(scan);

    数据填充完毕后,通过前边定义的发布者,将数据发布。

3、如何发布点云数据

3.1、点云消息的结构

      为存储和发布点云消息,ROS定义了sensor_msgs/PointCloud消息结构:

#This message holds a collection of 3d points, plus optional additional information about each point.

#Each Point32 should be interpreted as a 3d point in the frame given in the header

 

Header header

geometry_msgs/Point32[] points  #Array of 3d points

ChannelFloat32[] channels       #Each channel should have the same number of elements as points array, and the data in each channel should correspond 1:1 with each point

      如上所示,点云消息的结构支持存储三维环境的点阵列,而且channels参数中,可以设置这些点云相关的数据,例如可以设置一个强度通道,存储每个点的数据强度,还可以设置一个系数通道,存储每个点的反射系数,等等。

3.2、通过代码发布点云数据

     ROS发布点云数据同样简洁:

#include <ros/ros.h>
#include <sensor_msgs/PointCloud.h>

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

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

  unsigned int num_points = 100;

  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);

    //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;
    }

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

     分解代码来分析:

#include <sensor_msgs/PointCloud.h>

     首先也是要包含sensor_msgs/PointCloud消息结构。

    ros::Publisher cloud_pub = n.advertise<sensor_msgs::PointCloud>("cloud", 50);

     定义一个发布点云消息的发布者。

    sensor_msgs::PointCloud cloud;
    cloud.header.stamp = ros::Time::now();
    cloud.header.frame_id = "sensor_frame";

     为点云消息填充头信息,包括时间戳和相关的参考系id

    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;
    }

        将我们伪造的数据填充到点云消息结构当中。

   cloud_pub.publish(cloud);

       最后,发布点云数据。

 

参考连接: http://wiki.ros.org/navigation/Tutorials/RobotSetup/Sensors 

发表评论

后才能评论

评论列表(18条)

  • 李宁辉 2019年10月14日 下午6:05

    老师,你好.我用arduino发布PointCloud或者LaserScan消息类型的数据到话题上,可是有一些问题就是用PointCloud发布的时候,cloud.channels.resize(1)没有定义resize,我就把resize都注释掉了,运行的时候,我看到的话题数据中是
    points: []
    channels: [],用rviz也没有看到数据点.
    我试了一下LaserScan结果出现了设配不匹配的情况
    [ERROR] [1571047457.930214]: Unable to sync with device; possible link problem or link software version mismatch such as hydro rosserial_python with groovy Arduino
    请问您知道我出现了什么问题吗?

  • 杨欣瑞 2019年9月10日 下午4:50

    胡老师你好,最近想搭建一个slam的小车,但是不知道从何入手,不知道小车如何与ros建立联系,您能给点建议嘛?

    • 古月 回复 杨欣瑞 2019年9月12日 下午9:14

      先找一本ROS的书系统的学习下,最好找一个实物先研究,再自己做

  • 小白 2019年4月13日 上午11:25

    老师,您好!我想在小车底盘上加载四个超声波传感器,在网上查着可以在上层加入超声波的层,发布超声波的距离数据,但是四个相同的传感器发布相同的数据要怎么设置相应的topic和tf?

    • 古月 回复 小白 2019年4月15日 下午6:24

      自己写节点发布话题,模型可以放到URDF中

    • 握紧额的手_额要带你走 回复 古月 2020年5月13日 上午9:42

      老师,如果自己写节点发布话题,那四个超声波的数据来源不还是出自同一处吗?好像还没解决相同数据的问题.

  • leo 2019年3月10日 下午10:25

    古月老师,请教您一个问题,关于激光雷达的信息读取ranges,LMS1xx测量范围是270°,分辨率0.25或0.5。 为何用仿真读取到的数据是720个点,这是如何计算的?另外LMS1xx的ranges下标信息应如何解读,分别对应传感器的哪个方向?(关于720个点,我在opt/ros/indigo/share/lms1xx中发现了它的文件,里面写的sample720,resolution1,不明白为何这样写,与官网i信息不符)。求指教!

    • 古月 回复 leo 2019年3月16日 下午9:37

      这个不清楚,请参考该传感器驱动对应的wiki

  • 猫猫 2019年1月19日 下午1:41

    古月大神:
    我虚拟了一个激光数据, 发布频率是10HZ,发布的话题是”scan”,
    move_base costmap_common_params.yaml参数如下
    base_scan: {data_type: LaserScan, topic: /scan, observation_persistence: 0.0, marking: true, clearing: true, expected_update_rate: 0.5},更新的频率为:0.5.
    但是move_base启动起来之后,一直报这个警告:
    The /scan observation buffer has not been updated for 0.86 seconds,
    里面的没有更新的时间最多0.9秒,不知道是怎么回事?
    我的激光数据发布已经够快了,base_scan更新的频率也不太快,是0.5s,,为什么还会出现这个警告那?
    谢谢

  • 大力 2018年12月20日 下午5:08

    胡老师,我在自己写imu的topic发布的时候,遇到协方差矩阵,只知道是用来计算与预期之间的误差,这个对应的资料能科普一下吗?这个会对整体imu的精度有影响吗?

    • 古月 回复 大力 2018年12月20日 下午7:52

      协方差主要用在数据融合里,比如卡尔曼滤波,具体理论请网上学习

    • 大力 回复 古月 2018年12月21日 上午9:01

      好的。谢谢胡老师

  • 小白 2018年12月14日 下午3:22

    老师,您好!我采用的是razor_9dof_imu的惯性传感器,传感器通过串口与电脑相连,我想问一下,小车的位置、走直矫正是通过把imu的yaw轴数据传给stm32来控制?还是直接在电脑端通过imu、编码器、激光雷达等直接控制小车的位置、走直矫正?

    • 古月 回复 小白 2018年12月14日 下午9:04

      两种都可以做到,一般实时要求比较高的可能会用前者

  • 桃李荫下蹊 2016年5月1日 下午10:25

    请问大神:1.“机器人速度指令的格式为:x方向速度、y方向速度、速度向量角度” 是什么意思呢,不知道我理解的–机器人坐标系下的x方向速度,y方向速度,和机器人坐标系相对世界坐标系夹角的变化量–这样对吗?
    2.“导航功能包仅对差分等轮式机器人有效” 不知道我们用的三轮全向机器人(三个独立万向轮互成120度角分布)可以用吗?

    麻烦大神了

    • 古月 回复 桃李荫下蹊 2016年5月2日 下午8:24

      1. 角度就是x方向与y方向之间的夹角
      2.导航功能包主要针对外形为圆形或正方形的机器人建模,看你们的机器人模型是否符合要求。
      可以参考:http://wiki.ros.org/navigation

  • dajianli 2016年3月2日 下午2:14

    古月大神,联系不到你,,我您的文章转载了,www.rosclub.cn,牛人业话栏目,顺带能否添加友链呢