Робот -тележка на ROS.Часть 2. Cофт

Посты серии:
8. Управляем с телефона-ROS Control, GPS-нода
7. Локализация робота: gmapping, AMCL, реперные точки на карте помещения
6. Одометрия с энкодеров колес, карта помещения, лидар
5. Работаем в rviz и gazebo: xacro, новые сенсоры.
4. Создаем симуляцию робота, используя редакторы rviz и gazebo.
3. Ускоряемся, меняем камеру, исправляем походку
2. Софт
1. Железо

Продвигаемся к улыбке

Собрав «бургер» по схеме из прошлого поста, перейдем к программному наполнению.

Так как мы собираем по уже готовому проекту, логично привести инструкции, в нем указанные. Они находятся здесь.

Все очень удобно и там же можно скачать уже готовый образ с Raspbian Stretch + ROS + OpenCV, записать его на sd карту для raspberry. (ROS Kinetic, OpenCV 3.4.1. Да, есть и поновее, но иногда лучше взять и поехать, чем собирать все самому из исходников).

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

• нет GUI (графического интерфейса). Это не критично, тем более для ROS, но для езды по линии надо калибровать камеру на самой raspberry, и видеть как она (камера) передает цвета (об этом ниже);
• сборка OpenCV не выводит изображение на экран, даже если самостоятельно поставить GUI. Очевидно в проекте rosbots opencv собиралась без этой опции.
• нет мелких костылей (VNC, текстового редактора для записей, mc).
• Поэтому, OpenCV был пересобран с поддержкой вывода изображения в GUI (собрана openCV 3.4.3), установлен GUI, мелкие костыли.

Готовый образ здесь, и дальше работа будет строиться на его основе.

Настроим сеть (wi-fi) и ROS-master на raspberry pi

.
Настоятельно рекомендую для опытов использовать отдельный роутер со своим wi-fi. Можно для этих целей просто создать точку доступа на телефоне. Это связано с тем, что по wi-fi будет летать достаточно много пакетов и, желательно, чтобы они не тонули в общем трафике.

После заливки образа на sd карту raspberry, настроим сеть. Первоначальные настройки сети такие:

interface wlan0
static ip_address=192.168.43.174/24
static routers=192.168.43.1
static domain_name_servers=192.168.43.1

Содержатся в /etc/dhcpcd.conf

Поэтому можно не подключать шланги к raspberry, чтобы все поменять, а просто создать точку доступа с пафосным названием boss и паролем 1234554321. Адрес raspberry будет 192.168.43.174. Так же на этот адрес можно кроме ssh зайти по VNC: логин — pi, пароль — 123qweasdzxcV.

Настроим ROS-мастер

Небольшая ремарка для тех, кто не сталкивался с ROS (robotic operation system). ROS-мастер — это посредник, через который в ros общаются разные узлы (ноды, сервисы и т.п.) Если ros мастер не запущен или запущен не по тому адресу, ноды друг друга не увидят.

В нашей системе ROS мастер стартует автоматически с загрузкой ОС и все, что от нас требуется, указать ip-адрес для ROS-мастера в соответствующем файле системы.

Если вы настройки сети не меняли, которые указананы выше, то и настраивать ничего не требуется.

В противном случае правим bashrc:

nano ~/.bashrc

В самом конце файла исправьте ip адреса (оба) на ваш случай:

export ROS_MASTER_URI=http://192.168.43.174:11311
export ROS_HOSTNAME=192.168.43.174

Перезагрузитесь.

Теперь при запуске терминала на тележке, вывод будет таким (либо тем, что вы указали в настройках):

For all slaves, "export ROS_MASTER_URI=http://192.168.43.174:11311"

Это означает, что ROS мастер работает по указанному ip адресу.

Управляем тележкой по wi-fi

С начала проверим, что у нас работают ноды.

В терминале:

rosnode list

Вывод будет таким:

/rosout
/uno_serial_node

Если ничего не вывело, то проверьте прописали ли вы ROS-master в настройках как указано выше, подключили ли шланг usb к arduino, перезагрузились.

После проверки запустим 1-ю ноду, отвечающую за движение:

rosrun rosbots_driver part2_cmr.py 

*специальная ros команда запускает из пакета rosbots_driver python файл part2_cmr.py

Система сообщит, что нода запущена:



Здесь видно, что определен радиус колес и расстояние между ними. Исправить эти значения, как и иные, связанные с движением можно в файле robot.py по пути

