rospy 中的实时 2D 激光扫描仪数据

Question

我刚得到一台 Sick Tim 571 激光扫描仪。因为我是 ROS 的新手，所以我想在一个简单的 rospy 实现中测试一些东西。

我认为下面的代码会向我显示激光测量的实时输出，就像在 Rviz 中一样（Rviz 对我来说很好用）- 但在 Python 中并且可以使用测量在我自己的代码中。不幸的是，输出帧一遍又一遍地只显示一个静态测量（从 Python 代码第一次启动时开始）。

如果我运行 Rviz 与此 Python 代码并行，我将获得测量区域的动态更新表示。

我认为 .callback(data) 函数每次都使用一组新的激光扫描仪数据调用。但它似乎并不像我想象的那样有效。所以错误可能位于调用回调函数的.laser_listener()处。

TL;DR

如何在 rospy 中使用动态更新的（实时）激光扫描仪测量值？

import rospy
import cv2
import numpy as np
import math

from sensor_msgs.msg import LaserScan

def callback(data):
    frame = np.zeros((500, 500,3), np.uint8)
    angle = data.angle_min
    for r in data.ranges:
        #change infinite values to 0
        if math.isinf(r) == True:
            r = 0
        #convert angle and radius to cartesian coordinates
        x = math.trunc((r * 30.0)*math.cos(angle + (-90.0*3.1416/180.0)))
        y = math.trunc((r * 30.0)*math.sin(angle + (-90.0*3.1416/180.0)))

        #set the borders (all values outside the defined area should be 0)
        if y > 0 or y < -35 or x<-40 or x>40:
            x=0
            y=0

        cv2.line(frame,(250, 250),(x+250,y+250),(255,0,0),2)
        angle= angle + data.angle_increment 
        cv2.circle(frame, (250, 250), 2, (255, 255, 0))
        cv2.imshow('frame',frame)
        cv2.waitKey(1)

def laser_listener():
    rospy.init_node('laser_listener', anonymous=True)
    rospy.Subscriber("/scan", LaserScan,callback)
    rospy.spin()

if __name__ == '__main__':
    laser_listener()

[EDIT_1]:

当我将 print(data.ranges[405]) 添加到回调函数时，我得到了这个输出。它略有变化。但这是错误的。我在测量过程中覆盖了整个传感器。这些值仍然只适合程序启动时的时间。

1.47800004482
1.48000001907
1.48000001907 
1.48000001907
1.48300004005
1.47899997234
1.48000001907 
1.48099994659
1.47800004482
1.47899997234
1.48300004005
1.47800004482
1.48500001431
1.47599995136
1.47800004482
1.47800004482
1.47399997711
1.48199999332
1.48099994659
1.48000001907
1.48099994659

同上，但反过来。我从一个有盖的传感器开始，并在测量过程中掀开盖子。

0.0649999976158
0.0509999990463
0.0529999993742
0.0540000014007
0.0560000017285
0.0579999983311
0.0540000014007
0.0579999983311
0.0560000017285
0.0560000017285
0.0560000017285
0.0570000000298

[EDIT_2]:

哦...如果我注释掉整个 cv2 部分，我会得到实时打印输出！所以 cv2 减慢了它的速度，以至于我以更慢的速度显示 15Hz 测量值。

所以我现在的问题是：是否有能够以更高速率刷新的 cv2 替代品？

Answer 1

您可以使用 Librviz，但那是在 C++ 中，我还没有看到它的 python 版本。您可以使用 OpenGL (PyOpenGL)，但我不推荐它，因为它使您打算做的事情变得非常复杂，但速度很快。

为什么不将 rviz 仅用于可视化并在其他地方做其他事情？

我什至在 rviz 中看到了整个框架（查看 Moveit 框架）。 Rviz 是完全可定制的，你可以为它编写你自己的插件，它会处理你想要的任何主题的输出。

Answer 2

把cv2.circlecv2.imshowcv2.waitkey从for循环中移出来，问题就解决了

Answer 3

您尝试使用 cv2 实现什么 - 显示激光雷达扫描图像、边缘检测、线检测等？

RVIZ 已经显示了地图，所以仅实时激光雷达扫描图像输出 cv2 就有点过分了。

如果您使用 cv2 进行边缘检测或墙壁跟踪，您可以通过采样来显着加快速度，例如每秒一次扫描或十分之一的扫描，这通常是大致相同的，直到您了解您的处理量系统可以做到。然后在代码后面添加 Canny 边缘检测 and/or 行（墙）并每 10 秒显示一张调试图像，这样您就可以看到它是如何工作的。

您还可以使用 angle_min 和 angle_max 使您的代码更加通用，这样其他扫描器也可以工作。

rospy 中的实时 2D 激光扫描仪数据

Live 2D laser scanner data in rospy

python

numpy

ros

rospy

cv2