Live 2D лазерный сканер данных в роспи

Question

Я только что получил лазерный сканер Sick Tim 571. Так как я новичок в ROS, я хотел протестировать некоторые вещи в легкой rospy реализация.

Я думал, что код, приведенный ниже, покажет мне живой вывод лазерных измерений, как это возможно в Rviz (Rviz прекрасно работает для меня) - но в Python и с возможностью использовать измерения в моем собственном коде. К сожалению, выходной кадр показывает только одно статическое измерение (с момента, когда код Python был запущен в первый раз) снова и снова.

Если я запускаю Rviz параллельно с этим кодом Python, я получаю динамически обновляемое представление измеренной области.

Я думал, что .callback(data) функция вызывается с новым набором данных лазерного сканера каждый раз. Но похоже, что это работает не так, как я себе представлял. Так что ошибка, возможно, находится в .laser_listener() где вызывается функция обратного вызова.

---

TL; DR

Как я могу использовать динамически обновляемые (живые) измерения лазерного сканера в 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 что способно освежить с большей скоростью?

Answer 1

Вы можете использовать Librviz, но это в C++, и я не видел Python-версию для него. Вы можете использовать OpenGL (PyOpenGL), но я не рекомендую это делать, потому что это делает то, что вы хотели сделать, действительно сложным, но это быстро.

Почему бы не использовать rviz только для визуализации и делать другие вещи в другом месте?

Я даже видел целый фреймворк, помещенный в rviz(проверьте фреймворк Moveit). Rviz полностью настраиваемый Вы можете написать свои собственные плагины для него, и он будет обрабатывать вывод любой темы, которую вы хотите.

Answer 2

Чего вы пытались добиться с помощью cv2 - отображение изображений лидарного сканирования, обнаружение краев, обнаружение линий, что-то еще?

RVIZ уже отображает карту, поэтому вывод cv2 изображения лидарного сканирования в реальном времени является излишним.

Если вы используете cv2 для обнаружения краев или отслеживания стены, вы можете значительно ускорить работу, выбирая, скажем, одно сканирование в секунду или одно из десяти сканирований, что обычно примерно одинаково, пока вы не почувствуете, сколько обработки может сделать ваша система. Затем добавьте обнаружение хитрых краев и/или строку (стену) после кода и отображайте одно изображение отладки каждые 10 секунд, чтобы вы могли видеть, как это работает.

Вы также можете сделать свой код более универсальным, используя а также так что другие сканеры тоже будут работать.

Answer 3

просто удалите cv2.circle cv2.imshow cv2.waitkey из цикла for, и проблема будет решена.