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?

Asked By: chrisg

||

Source

Answer 1

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 2

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

Answered By: user18287137

Answer 3

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.

Answered By: brianlmerritt

Live 2D laser scanner data in rospy

Question:

Answers: