问题描述

实验平台：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)