Это последняя четвертая статья из запланированных в серии. В данной статье я расскажу об использовании джойстика PS3 Dualshock для управления роботом посредством ROS на примере управления простым серво, который использовался в туториале для rosserial_arduino. Кому интересно, прошу под кат.

Установка поддержки джойстика PS3 (Dualshock)

Нам понадобится сам джойстик PS3 Dualshock



и Bluetooth адаптер, например такой



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



Нам понадобится скрипт RetroPie-Setup для установки аппаратной поддержки джойстика Dualshock на Raspberry Pi.
Скачаем его отсюда и установим:

git clone https://github.com/RetroPie/RetroPie-Setup.git
cd RetroPie-Setup/
sudo ./retropie_setup.sh

Сначала выбираем Update ReproPie-Setup.
Затем снова запускаем скрипт retropie_setup.sh. Выбираем Setup / Configuration > 310 Install/Pair PS3 controller. Следуем инструкциям, устанавливаем.
После окончания установки нужно будет подключить джойстик к USB порту и нажать OK. После этого нужно отсоединить джойстик от USB порта и нажать круглую кнопку PS. Будет установлено соединение по Bluetooth. Теперь джойстик спарен и готов к использованию.
Протестировать джойстик можно с помощью графической утилиты jstest-gtk:

sudo apt-get install jstest-gtk.

Запустим утилиту:

jstest-gtk

Интерфейс программы очень простой.



Попробуйте понажимать кнопки и вы должны мгновенно увидеть как начинают меняться прогрессбары возле соответствующих названий осей для кнопок «bumper» или появляется подсветка кнопок при нажатии кнопки.
После нажатия кнопок вы увидите следующую картинку



После нажатия кнопок «bumper» вы увидите следующую картинку



Чтобы использовать джойстик в ROS нужно установить соответствующие пакеты:

rosinstall_generator ros_comm joystick_drivers image_transport --rosdistro indigo --deps --wet-only --exclude roslisp --tar > indigo-custom_ros.rosinstall
wstool update -t src
rosdep install --from-paths src --ignore-src --rosdistro indigo -y -r --os=debian:jessie
sudo ./src/catkin/bin/catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release --install-space /opt/ros/indigo

Стек joystick_drivers включает все необходимые узлы и драйвера. Первичная цель этого стека — конвертировать события джойстика в сообщения ROS.
В частности нам нужен будет узел joy. Детальное описание его можно найти на странице вместе с туториалами здесь и здесь.

Использование джойстика из ROS

Для начала запустим узел joy:

rosrun joy joy_node

Подключим джойстик, нажмем круглую кнопку включения PS и выполним:

rostopic echo joy

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

---
axes: (0.0, 0.0, 0.0, 0.0)
buttons: (0, 0, 0, 0, 0)
---
axes: (0.0, 0.0, 0.0, 0.12372203916311264)
buttons: (0, 0, 0, 0, 0)
---
axes: (0.0, 0.0, -0.18555253744125366, 0.12372203916311264)
buttons: (0, 0, 0, 0, 0)
---

Сообщения, публикуемые узлом joy, имеют тип sensor_msgs/Joy и включают линейную и угловую скорость. Значения скоростей включены в поле axes: первое значение определяет угловую скорость, второе — линейную.
Можно посмотреть структуру сообщений Joy с помощью команды:

rosmsg show sensor_msgs/Joy

Команда покажет следующую структуру:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
float32[] axes
int32[] buttons

Мы создадим скетч для управления серво. Создаем скетч. Для управления сервой нам будет нужна библиотека Arduino Servo. Подключение серво к Arduino можно посмотреть в туториале.
Пишем скетч. Для начала добавим заголовочный файл для сообщений sensor_msgs/Joy вверху файла:

#include <sensor_msgs/Joy.h>

Добавим заголовочные файлы для библиотеки Servo и для ROS:

#include <Servo.h>
#include <ros.h>

Далее следует стандартный код для скетча rosserial_arduino:

ros::NodeHandle nh;

Servo servo;

// Indices of components in joy message
int linear_ind = 1;
int angular_ind = 0;

void servo_cb(const sensor_msgs::Joy& joy)
{
  float angular = joy.axes[angular_ind];
  float linear = joy.axes[linear_ind];
  int cur_pos = servo.read();
  
  if(angular > 0)
  { 
    nh.loginfo("Turn left");
    cur_pos = cur_pos - 20;
    servo.write(cur_pos);
  }
  else if(angular < 0)
  { 
    nh.loginfo("Turn right");
    cur_pos = cur_pos + 20;
    servo.write(cur_pos);
  }

}


ros::Subscriber<sensor_msgs::Joy> sub("joy", &servo_cb);

void setup()
{
  pinMode(13, OUTPUT);
  
  nh.initNode();
  nh.subscribe(sub);
  
  servo.attach(9); // attach it to pin 9
}

void loop()
{
  nh.spinOnce();
  delay(1);
}

В методе setup привязываем серво к пину 9 и проводим стандартную инициализацию узла rosserial_arduino.
В скетче следует обратить внимание на строки:

// Indices of components in joy message
int linear_ind = 1;
int angular_ind = 0;

, где мы указываем индексы для значений угловой и линейной скорости в массиве axes в сообщении sensor_msgs/Joy. Мы просто создаем подписчика на топик joy и в методе servo_cb обрабатываем полученное сообщение. Здесь мы извлекаем значения угловой и линейной скорости. В данном скетче мы используем только угловую скорость, поскольку определяем поворот сервы.
Направления поворота определяется на основе соотношения кнопок джойстика и направления осей. Пример для двух главных кнопок «bumper» можно увидеть на изображении со страницы.



Здесь указывается что ось 16 задает поворот (левое нажатие — положительное значение, правое — отрицательное). В соответствии с этим правилом в скетче определяется поворот влево — вправо (угловая скорость — поле angular).
Ось 17 задает движение вперед — назад: вперед — положительное, назад — отрицательное. Значения этой оси соответствуют линейной скорости (поле linear):

float linear = joy.axes[linear_ind];

Используя монтировку для камеры можно примонтировать и управлять камерой Raspberry Pi Camera Board с помощью поворота сервы:



Таким образом можно написать логику контроллера для мобильного робота, определив контрольные действия определенным кнопкам джойстика (управление кнопкой «bumper» с помощью наклона или кнопками вверх-вниз-влево-вправо).

Как видите, можно найти множество применений джойстику для управления роботом. Все это возможно благодаря тому, что ROS предоставляет удобный интерфейс для подключения джойстика PS3 Dualshock к роботу на платформе Raspberry Pi и позволяет очень просто реализовывать логику управления различными актюаторами робота типа серво.

Желаю всем удачи в робототехнических проектах с использованием ROS и Raspberry Pi!