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

Question

我刚买了一台Sick Tim 571激光扫描仪。由于我是 ROS 新手，我想在简单的 rospy 实现中测试一些东西。

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

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

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

<小时/>

TL;博士

如何在 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 是完全可定制的，您可以为其编写自己的插件，它将处理输出您想要的任何主题。