# Live 2D laser scanner data in rospy

## Question:

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.

just move out cv2.circle cv2.imshow cv2.waitkey from the for loop,and the problem will be solved.

What were you trying to achieve with cv2 – displaying lidar scan images, edge detection, line detection, something else?

RVIZ displays the map already, so just a real time lidar scan image output cv2 is overkill.

If you are using cv2 for edge detection or wall following you could speed things up significantly by sampling say one scan per second or one in ten scans which is usually about the same until you get a feel for how much processing your system can do. Then add canny edge detection and/or line (wall) following code and display one debug image every 10 seconds just so you can see how it is working.

You can also make your code more universal by using `angle_min` and `angle_max` so other scanners will also work.

Categories: questions Tags: , , , ,
Answers are sorted by their score. The answer accepted by the question owner as the best is marked with
at the top-right corner.