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

Question

我刚得到一个生病的tim571激光扫描仪。因为我是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()

---

[编辑1]：

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

^{pr2}$

同上，相反。我从一个被覆盖的传感器开始，在测量过程中掀开盖子。在

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

---

[编辑2]：

哦。。。如果我注释掉整个cv2部分，我将得到实时打印输出！所以cv2减慢了很多，我得到了15Hz的测量值，速度慢得多。在

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

Answer 1

你可以使用LabVIEZ，但它是C++的，我还没有看到Python版本。 你可以使用OpenGL（PyOpenGL），但我不推荐它，因为它使你想要做的事情变得非常复杂，但它很快。在

为什么不把rviz只用于可视化，而在其他地方做其他事情呢？在

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