⚠️ 🤛🏾 👍🏾 ROS: peta kedalaman pada Raspberry Pi "darah rendah" 💍 🥃 🤜🏽

Jika Anda menggunakan ROS saat membuat robot, maka Anda mungkin tahu bahwa itu memiliki dukungan untuk bekerja dengan kamera stereo. Anda dapat membangun, misalnya, peta kedalaman bagian yang terlihat dari ruang atau awan titik. Dan saya bertanya-tanya betapa mudahnya menggunakan kamera stereo StereoPi berbasis raspberry di ROS. Sebelumnya, saya sudah yakin bahwa peta kedalaman dibangun dengan sempurna oleh OpenCV, tetapi saya tidak pernah berurusan dengan ROS. Dan saya memutuskan untuk mencobanya. Saya ingin berbicara tentang petualangan saya dalam menemukan solusi.

1. Apakah ada ROS pada Raspberry Pi?

Pada awalnya, saya memutuskan untuk mencari tahu apakah mungkin untuk membangun ROS untuk Raspberry Pi. Hal pertama yang Google katakan adalah daftar instruksi untuk menginstal berbagai versi ROS pada Raspberry Pi, yaitu halaman wiki ROS ini

Yah, sudah ada sesuatu untuk memulai! Saya ingat betul berapa lama waktu yang dibutuhkan untuk membangun OpenCV di Raspberry (sekitar delapan jam), jadi saya memutuskan untuk mencari gambar kartu MicroSD yang sudah jadi untuk menghemat waktu.

2. Apakah ada gambar kartu microSD yang sudah jadi dengan ROS untuk Raspberry?

Ternyata masalah ini sudah diatasi oleh beberapa tim pengembang. Jika Anda tidak mengambil satu kali pembuatan oleh penggemar, maka beberapa gambar menonjol yang terus diperbarui dengan rilis versi baru OS dan ROS.

Opsi pertama adalah ROS yang diinstal pada OS Raspbian asli dari tim ROSbots, di sini adalah halaman dengan tautan gambar yang diperbarui: ready-to-use-image-raspbian-stretch-ros-opencv

Yang kedua adalah gambar dari Robotika Ubiquiti di ubuntu .

Nah, pertanyaan kedua juga cukup cepat ditutup. Saatnya menyelam lebih dalam.

3. Bagaimana cara kerja ROS dengan kamera Raspberry Pi?

Dan kamera stereo mana yang umumnya didukung dalam ROS? Saya melihat halaman dengan kamera stereo, yang menyatakan ketersediaan driver siap pakai untuk ROS, yang ini:

wiki.ros.org/Sensors

Ada dua bagian:
2.3 Sensor 3D (pencari jarak & kamera RGB-D)
2.5 Kamera
Ternyata di bagian pertama tidak hanya kamera stereo, tetapi juga sensor TOF, dan pemindaian lidar terdaftar - secara umum, semua yang dapat langsung memberikan informasi dalam 3D. Dan di kedua sudah ada kamera stereo. Mencoba melihat driver untuk beberapa kamera stereo tidak menambah kegembiraan saya, karena mengisyaratkan perendaman serius dalam kode.

Oke, mundur selangkah. Bagaimana cara kerjanya dengan kamera Raspberry Pi tunggal di ROS?

Tiga kejutan menyenangkan menunggu saya di sini:

ternyata untuk ROS ada node raspicam_node khusus hanya untuk bekerja dengan kamera Raspberry Pi
macam-macam simpul terletak pada github, kode ini dipelihara secara aktif dan didokumentasikan dengan baik: github.com/UbiquityRobotics/raspicam_node
penulis simpul Rohan Agrawal ( @Rohbotics ) bekerja untuk perusahaan yang secara aktif mendukung salah satu gambar siap pakai untuk Raspberry Pi

Saya melihat raspicam_node repositori github dan melihat masalah. Di sana saya menemukan masalah terbuka dengan nama "mode stereo" yang luas hampir tujuh bulan yang lalu, tanpa jawaban dan komentar. Sebenarnya, di dalamnya semua acara dikembangkan lebih lanjut.

4. Hardcore atau tidak?