/home/pi/rosbots_catkin_ws/src/rosbots_driver/scripts/examples/coursera_control_of_mobile_robots/part2/full/controller

так как в самом part2_cmr.py этих параметров нет. Откроем второй терминал и введем rostopic list:



Здесь видно, что появился топик /part2_cmr/cmd_vel. В этом топике /part2_cmr «слушает», что ему будут говорить другие ноды и, в зависимости от того, что скажут, будет управлять движением. О том, что именно «слушает», а не «говорит» можно понять, используя команду.

rostopic info   /part2_cmr/cmd_vel

Здесь видно, что /part2_cmr subscriber (подписалась) на топик и слушает.

*В топик можно самостоятельно, без нод, что-то «сказать».

Например:

rostopic pub -1 /wheel_power_left std_msgs/Float32 '{data: 1.0}'

повращать вперед левым колесом

rostopic pub -1 /wheel_power_left std_msgs/Float32 '{data: 0.0}'

остановить левое колесо

rostopic pub -1 /wheel_power_left std_msgs/Float32 '{data: -1.0}'

Повращать назад назад колесом

rostopic pub -1 /wheel_power_left std_msgs/Float32 '{data: -0.5}'

Повращать назад левым колесом помедленнее.

Синтаксис такой: rostopic pub — желание говорить в топик, -1 — разовое желание, /wheel_power_left — топик, куда говорим, std_msgs/Float32 — язык (формат сообщений),'{data: -0.5}' — что говорим.

Теперь запустим того, кто будет говорить в топик /part2_cmr/cmd_vel. Это будет нода отправки команд с клавиатуры.

Не закрывая предыдущий терминал с работающий нодой, запустим еще один и введем:

rosrun teleop_twist_keyboard teleop_twist_keyboard.py /cmd_vel:=/part2_cmr/cmd_vel

*Так как публикация по умолчанию ведется в топик /cmd_vel, мы его перенаправляем используя
/cmd_vel:=/part2_cmr/cmd_vel, чтобы сообщения сыпались именно в /part2_cmr/cmd_vel.

Нода управления запустилась и можно поездить, нажимая клавиши на клавиатуре:



Если ехать не получается или идет едва уловимый писк из под колес — надо увеличить скорость, нажимая на «w» в терминале с запущенной нодой. То же самое (увеличить или уменьшить) можно сделать со скоростью поворота — кнопка «e». Важно также находиться в терминале с запущенной нодой, если переключиться в другой терминал кнопки управления работать не будут. Кнопка «k» в терминале управления — это стоп.

В отдельном терминале посмотрим на топик /part2_cmr/cmd_vel:



Теперь в топике /part2_cmr/cmd_vel есть и говорящий, и слушающий.

Едем по линии на OpenCV

Перед тем как куда-то поехать, надо убедиться, что робот ездит при управлении с клавиатуры. Здесь важная ремарка необходима. При управлении с клавиатуры в примере выше, поворот налево должен соответствовать нажатию клавиши j, направо l (латинская л), вперед i, назад ,(запятой). Если в вашем случае это не так, то могут быть проблемы с поездкой. Чтобы все привести в норму, надо на arduino в нашем бургере поменять проводные пары, идущие с драйвера двигателя на ноги 4,5,6,7 arduino: 4,5 поменять местами с 6,7 либо 4 и 5,6 и 7 друг с другом в зависимости, куда будут крутиться колеса. Можно это также сделать программно, поправив код для arduino по пути — /home/pi/gitspace/rosbots_driver/platformio/rosbots_firmware/examples/motor_driver/src/main.cpp

#define M_LEFT_PWM 6
#define M_LEFT_FR 7
#define M_RIGHT_PWM 5
#define M_RIGHT_FR 4

и перезалив его на arduino командой:

upload_firmware ~/gitspace/rosbots_driver/platformio/rosbots_firmware/examples/motor_driver

Поработаем с цветами

Наш садоводческий опыт будет заключаться в том, чтобы выделить линию на полу, по которой поедет робот, определить ее цвет. По умолчанию робот ее не видит. В качестве линии можно использовать либо скотч (желтый) либо изоленту либо что-то иное с характерным цветом и достаточно широкое. *Прозрачный скотч вряд ли подойдет, т.к. его будет сложно выделить на фоне.

Зайдем в папку и запустим скрипт:

