I just got a Sick Tim 571 laser scanner. Since I'm new to ROS I wanted to test some stuff in an easy rospy implementation.
I thought that the code below will show me a live output of the laser measurements like it is possible in Rviz (Rviz works fine for me) - but in Python and with the possibility to use the measurements in my own code. Unfortunately, the output frame shows only one static measurement (from the time when the Python code was started for the first time) over and over again.
If I run Rviz parallel to this Python code, I get a dynamically updated representation of the measured area.
I thought that the .callback(data) function is called with a new set of laser scanner data each time. But it seems like that it works not as I imagined. So the error is possibly located in .laser_listener() where the callback function is called.
TL;DR
How can I use dynamically updated (live) laser scanner measurements in 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]:
When I add print(data.ranges[405]) to the callback function I get this output. It changes slightly. But it's wrong. I covered the whole sensor in the middle of the measurement. The values still only fit for the time when the program was started.
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
The same as above but the other way around. I started with a covered sensor and lifted the cover during the measurement.
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]:
Oh... if I comment out the whole cv2 part, I get the realtime print output! So cv2 slows it so much down that I get the 15Hz measurement shown at a much slower rate.
So my question is now: Is there an alternative to cv2 that is capable to refresh at a higher rate?
You Can Use Librviz But thats In C++ and i haven't seen python version for it. You can Use OpenGL (PyOpenGL) But I Don't Recommend It Cause it makes what u intened to do Really Complex but it's fast.
Why Not Use the rviz for visualization Only and do Other things elsewhere?
I've even seen a whole framework Placed In rviz(check Moveit framework). Rviz is Completely Customizable You can write Your Own PlugIns for it and it will Handle The outputing whatever topic You want.