1

我刚买了一台 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能够以更高的速度刷新?

4

2 回答 2

1

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

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

我什至看到了放置在 rviz 中的整个框架(检查 Moveit 框架)。Rviz 是完全可定制的,您可以为它编写自己的插件,它将处理输出您想要的任何主题。

于 2018-02-05T21:57:25.477 回答
0

只需将 cv2.circle cv2.imshow cv2.waitkey 从 for 循环中移出,问题就解决了。

于 2022-02-23T09:21:31.440 回答