Добрый день, уважаемые читатели Хабра!

Я продолжаю серию статей о практическом использовании ROS на Raspberry Pi (первая статья, вторая статья).
В данной статье мы будем использовать пакет teleop_twist_keyboard из стека ros-teleop для управления роботом с помощью нажатия клавиш на клавиатуре. Стек ros-teleop включает помимо данного пакета еще несколько пакетов для различных способов управления, например с помощью джойстика. Кто готов приступить к изучению teleop_twist_keyboard, прошу под кат.

Мультитерминал с tmux

Между делом, я вам хочу рассказать об одном приеме, который позволяет работать удаленно на Raspberry Pi через SSH одновременно в нескольких терминалах. Для этого на RPi нужно установить утилиту tmux.

$ sudo apt-get install tmux

После этого запускаем утилиту:

$ tmux

Внизу окна терминала должна появиться зеленая полоса с номером окна — 0.
tmux — это очень удобный менеджер терминальных окон, который позволяет создать в одном окне терминала любое число окон, размещая их несколькими различными способами (отдельное окно-терминал (window), панель окна (pane)) и удобно переключаться между ними.

Нажмите на клавиатуре Ctrl + B и C. Должно открыться еще одно окно с номером 1. Также попробуйте комбинацию Ctrl + B и %. Текущее окно терминала будет разделено посередине вертикальной зеленой полосой на два окна (pane). Если нажать комбинацию Ctrl + B,: и ввести “split window”, то окно будет разделено горизонтально на два одинаковых окна. Чтобы удалить панель (pane) воспользуйтесь комбинацией Ctrl + B, X и затем нажмите Y. Чтобы переключиться на другую панель в том же окне, используйте комбинацию Ctrl + B, O. Чтобы переключиться между окнами-терминалами по номеру окна, используйте комбинацию Ctrl + B, <номер окна>.

Теперь добавим запуск программы в файл ~/.bashrc для автоматического запуска при открытии нового терминала. Добавьте следующие строки в файл:

[[ $TERM != "screen" ]] && exec tmux

Работа с пакетом teleop_twist_keyboard

Теперь познакомимся с пакетом teleop_twist_keyboard.
Запустим скрипт teleop_twist_keyboard.py из пакета teleop_twist_keyboard как обычный узел ROS:

$ roscore
$ rosrun teleop_twist_keyboard teleop_twist_keyboard.py

Мы получим вывод такого вида:

Reading from the keyboard  and Publishing to Twist!
---------------------------
Moving around:
   u    i    o
   j    k    l
   m    ,    .

q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
anything else : stop

CTRL-C to quit

Выведем список всех активных на данный момент топиков:

$ rostopic list

Топик /cmd_vel должен появиться в списке. На этот топик узел teleop_twist_keyboard публикует сообщения каждый раз, когда была нажата клавиша на клавиатуре.
Покажем вывод сообщений, публикуемых в топик /cmd_vel:

$ rostopic echo cmd_vel

Запустим rqt_graph для представления вычислительного графа ROS в графическом виде. Схема вычислительного графа показывается все активные узлы и топики, которые их связывают.

$ rosrun rqt_graph rqt_graph

Здесь мы видим, что узел teleop_twist_keyboard публикует собщения в топик /cmd_vel, а узел rostopic подписывается на этот топик (команда rostopic echo).
Давайте узнаем какого типа сообщения публикуются на топик /cmd_vel:

$ rostopic type /cmd_vel

Команда выведет строку:

geometry_msgs/Twist

Это означает, что сообщения имеют тип Twist из стандартного ROS пакета geometry_msgs.
Мы также можем получить информацию о структуре сообщения командой rosmsg:

$ rosmsg info geometry_msgs/Twist

geometry_msgs/Vector3 linear
  float64 x
  float64 y
  float64 z
geometry_msgs/Vector3 angular
  float64 x
  float64 y
  float64 z

