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

Question

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

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

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

我认为每次使用一组新的激光扫描仪数据调用.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是完全可定制的你可以为它编写自己的PlugIns，它将处理输出你想要的任何主题。