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