Поле ‘linear’ отвечает за линейную скорость, ‘angular’ — за угловую скорость.
Нажмите клавишу ‘i’, вывод будет таким (ассоциируется с движением вперед):

linear: 
  x: 0.5
  y: 0.0
  z: 0.0
angular: 
  x: 0.0
  y: 0.0
  z: 0.0
---

Нажмите клавишу ‘k’, вывод будет таким (остановка):

linear: 
  x: 0.0
  y: 0.0
  z: 0.0
angular: 
  x: 0.0
  y: 0.0
  z: 0.0
---

Нажмите клавишу ‘u’, вывод будет таким (поворот влево):

linear: 
  x: 0.5
  y: 0.0
  z: 0.0
angular: 
  x: 0.0
  y: 0.0
  z: 1.0
---

И наконец при нажатии на клавишу ‘o’ получим такой вывод (поворачиваем вправо):

linear: 
  x: 0.5
  y: 0.0
  z: 0.0
angular: 
  x: 0.0
  y: 0.0
  z: -1.0
---

Клавиши ‘j’ и ‘l’ отвечают за повороты влево и вправо на месте (без движения вперед).

Управление роботом с клавиатуры с teleop_twist_keyboard

У меня уже написан скетч rosserial для управления роботом посредством нажатия клавиш. Мы просто подписываемся на топик /cmd_vel и в зависимости от получаемого значения каждой из скоростей отдаем необходимую команду контроллеру движения (двигаться вперед, остановиться, повернуть влево или вправо). Скетч можно скачать отсюда.
Разберем подробнее код скетча.
В начале файла подключаем помимо стандартного заголовочного файла ros.h два дополнительных файла с типами сообщений geometry_msgs/Twist.h и geometry_msgs/Vector3.h:

#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Vector3.h>

Мы объявляем обработчик узла ros::NodeHandle:

ros::NodeHandle nh;

Основное значение имеет метод messageCb:

void messageCb(const geometry_msgs::Twist& message) 
{
  geometry_msgs::Vector3 linear = message.linear;
  float forward_vel = float(linear.x);
  
  if(forward_vel == 0) { stop(); return; }
  
  geometry_msgs::Vector3 angular = message.angular;
  float ang_vel = float(angular.z);
  
  if(ang_vel > 0) { turnLeft(); }
  else if(ang_vel < 0) { turnRight(); }
  else { goForward(); }
}

В этом методе мы производим обработку получаемого сообщения из топика cmd_vel. В переменной forward_vel мы сохраняем линейную скорость, в переменной angular — угловую. Линейная скорость нам позволяет отследить команду остановки (значение 0). Угловая скорость определяет направление поворота (если больше 0, то поворачиваем влево, меньше 0 — вправо, если 0 — двигаемся вперед).
Создаем подписчика на топик /cmd_vel:

ros::Subscriber<geometry_msgs::Twist> sub("/cmd_vel", &messageCb);

со указанием ссылки на метод обработки сообщений (messageCb) и типа принимаемого сообщения — geometry_msgs::Twist.
В конце скрипта следуют стандартные методы скетча для rosserial_arduino:

nh.initNode();
nh.subscribe(sub);
Serial.begin(57600);

Запустите rosserial_arduino сервер:

$ rosrun rosserial_python serial_node _port:=/dev/ttyACM0

и загрузите скетч на плату Arduino.

Переключитесь на терминал, где запущен узел teleop_twist_keyboard, попробуйте нажатие клавиш ‘u’, ‘i’, ‘o’ и ‘k’ и проверьте в терминале вывод сервера rosserial.

Таким образом, используя пакет teleop_twist_keyboard мы теперь можем выполнять простое удаленное управление роботом передавая команды движения: двигаться вперед, остановиться, повернуть влево или вправо. В дальнейшем мы научимся управлять роботом с помощью джойстика, что значительно удобнее, с применением уже другого пакета в ROS. Но об этом в следующей статье.