cd /home/pi/rosbots_catkin_ws/src/rosbots_driver/scripts/rosbots_driver
python bgr-to-hsv.py

*Внимание! Если вы используете оригинальный образ от rosbots, а не мой, этой программы там нет.

Откроется два окна:



Перед нами диапазоны цветов в HSV. Что такое hsv и почему не rgb, прошу погуглить самостоятельно.

h1,s1,v1 — нижний и h2,s2,v2 — соответственно, верхний диапазон.

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



Линия изоленты непривычно белого цвета, все остальное — черное. Этот результат необходим.
Запишем, запомним цифры HSV диапазонов. Мой случай — 56,155,40 и 136,255,255. Диапазоны HSV будут разными при разной освещенности около камеры робота.

Закроем окна, введя ctrl+c в терминале и внесем HSV диапазоны в файл follow_line_step_hsv.py:

cd /home/pi/rosbots_catkin_ws/src/rosbots_driver/scripts/rosbots_driver
nano follow_line_step_hsv.py

В строках:

lower_yellow = np.array([21,80,160])
upper_yellow = np.array([255,255,255])

Поставим цифры своих HSV диапазонов.

Время ехать по линии

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

rosrun rosbots_driver part2_cmr.py

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

sudo modprobe bcm2835-v4l2
roslaunch usb_cam usb_cam-test.launch

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

cd /home/pi/rosbots_catkin_ws/src/rosbots_driver/scripts/rosbots_driver
python follow_line_step_hsv.py

Если все пошло удачно, то робот поедет по линии, а также появится дополнительное окно:



В этом окне изолента будет помечаться красным кругом.

Общий смысл кода — выделить цветовой сегмент на определенном расстоянии от камеры, нарисовать красный кружок и ехать к этому кругу, стараясь, чтобы он находился в центре.

Наконец, о важном — о котах и улыбках

Так как наша цель — поехать к коту или к улыбающемуся человеку, то нам придется использовать что-то посложнее в своем коде. Также нам понадобятся коты и улыбающиеся люди. Со вторым сейчас сложнее: мало кто улыбается в это непростое, тревожное время. Поэтому начнем с котов.

Для экспериментов подойдут фото котов в фас.

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

cd /home/pi/rosbots_catkin_ws/src/rosbots_driver/scripts/rosbots_driver
python pi_camera_driver.py

Во 2-м терминале ноду-мотор:

rosrun rosbots_driver part2_cmr.py

В 3-м терминале ноду-поиска кота:

cd /home/pi/rosbots_catkin_ws/src/rosbots_driver/scripts/rosbots_driver
python follow_cat2.py

Тележка понемногу будет двигаться к коту:



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

В 3-м терминале ноду-поиска кота можно закрыть — ctrl+c и вместо ее запустить поиск улыбки на лице малоизвестной публичной личности:

python follow_smile.py

Тележке придется медленно, недоверчиво ехать к улыбке малоизвестной персоны:



Как, возможно, многие уже догадались, в скриптах, которые мы запускали используются каскады Хаара. По тому же принципу, что и с поездкой по линии, выделяется квадрат искомой области и программа пытается удержать его в центре, двигая робота.

К сожалению, производительность на raspberry 3b оставляет желать лучшего, несмотря на настройки камеры 320x240 и 15 Fps. Задержки ощутимы с нарастанием времени. Не каждый кот выдержит.

Как это можно улучшить?

Попробовать пересобрать оптимизированную opencv, как рекомендует Адриан (https://www.pyimagesearch.com/2017/10/09/optimizing-opencv-on-the-raspberry-pi/)? Использовать ресурсы внешнего ПК для обработки изображений? Попробовать не сжимать картинки в jpeg, которые летят в обработчик Хаара? Да и еще один большой минус — коты должны быть большими и в фас. Расстояние 15 см на листе A4. При удалении от камеры, кот уже неузнаваем и неуязвим. Поставить на камеру raspberry монокль с 8x увеличением?

P.S.: Если у вас дойдут руки до экспериментов с образом, который приведен в статье, то можете поездить еще за различными частями тела, соответственно запуская вместо ноды-кота:

python follow_fullbody.py
python follow_upperbody.py
python follow_lowerbody.py

лицом или глазом:

python follow_face.py 
python follow_right_eye.py 

Если есть интерес к тому как плавно трогаться с места, чтобы робот не разливал чай, а также как управлять им не с самой raspberry, напишите.