Agar tidak mengajukan pertanyaan anak-anak kepada penulis, saya memutuskan untuk melihat kode sumber dan mengevaluasi apa yang mengancam penambahan mode stereo. Saya lebih tertarik pada bagian sistem di sini: github.com/UbiquityRobotics/raspicam_node/tree/kinetic/src
Nah, orang-orang menulis driver yang terjun ke level MMAL. Saya juga ingat bahwa kode sumber untuk raspberry dalam mode stereo juga terbuka (evolusi dapat dilacak di sini di forum raspberry ), dan tugas menulis driver stereo lengkap dapat dipecahkan, tetapi dalam skala besar. Melihat deskripsi driver kamera lain, saya menyadari bahwa perlu untuk menerbitkan tidak hanya gambar kiri dan kanan, tetapi juga memberikan parameter dari kedua kamera, menerapkan hasil kalibrasi untuk masing-masing, dan melakukan banyak hal lainnya. Ini menarik eksperimen selama satu atau dua bulan. Oleh karena itu, saya memutuskan untuk memparalelkan pendekatan, yaitu: menulis kepada penulis pertanyaan tentang dukungan stereo, dan mencari solusi yang lebih sederhana, tetapi bekerja sendiri.

5. Dialog dengan penulis

Dalam utas tentang mode stereo pada github, saya mengajukan pertanyaan kepada penulis, menyebutkan bahwa stereo telah didukung oleh raspberry sejak 2014, dan menyarankan, jika perlu, mengiriminya papan debug untuk percobaan. Biarkan saya mengingatkan Anda bahwa saya masih ragu bahwa dalam distribusi ini stereo akan berfungsi seperti pada Raspbian asli.

Rohan merespon dengan cepat, mengatakan bahwa distrib mereka menggunakan raspberry kernel dan semuanya akan berfungsi. Dan diminta memeriksanya di salah satu majelis mereka.

Inti raspberry! Hore! Secara teoritis, gambar stereo harus ditangkap tanpa menari dengan rebana!

Saya mengunduh dan menggunakan gambar terbaru mereka menggunakan tautan dari Rohan dan menjalankan skrip python sederhana untuk menangkap pasangan stereo. Berhasil!

gambar

Setelah itu, Rohan menulis bahwa ia akan melihat kode driver untuk mode stereo, dan menulis beberapa pertanyaan. Sebagai contoh, mode stereo kami menghasilkan satu gambar terpaku, dan kami perlu memotongnya menjadi dua - kiri dan kanan. Dan pertanyaan kedua tentang parameter kalibrasi dari masing-masing kamera adalah bagaimana menghadapinya.

Saya mengatakan bahwa sebagai langkah pertama, Anda dapat mengambil gambar dari kamera secara mandiri. Ya, mereka tidak akan disinkronkan dalam waktu pengambilan dan pengaturan (seperti kecerahan-kontras-white balance), tetapi sebagai langkah pertama, ini mungkin akan turun.

Rohan segera meluncurkan tambalan yang memungkinkan Anda menentukan secara langsung dari ROS kamera yang akan diambil gambarnya. Saya memeriksanya - memilih kamera berfungsi, itu sudah hasil yang sangat baik.

6. Bantuan yang tidak terduga

Dan kemudian komentar dari pengguna Wezzoid muncul di utas. Dia mengatakan bahwa dia sedang mengerjakan proyek yang didasarkan pada kamera stereo pada Pi Compute 3 menggunakan raspberry devboards. Robot berjalan empat kakinya melacak posisi objek di luar angkasa, mengubah posisi kamera dan menjaga jarak yang ditentukan untuk itu (proyek diposting di hackaday.io di sini ).

gambar

Dan dia membagikan kode di mana dia mengambil gambar, memotongnya menjadi dua dengan python dan membaginya seperti node kamera kiri dan kanan.
Python bukan teman yang sangat cepat dalam hal ini, jadi dia menggunakan resolusi rendah 320x240 dan peretasan kehidupan yang baik. Jika kita menangkap gambar stereo berdampingan (satu kamera di sebelah kiri gambar stereo, yang kedua di sebelah kanan), maka python harus memotong masing-masing dari 240 garis menjadi dua. Tetapi jika Anda membuat gambar atas-bawah (kamera kiri adalah bagian atas bingkai, kanan adalah bawah), maka python memotong array menjadi dua dalam satu operasi. Yang berhasil dilakukan oleh pengguna Wezzoid.
Selain itu, ia memposting kode python-nya di Pastebin, yang melakukan operasi ini. Ini dia:

