Практическое использование ROS на Raspberry Pi — часть 2

    Добрый день, уважаемые читатели Хабра! Это вторая статья из цикла статей о практическом использовании ROS на Raspberry Pi. В первой статье цикла я описал установку необходимых компонент ROS и настройку рабочего окружения для работы.
    Во второй части цикла мы приступим к практическому использованию возможностей ROS на платформе Raspberry Pi. Конкретно в данной статье я собираюсь рассказать об использовании камеры Raspberry Pi Camera Board на Raspberry Pi в связке с ROS для решения задач компьютерного зрения. Кто заинтересован, прошу под кат.


    Камера RPi Camera Board


    Для работы нам нужна будет такая вот камера Raspberry Pi Camera Board:
    image

    Эта камера подключается напрямую к графическому процессору через CSi-разъем на плате, что позволяет записывать и кодировать изображение с камеры без использования процессорного времени. Для подключения камеры используется ZIF-шлейф. Разъем для подключения шлейфа на плате находится между портами Ethernet и HDMI:

    image

    Установка библиотек


    Итак приступим к установке необходимых библиотек. Для начала установим OpenCV:

    $ sudo apt-get install libopencv-dev
    

    Для использования камеры Raspberry Pi Camera нам будет нужна библиотека raspicam. Скачайте архив отсюда.
    Далее установим библиотеку:

    $ tar xvzf raspicamxx.tgz
    $ cd raspicamxx
    $ mkdir build
    $ cd build
    $ cmake ..
    $ make
    $ sudo make install
    $ sudo ldconfig
    

    Нужно включить поддержку камеры в Raspbian через программу raspi-config:

    $ sudo raspi-config
    

    Выберите опцию 5 — Enable camera, сохраните выбор и выполните ребут системы.

    Начало работы с ROS


    Для удобства работы создадим новый catkin воркспейс для своих пакетов:

    $ mkdir -p ~/driverobot_ws/src
    $ cd ~/driverobot_ws/src
    $ catkin_init_workspace
    $ cd ~/driverobot_ws
    $ catkin_make
    

    Создайте новый пакет ROS:

    $ cd src/
    $ catkin_create_pkg raspi_cam_ros image_transport cv_bridge roscpp std_msgs sensor_msgs compressed_image_transport opencv2
    

    Спецификация команды catkin_create_pkg следующая: catkin_create_pkg <package_name> [depend1] [depend2],
    где вы можете указать под depend сколько угодно зависимостей — библиотек, которые будет использовать пакет.
    Эта команда создает каркас проекта ROS: пустую директорию src для скриптов узлов и конфигурационные файлы CMakeLists.txt and package.xml.
    Я нашел простой скрипт, который получает кадры с камеры и публикует их с помощью «паблишера» (publisher), и адаптировал его под ROS Indigo. Скачать его можно отсюда.
    Для его использования нужно установить Raspberry Pi UV4L camera driver:

    $ curl http://www.linux-projects.org/listing/uv4l_repo/lrkey.asc | sudo apt-key add -
    

    Добавьте следующую строку в файл /etc/apt/sources.list

    deb http://www.linux-projects.org/listing/uv4l_repo/raspbian/ wheezy main
    

    , обновите пакеты и выполните установку:

    $ sudo apt-get update
    $ sudo apt-get install uv4l uv4l-raspicam
    

    Для загрузки драйвера при каждой загрузке системы также установим необязательный пакет:

    $ sudo apt-get install uv4l-raspicam-extras
    

    Перейдем к редактированию пакета. Вставьте в ваш файл CMakeLists.txt строки отсюда.
    Самые важные строки в файле CMakeLists.txt:

    link_directories(/usr/lib/uv4l/uv4lext/armv6l/)
    …
    target_link_libraries(capture ${catkin_LIBRARIES} uv4lext)
    

    Таким образом мы добавляем ссылку на драйвер uv4l, необходимый для компиляции пакета.
    В конце файла мы задаем специальные строки для нашего узла:

    add_executable(capture src/capturer.cpp)
    target_link_libraries(capture ${catkin_LIBRARIES} uv4lext)
    

    Строка add_executable создает бинарник для запуска и target_link_lbraries линкует дополнительные библиотеки для бинарника capture.
    Вставьте недостающие строки отсюда в файл package.xml, чтобы он выглядел вот так:

    <?xml version="1.0"?>                                                                                            
    <package>                                                                                                        
      <name>raspi_cam_ros</name>                                                                                      
      <version>0.0.0</version>                                                                                       
      <description>The raspi_cam_ros package</description>                                                            
                                                                                                                     
      <maintainer email="pi@todo.todo">pi</maintainer>                                                               
                                                                                                                     
      <license>TODO</license>                                                                                        
      <buildtool_depend>catkin</buildtool_depend>                                                                    
      <build_depend>cv_bridge</build_depend>                                                                         
      <build_depend>image_transport</build_depend>                                                                   
      <build_depend>roscpp</build_depend>                                                                            
      <build_depend>std_msgs</build_depend>                                                                          
      <build_depend>sensor_msgs</build_depend>                                                                       
      <build_depend>opencv2</build_depend>                                                                           
      <build_depend>compressed_image_transport</build_depend>                                                        
      <run_depend>cv_bridge</run_depend>                                                                             
      <run_depend>image_transport</run_depend>                                                                       
      <run_depend>roscpp</run_depend>                                                                                
      <run_depend>std_msgs</run_depend>                                                                              
      <run_depend>sensor_msgs</run_depend>                                                                           
      <run_depend>opencv2</run_depend>                                                                               
      <run_depend>compressed_image_transport</run_depend>                                                                                                                                                                    
    </package>
    

    В первых строках задаются базовые параметры узла — название, версия, описание, информация об авторе. Строки build_depend определяют зависимости от библиотек, необходимые для компиляции пакета, а строки run_depend — зависимости, необходимые для запуска кода в пакете.
    Создайте файл capturer.cpp внутри папки src и вставьте строки отсюда. Здесь в методе main() происходит инициализация узла и его запуск в цикле:

    ros::init(argc, argv,"raspi_cam_ros");
    ros::NodeHandle n;
    UsbCamNode a(n);
    a.spin();
    

    Вся логика скрипта заключается в том, что мы получаем картинку с камеры средствами OpenCV, оборачиваем ее в сообщение для ROS в методе fillImage и публикуем в топик. Здесь используется пакет image_transport для создания “паблишера” картинки.
    Запустим драйвер uv4l выполнив команду:

    $ uv4l --driver raspicam --auto-video_nr --width 640 --height 480 --nopreview
    

    что создаст MJPEG-стриминг.
    Скомпилируем наш узел ROS:

    $ roscore
    $ cd ~/driverobot_ws
    $ catkin_make
    

    Проверьте значение переменной ROS_PACKAGE_PATH:

    echo $ROS_PACKAGE_PATH
    /opt/ros/indigo/share:/opt/ros/indigo/stacks
    

    Значение переменной ROS_PACKAGE_PATH должно включать путь до нашего воркспейса. Добавим наш воркспейс в путь:

    $ source devel/setup.bash
    

    Теперь запустив команду echo $ROS_PACKAGE_PATH еще раз мы должны увидеть подобный вывод:

    /home/youruser/catkin_ws/src:/opt/ros/indigo/share:/opt/ros/indigo/stacks
    

    , где /home/<user_name>/catkin_ws/src — это путь до нашего воркспейса. Это означает, что ROS может «видеть» наши узлы, созданные в catkin_ws и мы можем их запускать через rosrun.
    Запустим наш узел ROS:

    $ rosrun raspi_cam_ros capture
    

    Запустим графическую программу rqt_image_view для отображения видеопотока с топика:

    $ rosrun rqt_image_view rqt_image_view
    

    Выберем топик image_raw в окне rqt_image_view

    image

    При запуске узла может возникнуть ошибка “Gtk-WARNING **: cannot open display: -1” при работе через ssh или “GdkGLExt-WARNING **: Window system doesn't support OpenGL.” при запуске в режиме работы удаленного рабочего стола VNC. Решение — подключиться к Raspberry Pi через SSH с X11 forwarding:

    $ ssh -X pi@<host_pi>
    

    Можно узнать с какой частотой публикуются сообщения в топик с помощью команды rostopic:

    rostopic hz image_raw
    

    Эта команда вычисляет частоту получения сообщений на топик каждую секунду и выводит ее в консоль.
    У меня для модели B+ был вывод такого вида:

    average rate: 7.905
                  min: 0.075s max: 0.249s std dev: 0.02756s 
    

    Как видим частота публикации сообщений — 8 Гц.
    Я также проверил частоту публикации изображений с камеры на модели RPi 2. Здесь результаты были в разы лучше:

    average rate: 30.005
                  min: 0.024s max: 0.043s std dev: 0.00272s 
    

    Сообщения уже публикуются с частотой 30 Гц, что является довольно хорошим увеличением скорости по сравнению с моделью B+.

    Visual-based управление роботом с OpenCV и ROS


    Сейчас мы напишем небольшой пакет ROS для использования компьютерного зрения на роботе с Raspberry Pi, который будет выполнять алгоритм распознавания (в нашем случае визуальное ориентирование методом line following) и публиковать величину необходимого смещения робота в топик. С другой стороны узел управления движением робота будет подписываться на этот топик и посылать команды управления движением на Arduino.
    Сейчас добавим в скрипт capturer.cpp “паблишер”, который будет публиковать величину сдвига. Сначала включим определение типа сообщения для величины сдвига — std_msgs/Int16.

    #include <std_msgs/Int16.h>
    

    rosserial берет специальные файлы сообщений msg и генерирует исходный код для них. Используется такой шаблон:
    package_name/msg/Foo.msg → package_name::Foo


    Исходный код для стандартных сообщений rosserial хранится в папке package_name внутри директории ros_lib.
    Далее инициализируем сообщение для данного типа:

    std_msgs::Int16 shift_msg;
    

    Создаем “паблишера”:

    ros::Publisher shift_pub;
    

    и внутри конструктора UsbCamNode даем ему определение:

    shift_pub = nh.advertise<std_msgs/Int16>(“line_shift”, 1);
    

    Здесь мы задаем тип сообщений и название топика для публикации.
    Дальше добавим логику вычисления величины сдвига линии средствами OpenCV и ее публикации в топик line_shift в метод take_and_send_image() перед строкой #ifdef OUTPUT_ENABLED:

    // Some logic for the calculation of offest
    shift_msg.data = offset;
    shift_pub.publish(shift_msg);
    

    У меня нет готового алгоритма следования линии, поэтому читатель волен написать свою собственную логику здесь.
    Фактически данные в сообщении сохраняются в поле data. Структуру сообщения можно посмотреть с помощью команды:

    $ rosmsg show std_msgs/Int16
    

    Теперь запустим узел:

    $ rosrun raspi_cam_ros capturer
    

    Используем команду rostopic echo для вывода данных, публикуемых в топик line_shift:

    $ rostopic echo line_shift
    

    Теперь добавим “сабскрайбер” в узле управления роботом. Включим определение типа сообщения:

    #include <std_msgs/UInt16.h>
    

    Затем добавляем callback-функцию, которая выполняется при получении сообщения из топика.

    void messageCb(const std_msgs::UInt16& message) 
    {
      int shift_val = int(message.data);
      
      char* log_msg;
      if(shift_val < 0) log_msg = "Left";
      else if(shift_val > 0 ) log_msg = "Right";
      else log_msg = "Forward";
      
      nh.loginfo(log_msg);
    }
    

    callback функция должна иметь тип void и принимать const ссылку типа сообщения в качестве аргумента.
    Для простоты я вывожу в лог сообщение о направлении смещения линии. Вы можете здесь добавить собственную логику движения робота для своего сценария.
    Создаем подписчика на сообщения из топика line_shift.

    ros::Subscriber<std_msgs::UInt16> sub("line_shift", &messageCb);
    

    Здесь задаем название топика и ссылку на callback-функцию.
    Дальше идут стандартные методы скетча для rosserial_arduino:

    void setup()
    {
      nh.initNode();
      nh.subscribe(sub);
      
      Serial.begin(57600);
    }
    
    void loop()
    {
      nh.spinOnce();
      delay(100);
    }
    

    Единственное отличие — это то, что мы добавляем nh.subscribe(sub) для создания фактической “подписки” узла на топик.
    Скетч для управления роботом можно скачать отсюда.
    Маленькая хитрость! В ROS существуют специальные launch файлы, которые позволяют запускать узлы как отдельные процессы автоматически с определенными параметрами. launch файлы создаются в формате xml и их синтаксис позволяет запускать множество узлов сразу. Однако launch файл не гарантирует, что узлы будут запущены в точно заданном порядке.
    Можно создать launch файл для более легкого запуска сервера rosserial_python.

    $ cd <catkin_ws>/src
    $ catkin_create_pkg rosserial_controller
    $ cd src/rosserial_controller
    $ vim rosserial_controller.launch
    

    Напишем launch файл такого содержания:

    <launch>
     <node pkg="rosserial_python" type="serial_node.py" name="arduino_serial">
           <param name="port" value="/dev/ttyACM0"/>       
     </node>
    </launch>
    

    Скомпилируем и запустим его:

    $ cd ~/<catkin_ws>
    $ catkin_make
    $ source devel/setup.bash
    $ roslaunch rosserial_controller rosserial_controller.launch
    

    Мы можем визуализировать значения, публикуемые в тему line_shift, с помощью утилиты rqt_plot, как это сделано в статье:

    $ rqt_plot line_shift
    

    Теперь вы можете использовать все преимущества камеры Raspberry Pi Camera и библиотеки OpenCV в своих сценариях визуального ориентирования робота, распознавания объектов, слежения и многих других. Дайте волю фантазии!
    В следующий раз мы поговорим об управлении роботом в режиме teleoperation с помощью нажатия клавиш на клавиатуре.

    PS. В сети есть полезные шпаргалки — ROS Cheatsheet. Для версии ROS Indigo такую можно скачать отсюда.
    Поделиться публикацией
    Похожие публикации
    Никаких подозрительных скриптов, только релевантные баннеры. Не релевантные? Пиши на: adv@tmtm.ru с темой «Полундра»

    Зачем оно вам?
    Реклама
    Комментарии 4
    • 0
      Есть ли видео работы? Если не столь важна скорость обработки можно ли использовать обычную веб-камеру?
      • 0
        Здесь видео стриминга на топик с RPi 2: youtu.be/APJQ2dlE-oE, здесь с RPi B+: youtu.be/wE1VYIkLuJQ. Не могу сказать об обычной камере, не пробовал с обычной. Возможно здесь играет роль подключение камеры RPi к GPU
      • 0
        Все этапы проходит успешно, кроме
        rosrun test_rpi_cam capture
        В ответ получаю
        Error: package 'test_rpi_cam' not found
        В списке пакетов ничего не добавляется, это видно командой
        rospack list-names
        В статье в файле package.xml name должно быть test_rpi_cam
        А на github в приведённой ссылке везде raspi_cam_ros, кроме файла capture.cpp

        Попытка привести имена к общему не приводит к успеху.
        После компиляции catkin_make получаю такой ответ:

        Ответ
        Base path: /home/pi/waybot_ws
        Source space: /home/pi/waybot_ws/src
        Build space: /home/pi/waybot_ws/build
        Devel space: /home/pi/waybot_ws/devel
        Install space: /home/pi/waybot_ws/install


        Running command: "make cmake_check_build_system" in "/home/pi/waybot_ws/build"


        Running command: "make -j4 -l4" in "/home/pi/waybot_ws/build"


        Имя директории менял на своё, но пробовал и то, что в статье.
        Что делаю не так?
        • 0
          Запустите и посмотрите вывод команды:

          $ echo $ROS_PACKAGE_PATH

          Если вывод такой:

          /opt/ros/hydro/share:/opt/ros/hydro/stacks

          , значит в пути к исполняемым файлам нет вашего рабочего каталога waybot_ws.
          Выполните:

          $ source ~/waybot_ws/devel/setup.bash

          Скрипт добавит /home/pi/waybot_ws/src в $ROS_PACKAGE_PATH. Эту команду нужно выполнять каждый раз, когда вы создаете новый пакет в catkin workspace.
          Это должно решить проблему.
          Кстати, спасибо за указание на имя пакета. Сейчас исправлю на правильное.

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