问题描述
实验平台：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查看，这里为了方便，我直接摘抄这个结构体的描述文档到了下面
std_msgs/Header header
float32 angle_min
float32 angle_max
float32 angle_increment
float32 time_increment
float32 scan_time
float32 range_min
float32 range_max
float32[] ranges
float32[] intensities
其中ranges是一个浮点数组，每个数组成员代表着激光传感器返回的距离，这个距离有可能是inf，因为激光传感器可能会因为没有探测到物体而返回无限远。这个我们需要在后面留意一下。intensities也是一个浮点数组，用来保存对应ranges上的强度。
所有代码如下：
#!/usr/bin/env python
import cv2
import numpy as np
import time
import rospy
from sensor_msgs.msg import LaserScan
import os
def scan_callback(data):
distances = data.ranges
angle_min = data.angle_min
angle_increment = data.angle_increment
pixels_relative_center = []
for index in range(len(distances)):
distance = distances[index]
if distance == float('inf'):
continue
angle = angle_min + angle_increment * index
x = np.cos(angle) * distance
y = np.sin(angle) * distance
pixel_x = int(round(x * 30))
pixel_y = int(round(y * 30))
pixels_relative_center.append((pixel_x, pixel_y))
pixels_relative_center = list(set(pixels_relative_center))
# create rgb image
rgb_image = np.zeros((801, 801, 3), np.uint8)
rgb_image.fill(255)
pixels_relative_origin = []
for (pixel_x, pixel_y) in pixels_relative_center:
col = 400 + pixel_x
row = 400 - pixel_y
pixels_relative_origin.append((row, col))
# draw the point
for (row, col) in pixels_relative_origin:
cv2.circle(rgb_image, (col, row), 2, (0, 0, 0), -1)
# draw the point of robot
cv2.circle(rgb_image, (400, 400), 4, (0, 255, 0), -1)
cv2.imshow("rgb", rgb_image)
cv2.waitKey(30)
def shutdown():
cv2.destroyAllWindows()
# noinspection PyProtectedMember
os._exit(0)
rospy.on_shutdown(shutdown)
rospy.init_node('laser_image', anonymous=True)
rospy.Subscriber("/scan", LaserScan, scan_callback, queue_size=5)
while True:
time.sleep(1)
可以持续移动机器人，将会发现数据图随着机器人的变化而变化。
代码讲解
使用循环，每次步进angle_increment 的角度，然后使用了三角函数公式来计算出激光数据点相对于机器人中心点（准确说应该是/base_laser_link，但是这里为了简便就说是中心点了）的坐标x和y。
x = np.cos(angle) * distance
y = np.sin(angle) * distance
为了能够将x，y坐标转换为像素的个数（方便显示），我将他们整体扩大了30倍。实际情况中如果有map，应该结合map的分辨率来计算这个值
pixel_x = int(round(x * 30))
pixel_y = int(round(y * 30))
然后，我们新建了一个801×801的空白画布。这个画布大小是我自己定义的，足够容纳我实验环境下激光传感器返回的所有点。pixels_relative_center 数组保存的是相对于机器人中心点，激光数据点的像素坐标。在后面的循环中，我们将这些数据点转换为相对于画布原点的像素坐标，并将他们放在pixels_relative_origin 数组中。这里需要注意的是，opencv画布坐标轴的位置和方向是不一样的。400就是我们的画布中心点，所以转换方式如下
col = 400 + pixel_x
row = 400 - pixel_y
最后，我们将这些点画出来即可
cv2.circle(rgb_image, (col, row), 2, (0, 0, 0), -1)