Kode wezzoid untuk menerbitkan node dua kamera dari pasangan stereo

#!/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/7303362 from 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 handling import sys def parse_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 camera print "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 publishers print "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: def signal_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 camera for 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 info if time.time() > frametimer +1.0: if toggle: indicator = ' o' # just so it's obviously alive if values aren't changing else: 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. Mulai menerbitkan node dari kamera kiri dan kanan

Pada awal pertama, kode mengutuk bahwa tidak ada akses ke file YML dengan parameter kamera. Saya menggunakan kamera V2 berwarna raspberry dan ingat bahwa file yang sudah jadi dengan hasil kalibrasi untuk model kamera yang berbeda datang ke raspicam_node di github: github.com/UbiquityRobotics/raspicam_node/tree/kinetic/camera_info
Saya mengambil salah satu dari mereka, membuat dua salinan dan menyimpannya dengan nama left.yml dan right.yml, menulis di dalamnya resolusi kamera dari skrip penulis. Inilah yang terjadi pada kamera kiri:

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]

Untuk kanan, nama kamera diganti dengan kanan, dan file itu sendiri dinamai right.yml. Sisa file itu identik.

Karena saya tidak berencana untuk melakukan proyek yang kompleks, saya tidak mengulangi jalur panjang penulis dengan subfolder dan hanya meletakkan file di root folder home di sebelah skrip python. Kode mulai berhasil, menampilkan pesan status di konsol.

gambar

Hanya tinggal melihat apa yang akhirnya diterbitkan oleh kamera kiri dan kanan kami. Untuk melakukan ini, saya meluncurkan rqt_image_view. Item / kiri / image_raw dan / kanan / image_raw muncul di menu drop-down.Ketika saya memilih mereka, saya melihat gambar dari kamera kiri dan kanan.

gambar

Nah, hal ini sudah didapat! Sekarang bagian yang menyenangkan.

8. Kami melihat peta kedalaman.

Untuk melihat peta kedalaman, saya tidak datang dengan pendekatan saya sendiri dan pergi ke manual ROS klasik untuk mengatur parameter stereo .
Dari sana, saya menemukan bahwa akan lebih baik untuk mempublikasikan kedua node dalam namespace tertentu, dan tidak di root seperti yang dilakukan Wezzoid. Akibatnya, garis-garis lama publikasi formulir

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

mulai terlihat seperti ini:

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

Setelah itu, kami meluncurkan node pemrosesan mode stereo_image_proc:

 ROS_NAMESPACE=stereo rosrun stereo_image_proc stereo_ige_proc

Yah, kami juga ingin melihat hasilnya, jadi kami memulai pengamat:

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

Dan untuk mengonfigurasi parameter peta kedalaman, jalankan utilitas konfigurasi:

 rosrun rqt_reconfigure rqt_reconfigure

Hasilnya, kita melihat gambar di bagian paling awal artikel. Ini sedikit lebih besar:

gambar

Semua file yang saya posting di github: github.com/realizator/StereoPi-ROS-depth-map-test

9. Rencana segera

Setelah saya mempublikasikan hasilnya dalam diskusi tentang github, Rohan menulis “Keren! Saya harus mengambil StereoPi saya. " Kami menulis kepadanya melalui surat, saya mengiriminya biaya. Saya berharap bahwa dengan perangkat keras yang berfungsi di tangannya akan lebih mudah baginya untuk menyelesaikan dan men-debug driver stereo lengkap untuk ROS dan Raspberry.

10. Ringkasan

Peta kedalaman dari gambar stereo pada raspberry di ROS dapat diperoleh dengan beberapa cara. Jalur yang dipilih untuk verifikasi cepat bukan yang paling optimal dalam hal kinerja, tetapi dapat digunakan untuk tujuan aplikasi. Keindahan kesederhanaannya dan kemampuan untuk segera memulai percobaan.

Nah, dari lucu: setelah menerima hasil, saya perhatikan bahwa Wezzoid, yang mengusulkan solusinya, adalah penulis pertanyaan tentang penerbitan dua gambar stereo. Dia bertanya pada dirinya sendiri, dia memutuskan.

ROS: peta kedalaman pada Raspberry Pi "darah rendah"