About Teletype
Join
Одноплатные компьютеры
@armlab
November 30, 2018

ROS: карта глубин на Raspberry Pi «малой кровью»

Если вы используете 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, вот эту:

wiki.ros.org/Sensors

Там нашлось два раздела:

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, который проделывал эту операцию. Вот он:

n/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, предложивший свое решение, и был автором вопроса про публикацию двух стереокартинок. Сам спросил, сам решил.

Источник: Habr.com

Одноплатные компьютеры
November 30, 2018, 13:53
0 reactions
0 views