I just got a Sick Tim 571 laser scanner. Since I’m new to ROS I wanted to test some stuff in an easy
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.
How can I use dynamically updated (live) laser scanner measurements in
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()
When I add
print(data.ranges) 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
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.
Answered By – Mohammad Ali
Answer Checked By – Pedro (BugsFixing Volunteer)