Если вы используете ROS при создании роботов, то наверняка знаете, что в ней есть поддержка работы со стереокамерами. Можно построить, например, карту глубин видимой части пространства или облако точек. И мне стало интересно, насколько просто будет использовать в ROS стереокамеру StereoPi на базе малины. Раньше я уже убедился, что карта глубин отлично строится силами OpenCV, но вот с ROS никогда дела не имел. И решил попробовать. Я хочу рассказать о моих приключениях в поиске решения.
1. Бывает ли вообще ROS на Raspberry Pi?
Сначала я решил узнать, есть ли возможность собрать ROS для Raspberry Pi. Первое, что мне подсказал гугл – это список инструкций по установке разных версий ROS на Raspberry Pi, а именно вот эту страничку ROS wiki
Отлично, уже есть от чего отталкиваться! Я хорошо помнил, сколько занимала сборка OpenCV на Raspberry (примерно часов восемь), поэтому решил поискать готовые образы карточек MicroSD для экономии времени.
2. Есть ли готовые образы карточек MicroSD с ROS для Raspberry?
Оказалось, что и этот вопрос решен уже несколькими командами разработчиков. Если не брать разовые сборки энтузиастами, то выделялась парочка образов, которые постоянно обновляются с выходом новых версий ОС и ROS.
Первый вариант – это ROS, установленный на родную ОС Raspbian от команды ROSbots, вот страничка с обновляемой ссылкой на образ: ready-to-use-image-raspbian-stretch-ros-opencv
Второй – это образы от Ubiquiti Robotics на убунте.
Ну что же, второй вопрос тоже был достаточно быстро закрыт. Пора заныривать поглубже.
3. Как в ROS устроена работа с камерой Raspberry Pi?
А какие стереокамеры вообще поддерживаются в ROS? Я посмотрел страничку со стереокамерами, для которых заявлено наличие готовых драйвера под ROS, вот эту:
Там нашлось два раздела:
2.3 3D Sensors (range finders & RGB-D cameras)
2.5 Cameras
Оказалось, что в первом разделе указаны не только стереокамеры, но и TOF сенсоры, и сканирующие лидары — в общем всё, что сразу может давать информацию в 3D. А во втором уже идут стереокамеры. Попытка посмотреть драйвера к нескольким стереокамерам радости мне не прибавила, так как намекала на серьезное погружение в код.
Хорошо, отступим на шаг назад. Как идет работа с одной камерой Raspberry Pi в ROS?
Тут меня ждало три приятных сюрприза:
- оказывается, для ROS есть специальная нода raspicam_node как раз для работы с камерой Raspberry Pi
- сорсы ноды лежат на гитхабе, код активно поддерживается и хорошо документирован: github.com/UbiquityRobotics/raspicam_node
- автор ноды Rohan Agrawal (@Rohbotics) работает в компании, которая активно поддерживает один из готовых образов для Raspberry Pi
Я посмотрел гитхабовский репозиторий raspicam_node и заглянул в issues. Там я нашел открытую issue с ёмким названием «stereo mode» почти семимесячной давности, без ответов и комментариев. Собственно, в ней дальше и развивались все события.
4. Хардкор или нет?
Чтобы не задавать детских вопросов авторам я решил посмотреть исходники и оценить, чем грозит добавление стереорежима. Меня больше заинтересовала сишная часть вот тут: github.com/UbiquityRobotics/raspicam_node/tree/kinetic/src
Ну что, ребята написали драйвер погружаясь на уровень MMAL. Я также вспомнил, что исходники по работе малины в стереорежиме тоже открыты (эволюцию можно проследить вот тут на форуме малины), и задача написания полноценного стереодрайвера решаема, но масштабна. Глянув на описание драйверов других камер я понял, что необходимо публиковать не только левую и правую картинки, но и выдавать параметры обеих камер, применять к каждой свои результаты калибровки и делать много других вещей. Это тянуло на эксперименты длиной в месяц-два. Поэтому я решил распараллелить подход, а именно: написать автору вопрос о поддержке стерео, а самому поискать более простое, но работающее решение.
5. Диалоги с автором
В ветке про стереорежим на гитхабе я задал вопрос автору, упомянув, что стерео поддерживается малиной аж с 2014 года, и предложил в случае надобности выслать ему отладочную плату для экспериментов. Напомню, что я все еще сомневался в том, что в данном дистрибутиве стерео будет работать как в родном Raspbian.
Rohan ответил на удивление быстро, сообщив, что их дистриб использует малиновое ядро и все должно работать. И попросил проверить это на одной из их сборок.
Малиновое ядро! Ура! Теоретически стереокартинка должна захватываться без танцев с бубном!
Я скачал и развернул их последний образ по ссылке от Rohan и запустил простенький скрипт на питоне по захвату стереопары. Оно работало!
После этого Rohan написал, что он глянет код драйвера на предмет стереорежима, и написал парочку вопросов. Например, у нас стереорежим выдает одну склеенную картинку, а нам бы надо ее резать на две – левую и правую. И второй вопрос о параметрах калибровки каждой камеры – как с этим быть.
Я сказал, что в качестве первого шага можно брать картинки с камер независимо. Да, они не будут синхронизированы по времени захвата и настройкам (типа яркости-контрастности-баланса белого), но в качестве первого шага это может вполне сойти.
Rohan оперативно выкатил патч, позволяющий непосредственно из ROS указывать, с какой из камер брать картинки. Я его проверил – выбор камеры работает, уже отличный результат.
6. Неожиданная помощь
И тут в ветке появляется комментарий от юзера Wezzoid. Он рассказал, что делал проект на базе стереокамеры на Pi Compute 3 с использованием малиновой девборды. Его четырехногий шагающий робот трекал положение объекта в пространстве, менял положение камер и держал заданное расстояние до него (проект выложен на hackaday.io тут ).
И он поделился кодом, в котором он захватывал картинку, резал ее питоновыми средствами пополам и публиколвал как ноды левой и правой камер.
Питон в этих вопросах не очень быстрый товарищ, поэтому он использовал невысокое разрешение 320х240 и хороший лайфхак. Если мы захватываем стереокартинку side-by-sibe (одна камера слева на стереокартинке, вторая справа), то питон должен порезать каждую из 240 строчек пополам. А вот если сделать картинку top-bottom (левая камера – верхняя половина кадра, правая – нижняя), то питон режет массив пополам за одну операцию. Что и было успешно сделано юзером Wezzoid.
Плюс он выложил свой питоновый код на Pastebin, который проделывал эту операцию. Вот он:
#!/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. Запускаем публикацию нод левой и правой камер
При первом запуске код ругнулся, что нету доступа к YML файлам с параметрами камер. Я использовал малиновые камеры V2 и помнил, что на гитхабе в комплекте к raspicam_node шли готовые файлики с результатами калибровки для разных моделей камер: github.com/UbiquityRobotics/raspicam_node/tree/kinetic/camera_info
Я взял один из них, сделал две копии и сохранил с именами left.yml и right.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]
Для правой имя камеры заменено на right, а сам файлик назван right.yml. В остальном файл идентичен.
Так как я не планировал делать сложный проект, я не стал повторять длинные пути автора с вложенными подпапками и просто положил файлики в корень домашней папки рядом с питоновым скриптом. Код успешно стартовал, выводя в консоли статусные сообщения.
Оставалось только посмотреть, что же в итоге публикуется нашими левой и правой камерами. Для этого я запустил rqt_image_view. В выпадающем меню появились пункты /left/image_raw и /right/image_raw, при выборе которых я видел изображения с левой и правой камер.
Ну что же, эта штука заработала! Теперь самое интересное.
8. Смотрим карту глубин.
Для просмотра карты глубин я не стал придумывать свой подход и пошел по классическому мануалу из ROS по настройке стереопараметров.
Оттуда я выяснил, что хорошо бы публиковать обе ноды в определенном namespace, а не в корне как это делал Wezzoid. В итоге старые строчки публикации вида
left_img_pub = rospy.Publisher('left/image_raw', Image, queue_size=1)стали выглядеть так:
left_img_pub = rospy.Publisher('stereo/right/image_raw', Image, queue_size=1)После этого запускаем ноду обработки стереорежима stereo_image_proc:
ROS_NAMESPACE=stereo rosrun stereo_image_proc stereo_ige_procНу и нам же хочется посмотреть на результат, поэтому стартуем смотрелку:
rosrun image_view stereo_view stereo:=/stereo image:=image_rect_colorИ для настройки параметров карты глубин запускаем утилиту настройки:
rosrun rqt_reconfigure rqt_reconfigureВ итоге видим картинку, приведенную в самом начале статьи. Вот фрагмент чуть крупнее:
Все файлики я выложил на гитхабе: github.com/realizator/StereoPi-ROS-depth-map-test
9. Ближайшие планы
После моей публикации результата в дискуссии на гитхабе Rohan написал «Круто! Походу нужно мне забрать свою StereoPi». Мы списались с ним по почте, я отправил ему плату. Надеюсь, что с работающим железом в руках ему будет проще допилить и отладить полноценный стереодрайвер для ROS и Raspberry.
10. Итоги
Карту глубин из стереокартинки на малине в ROS можно получить, причем несколькими путями. Выбранный для быстрой проверки путь является не самым оптимальным в плане производительности, но может использоваться в прикладных целях. Прелесть в его простоте и возможности сразу начать эксперименты.
Ну и из забавного: уже после получения результатов я заметил что Wezzoid, предложивший свое решение, и был автором вопроса про публикацию двух стереокартинок. Сам спросил, сам решил.
Автор: Realizator
