Практическое использование 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:


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

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

Итак приступим к установке необходимых библиотек. Для начала установим 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="[e-mail]">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



При запуске узла может возникнуть ошибка “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 такую можно скачать отсюда.