Практическое использование 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:
$ 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 [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, чтобы он выглядел вот так:
test_rpi_cam
0.0.0
The test_rpi_cam package
pi
TODO
catkin
cv_bridge
image_transport
roscpp
std_msgs
sensor_msgs
opencv2
compressed_image_transport
cv_bridge
image_transport
roscpp
std_msgs
sensor_msgs
opencv2
compressed_image_transport
В первых строках задаются базовые параметры узла — название, версия, описание, информация об авторе. Строки build_depend определяют зависимости от библиотек, необходимые для компиляции пакета, а строки run_depend — зависимости, необходимые для запуска кода в пакете.
Создайте файл capturer.cpp внутри папки src и вставьте строки отсюда. Здесь в методе main () происходит инициализация узла и его запуск в цикле:
ros::init(argc, argv,"test_rpi_cam");
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//catkin_ws/src — это путь до нашего воркспейса. Это означает, что ROS может «видеть» наши узлы, созданные в catkin_ws и мы можем их запускать через rosrun.
Запустим наш узел ROS:
$ rosrun test_rpi_cam 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@
Можно узнать с какой частотой публикуются сообщения в топик с помощью команды 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
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("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
Затем добавляем 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 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 /src
$ catkin_create_pkg rosserial_controller
$ cd src/rosserial_controller
$ vim rosserial_controller.launch
Напишем launch файл такого содержания:
Скомпилируем и запустим его:
$ cd ~/
$ 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 с помощью нажатия клавиш на клавиатуре.
