ROS激光传感器可视化

问题描述

实验平台:TIAGo

编程语言:Python

TIAGo所使用的激光传感器在移动基座的底部,具体的描述信息如下

Laser range-finder

Located at the front of the base. This sensor measures distances in a horizontal plane. It is a valuable asset for navigation and mapping. Bad measurements can be caused by reflective or transparent surfaces.

为了获取激光数据,我们必须订阅 /scan 消息,消息返回的类型为 sensor_msgs/LaserScan ,该类型的文档可以访问http://docs.ros.org/indigo/api/sensor_msgs/html/msg/LaserScan.html查看,这里为了方便,我直接摘抄这个结构体的描述文档到了下面

其中ranges是一个浮点数组,每个数组成员代表着激光传感器返回的距离,这个距离有可能是inf,因为激光传感器可能会因为没有探测到物体而返回无限远。这个我们需要在后面留意一下。intensities也是一个浮点数组,用来保存对应ranges上的强度。

所有代码如下:

可以持续移动机器人,将会发现数据图随着机器人的变化而变化。

代码讲解

使用循环,每次步进 angle_increment 的角度,然后使用了三角函数公式来计算出激光数据点相对于机器人中心点(准确说应该是/base_laser_link,但是这里为了简便就说是中心点了)的坐标x和y。

为了能够将x,y坐标转换为像素的个数(方便显示),我将他们整体扩大了30倍。实际情况中如果有map,应该结合map的分辨率来计算这个值

然后,我们新建了一个801×801的空白画布。这个画布大小是我自己定义的,足够容纳我实验环境下激光传感器返回的所有点。 pixels_relative_center 数组保存的是相对于机器人中心点,激光数据点的像素坐标。在后面的循环中,我们将这些数据点转换为相对于画布原点的像素坐标,并将他们放在 pixels_relative_origin 数组中。这里需要注意的是,opencv画布坐标轴的位置和方向是不一样的。400就是我们的画布中心点,所以转换方式如下

最后,我们将这些点画出来即可