ROS: Karte der Tiefen auf dem Raspberry Pi "wenig Blut"

    Bild

    Wenn Sie beim Erstellen von Robotern ROS verwenden, wissen Sie wahrscheinlich, dass das Arbeiten mit Stereokameras unterstützt wird. Sie können beispielsweise eine Karte der Tiefen des sichtbaren Teils des Raums oder eine Punktewolke erstellen. Und ich fragte mich, wie einfach es wäre, eine Raspberry-basierte StereoPi-Stereokamera in ROS zu verwenden. Ich war schon früher überzeugt, dass die Tiefenkarte von den Kräften von OpenCV perfekt erstellt wurde, aber ich habe mich nie mit ROS befasst. Und ich entschied mich zu versuchen. Ich möchte über meine Abenteuer bei der Suche nach einer Lösung sprechen.

    1. Kommt ROS auf einem Raspberry Pi überhaupt vor?


    Zunächst beschloss ich herauszufinden, ob es möglich ist, eine ROS für den Raspberry Pi zu bauen. Das erste , was ich zu Google vorgeschlagen - ist eine Liste mit Anweisungen, wie verschiedene Versionen von ROS auf dem Raspberry Pi zu installieren, und es ist diese Seite ROS das Wiki

    ausgezeichnet, schon auf dem, was einen Anfang! Ich erinnerte mich gut daran, wie lange die Montage von OpenCV auf Raspberry dauerte (etwa acht Stunden), und ich beschloss, nach vorgefertigten Bildern von MicroSD-Karten zu suchen, um Zeit zu sparen.

    2. Gibt es fertige Bilder von microSD-Karten mit ROS für Himbeere?


    Es stellte sich heraus, dass dieses Problem bereits von mehreren Entwicklungsteams gelöst wurde. Wenn Sie keine einmaligen Build-Enthusiasten nehmen, wurden einige Bilder hervorgehoben, die mit der Veröffentlichung neuer Versionen von OS und ROS ständig aktualisiert werden.

    Die erste Option ist ROS, installiert auf dem nativen Raspbian-Betriebssystem des ROSbots-Teams. Hier ist eine Seite mit einem aktualisierten Link zum Bild: Ready-to-Use-Image-Raspbian-Stretch-Ros-Opencv Die

    zweite ist die Bilder von Ubiquiti Robotics auf Ubunt .

    Nun, die zweite Frage wurde auch schnell geschlossen. Es ist Zeit tief zu tauchen.

    3. Wie funktioniert ROS mit der Raspberry Pi-Kamera?


    Und welche Stereokameras werden im Allgemeinen von ROS unterstützt? Ich habe mir die Seite mit Stereokameras angesehen, für die das Vorhandensein von vorgefertigten Treibern für ROS angegeben ist:

    wiki.ros.org/Sensors

    Es gab zwei Bereiche:
    2.3 3D-Sensoren (Entfernungsmesser und RGB-D-Kameras)
    2.5 Kameras

    Das stellte sich im ersten Abschnitt heraus Es werden nicht nur Stereokameras angezeigt, sondern auch TOF-Sensoren und Scan-Lidars - im Allgemeinen alles, was sofort Informationen in 3D liefern kann. Und in der zweiten gibt es Stereokameras. Der Versuch, die Treiber für mehrere Stereokameras anzuschauen, trug nicht zu meiner Freude bei, da ich auf ein ernstes Eintauchen in den Code hinwies.

    Treten Sie zurück. Wie ist die Arbeit mit einer Raspberry Pi-Kamera in ROS?

    Hier hatte ich drei angenehme Überraschungen:

    • Es stellt sich heraus, dass es für ROS einen speziellen Knoten raspicam_node gibt, nur um mit der Raspberry Pi-Kamera zu arbeiten
    • Die Soda-Knoten liegen auf github, der Code wird aktiv verwaltet und gut dokumentiert: github.com/UbiquityRobotics/raspicam_node
    • Rohan Agrawal Node Author ( @Rohbotics ) arbeitet in einem Unternehmen, das eines der fertigen Bilder für Raspberry Pi aktiv unterstützt

    Ich schaute auf das githabby Repository von raspicam_node und untersuchte die Probleme. Dort fand ich ein fast sieben Monate altes Problem mit dem Namen "Stereo-Modus" ohne Antworten und Kommentare. Eigentlich sind darin weitere und alle Ereignisse entstanden.

    4. Hardcore oder nicht?


    Um den Autoren keine Kinderfragen zu stellen, habe ich beschlossen, mir die Quellen anzusehen und zu bewerten, was durch den Zusatz des Stereomodus bedroht ist. Ich war mehr an dem Sishnaya-Teil interessiert: github.com/UbiquityRobotics/raspicam_node/tree/kinetic/src
    Nun, die Jungs haben einen Treiber geschrieben, der ins MMAL-Level stürzt. Ich erinnerte mich auch, dass die Quelle für die Arbeit von Himbeer in Stereo ebenfalls offen ist (die Entwicklung kann hier auf dem Forum Himbeer verfolgt werden), und die Aufgabe, einen vollwertigen Stereotreiber zu schreiben, ist zwar lösbar, aber im großen Maßstab. Bei der Beschreibung der Treiber anderer Kameras wurde mir klar, dass nicht nur das linke und das rechte Bild veröffentlicht werden musste, sondern auch die Parameter beider Kameras ausgegeben werden mussten, um meine eigenen Kalibrierungsergebnisse auf die einzelnen Kameras anzuwenden und viele andere Dinge zu tun. Es dauerte ein oder zwei Monate an Experimenten. Daher beschloss ich, den Ansatz zu parallelisieren, nämlich dem Autor eine Frage zur Stereo-Unterstützung zu schreiben und selbst nach einer einfacheren, aber funktionierenden Lösung zu suchen.

    5. Dialoge mit dem Autor


    Im Zweig über Stereo auf Github stellte ich dem Autor eine Frage und erwähnte, dass die Stereoanlage seit 2014 von Himbeeren unterstützt wird, und schlug gegebenenfalls vor, ihm ein Debugging-Board für Experimente zu schicken. Ich möchte Sie daran erinnern, dass ich immer noch daran gezweifelt habe, dass die Stereoanlage in diesem Distribution-Kit wie im einheimischen Raspbian funktioniert.

    Rohan antwortete überraschend schnell und sagte, dass ihr Distributor den Himbeer-Kernel verwendet und alles funktionieren sollte. Und bat darum, es auf einer ihrer Baugruppen zu überprüfen.

    Himbeerkern! Hurra! Theoretisch sollte das Stereobild aufgenommen werden, ohne mit einem Tamburin zu tanzen!

    Ich habe ihr aktuelles Bild von dem Link von Rohan heruntergeladen und bereitgestellt und ein einfaches Python-Skript ausgeführt, um das Stereopaar zu erfassen. Es hat funktioniert

    Bild

    Danach schrieb Rohan, dass er den Treibercode für Stereo betrachten würde, und schrieb ein paar Fragen. In unserem Stereomodus erhalten wir zum Beispiel ein verklebtes Bild, und wir müssten es in zwei Teile schneiden - links und rechts. Die zweite Frage zu den Kalibrierungsparametern jeder Kamera ist der Umgang damit.

    Ich sagte, dass Sie als ersten Schritt unabhängig von einer Kamera Bilder aufnehmen können. Ja, sie werden nicht in der Aufnahmezeit und den Einstellungen synchronisiert (z. B. Helligkeit-Kontrast-Weißabgleich), aber als erster Schritt kann es gut gehen.

    Rohan hat sofort einen Patch herausgebracht , mit dem Sie direkt angeben können, von welchen Kameras die Bilder vom ROS aufgenommen werden. Ich habe es überprüft - die Wahl der Kamera funktioniert bereits, ein hervorragendes Ergebnis.

    6. Unerwartete Hilfe


    Und hier im Thread gibt es einen Kommentar vom Benutzer Wezzoid. Er erzählte, dass er ein Projekt auf Basis einer Stereokamera auf Pi Compute 3 mit crimson devborda machte. Sein vierbeiniger Laufroboter trekal Position eines Objekts im Raum, ändern Sie die Position der Kameras, um einen vorgegebenen Abstand von ihm halten (das Projekt auf hackaday.io gepostet hier ).

    Bild

    Und er teilte den Code, in dem er das Bild aufgenommen hatte, schnitt es mit Python-Werkzeugen in zwei Hälften und veröffentlichte es als Knoten der linken und rechten Kamera.
    Python ist in diesen Dingen kein sehr schneller Kamerad, also benutzte er eine niedrige Auflösung von 320x240 und einen guten Life-Hack. Wenn wir das Side-by-Sibe-Stereobild aufnehmen (eine Kamera links im Stereobild, die zweite rechts), dann sollte der Python jede der 240 Zeilen in zwei Hälften schneiden. Wenn Sie jedoch ein Bild von oben nach unten erstellen (die linke Kamera ist die obere Hälfte des Bildes, die rechte die untere), dann schneidet der Python das Array in einem Durchgang in zwei Hälften. Was wurde erfolgreich vom Benutzer Wezzoid gemacht.
    Außerdem hat er seinen Python-Code auf Pastebin veröffentlicht, der diese Operation durchführte. Da ist er:

    Wezzoid-Code zum Veröffentlichen von Knoten zweier Kameras aus einem Stereopaar
    #!/usr/bin/env python# picamera stereo ROS node using dual CSI Pi CS3 board# Wes Freeman 2018# modified from code by Adrian Rosebrock, pyimagesearch.com# and jensenb, https://gist.github.com/jensenb/7303362from picamera.array import PiRGBArray
    from picamera import PiCamera
    import time
    import rospy
    from sensor_msgs.msg import CameraInfo, Image
    import yaml
    import io
    import signal # for ctrl-C handlingimport sys
    defparse_calibration_yaml(calib_file):with file(calib_file, 'r') as f:
            params = yaml.load(f)
        cam_info = CameraInfo()
        cam_info.height = params['image_height']
        cam_info.width = params['image_width']
        cam_info.distortion_model = params['distortion_model']
        cam_info.K = params['camera_matrix']['data']
        cam_info.D = params['distortion_coefficients']['data']
        cam_info.R = params['rectification_matrix']['data']
        cam_info.P = params['projection_matrix']['data']
        return cam_info
    # cam resolution
    res_x = 320#320 # per camera
    res_y = 240#240 
    target_FPS = 15# initialize the cameraprint"Init camera..."
    camera = PiCamera(stereo_mode = 'top-bottom',stereo_decimate=False)
    camera.resolution = (res_x, res_y*2) # top-bottom stereo
    camera.framerate = target_FPS
    # using several camera options can cause instability, hangs after a while
    camera.exposure_mode = 'antishake'#camera.video_stabilization = True # fussy about res?
    stream = io.BytesIO()
    # ----------------------------------------------------------#setup the publishersprint"init publishers"# queue_size should be roughly equal to FPS or that causes lag?
    left_img_pub = rospy.Publisher('left/image_raw', Image, queue_size=1)
    right_img_pub = rospy.Publisher('right/image_raw', Image, queue_size=1)
    left_cam_pub = rospy.Publisher('left/camera_info', CameraInfo, queue_size=1)
    right_cam_pub = rospy.Publisher('right/camera_info', CameraInfo, queue_size=1)
    rospy.init_node('stereo_pub')
    # init messages
    left_img_msg = Image()
    left_img_msg.height = res_y
    left_img_msg.width = res_x
    left_img_msg.step = res_x*3# bytes per row: pixels * channels * bytes per channel (1 normally)
    left_img_msg.encoding = 'rgb8'
    left_img_msg.header.frame_id = 'stereo_camera'# TF frame
    right_img_msg = Image()
    right_img_msg.height = res_y
    right_img_msg.width = res_x
    right_img_msg.step = res_x*3
    right_img_msg.encoding = 'rgb8'
    right_img_msg.header.frame_id = 'stereo_camera'
    imageBytes = res_x*res_y*3# parse the left and right camera calibration yaml files
    left_cam_info = parse_calibration_yaml('/home/pi/catkin_ws/src/mmstereocam/camera_info/left.yaml')
    right_cam_info = parse_calibration_yaml('/home/pi/catkin_ws/src/mmstereocam/camera_info/right.yaml')
    # ---------------------------------------------------------------# this is supposed to shut down gracefully on CTRL-C but doesn't quite work:defsignal_handler(signal, frame):print'CTRL-C caught'print'closing camera'
        camera.close()
        time.sleep(1)
        print'camera closed'    
        sys.exit(0)
    signal.signal(signal.SIGINT, signal_handler)
    #-----------------------------------------------------------print"Setup done, entering main loop"
    framecount=0
    frametimer=time.time()
    toggle = True# capture frames from the camerafor frame in camera.capture_continuous(stream, format="rgb", use_video_port=True):
        framecount +=1
        stamp = rospy.Time.now()
        left_img_msg.header.stamp = stamp
        right_img_msg.header.stamp = stamp
        left_cam_info.header.stamp = stamp
        right_cam_info.header.stamp = stamp    
        left_cam_pub.publish(left_cam_info)
        right_cam_pub.publish(right_cam_info)    
        frameBytes = stream.getvalue()    
        left_img_msg.data = frameBytes[:imageBytes]
        right_img_msg.data = frameBytes[imageBytes:]      
        #publish the image pair
        left_img_pub.publish(left_img_msg)
        right_img_pub.publish(right_img_msg)
        # console infoif time.time() > frametimer +1.0:
            if toggle: 
                indicator = '  o'# just so it's obviously alive if values aren't changingelse:
                indicator = '  -'
            toggle = not toggle        
            print'approx publish rate:', framecount, 'target FPS:', target_FPS, indicator
            frametimer=time.time()
            framecount=0# clear the stream ready for next frame
        stream.truncate(0)
        stream.seek(0)


    7. Starten Sie die Veröffentlichung der Knoten der linken und rechten Kamera


    Beim ersten Start wurde der Code verflucht, dass kein Zugriff auf YML-Dateien mit Kameraparametern möglich war. Ich verwendete die V2-Purpurkameras und erinnerte mich daran, dass auf dem githabe gebündelten raspicam_node Dateien mit Kalibrierungsergebnissen für verschiedene Kameramodelle vorhanden waren: github.com/UbiquityRobotics/raspicam_node/tree/kinetic/camera_info.
    Ich nahm eine davon, machte zwei Kopien und rettete sie mit den Namen left.yml und right.yml schreiben Sie die Auflösung der Kameras aus dem Skript des Autors. Folgendes ist für die linke Kamera passiert:

    left.yml
    image_width: 320
    image_height: 240
    camera_name: left
    camera_matrix:
      rows: 3
      cols: 3
      data: [1276.704618338571, 0, 634.8876509199106, 0, 1274.342831275509, 379.8318028940378, 0, 0, 1]
    distortion_model: plumb_bob
    distortion_coefficients:
      rows: 1
      cols: 5
      data: [0.1465167016954302, -0.2847343180128725, 0.00134017721235817, -0.004309553450829512, 0]
    rectification_matrix:
      rows: 3
      cols: 3
      data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
    projection_matrix:
      rows: 3
      cols: 4
      data: [1300.127197265625, 0, 630.215390285608, 0, 0, 1300.670166015625, 380.1702884455881, 0, 0, 0, 1, 0]


    Dafür wird der richtige Kameraname durch rechts ersetzt und die Datei selbst heißt right.yml. Der Rest der Datei ist identisch.

    Da ich nicht vorhatte, ein komplexes Projekt durchzuführen, habe ich die langen Pfade des Autors mit verschachtelten Unterordnern nicht wiederholt und die Dateien einfach im Stammverzeichnis des Ausgangsordners neben dem Python-Skript abgelegt. Der Code wurde erfolgreich gestartet und zeigt Statusmeldungen in der Konsole an.

    Bild

    Es blieb nur noch zu sehen, was letztendlich von unseren linken und rechten Kameras veröffentlicht wurde. Dafür habe ich rqt_image_view ausgeführt. Die Elemente / left / image_raw und / right / image_raw wurden im Dropdown-Menü angezeigt, und als ich sie auswählte, sah ich Bilder von der linken und rechten Kamera.

    Bild

    Nun, diese Sache hat verdient! Nun der lustige Teil.

    8. Beobachten Sie die Tiefenkarte.


    Um die Tiefenkarte anzuzeigen, habe ich meinen Ansatz nicht erfunden und ging das klassische ROS-Handbuch durch, um die Stereoparameter einzustellen .
    Von dort aus fand ich heraus, dass es schön wäre, beide Knoten in einem bestimmten Namespace zu veröffentlichen und nicht wie bei Wezzoid im Stammverzeichnis. Als Ergebnis die alten Zeilen der Veröffentlichung des Formulars

    left_img_pub = rospy.Publisher('left/image_raw', Image, queue_size=1)

    begann so auszusehen:

    left_img_pub = rospy.Publisher('stereo/right/image_raw', Image, queue_size=1)

    Führen Sie danach den Stereo-Stereo-Verarbeitungsknoten aus: stereo_image_proc:

    ROS_NAMESPACE=stereo rosrun stereo_image_proc stereo_ige_proc

    Nun, wir wollen auch das Ergebnis betrachten, also starten wir den Betrachter:

    rosrun image_view stereo_view stereo:=/stereo image:=image_rect_color

    Führen Sie das Konfigurationsdienstprogramm aus, um die Parameter der Tiefenkarte zu konfigurieren:

    rosrun rqt_reconfigure rqt_reconfigure

    Als Ergebnis sehen wir das Bild ganz am Anfang des Artikels. Hier ist ein etwas größeres Fragment:

    Bild

    Ich habe alle Dateien auf die Githaba gestellt: github.com/realizator/StereoPi-ROS-depth-map-test

    9. Zukunftspläne


    Nach meiner Veröffentlichung des Ergebnisses in der Diskussion über die Githaba schrieb Rohan „Cool! Die Wanderung muss ich mein StereoPi abholen. Wir haben mit ihm per Post abgeschrieben, ich habe ihm eine Gebühr geschickt. Ich hoffe, dass es ihm mit einem Bügeleisen in der Hand leichter fällt, einen vollwertigen Stereotreiber für ROS und Raspberry zu beenden und zu debuggen.

    10. Ergebnisse


    Die Tiefenkarte aus dem Stereobild auf Himbeere in ROS kann auf verschiedene Arten erhalten werden. Der für die schnelle Überprüfung gewählte Pfad ist nicht optimal für die Leistung, kann jedoch für Anwendungszwecke verwendet werden. Die Schönheit seiner Einfachheit und die Fähigkeit, Experimente sofort zu beginnen.

    Nun, zum Spaß: Nachdem ich die Ergebnisse erhalten hatte, fiel mir auf, dass Wezzoid, der seine Lösung vorschlug, der Autor der Frage nach der Veröffentlichung von zwei Stereobildern war. Fragte er, entschied er.

    Jetzt auch beliebt: