Документация: ROS2 пакет ears_driver

Назначение

Пакет ears_driver управляет двумя “ушами” робоголовы (левое и правое), каждое ухо двигается отдельным сервоприводом.

Аппаратная часть:

• 2× servo (левое/правое ухо)
• PWM-контроллер PCA9685
• подключение по I2C (/dev/i2c-1), адрес обычно 0x40

Движение к целевым углам выполняется:

• плавно по кубическому полиному при duration > 0 (обновление ~50 Гц),
• мгновенно при duration = 0 (мгновенно = максимально быстро, без профиля разгона/торможения).

---

Структура пакета (типовая)

ears_driver/
├── CMakeLists.txt                  # Файл CMake сборки пакета
├── config
│   └── ears_driver.yaml            # Конфиг-файл
├── launch
│   └── ears_driver.launch.py       # Launch-файл запуска
├── package.xml
├── README.md
└── src
    ├── main.cpp                    # Исходный код ears_driver
    └── servo_pulse_check.cpp       # Исходный код утилиты калибровки импульсов PCA9685

---

Принцип работы

1) PCA9685 и PWM

PCA9685 использует 12-битные значения PWM: 0..4095.
Частота в драйвере выставляется на 50 Гц (период ~20 мс).

Приблизительная оценка длительности импульса:

• 1 count ≈ 20000 / 4096 ≈ 4.8828 мкс
• pulse_us ≈ pulse_count * 4.8828

Примеры:

• 130 → ~635 мкс
• 530 → ~2588 мкс

Именно поэтому параметры servo_pulse_low/high обычно в районе 100..600.

---

2) Преобразование “углы ушей” → “углы сервоприводов”

Пользователь задаёт:

• left (в сервисе это angle_a)
• right (в сервисе это angle_b)

Драйвер переводит их в абсолютные углы сервоприводов (в градусах 0..180) относительно нейтрали 90°:

servo_left_angle  = 90 + left  + servo_1_coef
servo_right_angle = 90 - right - servo_2_coef

Затем каждый угол:

• ограничивается в [0..180],
• переводится в PWM-count линейной интерполяцией между servo_pulse_low и servo_pulse_high.

Положительный угол наклоняет ухо вперёд - в сторону дисплея. Отрицательный угол наклоняет ухо назад - в сторону кнопки включения.

---

3) Плавное движение (duration > 0)

Если duration > 0, используется кубический полином с нулевой скоростью в начале и в конце:

θ(t) = a0 + a2·t² + a3·t³
a0 = θ_current
a2 = 3·(θ_goal − θ_current) / T²
a3 = −2·(θ_goal − θ_current) / T³

Обновление положения выполняется в отдельном потоке с частотой ~50 Гц.

---

ROS2 API

Нода создаётся с именем ears_driver и обычно запускается в namespace ears_driver, поэтому сервисы по умолчанию:

• /ears_driver/ears_set_angle
• /ears_driver/is_idle

1) Сервис ears_set_angle

Тип: robohead_interfaces/srv/Move
Имя: задаётся параметром srv_ears_set_angle_name (по умолчанию ears_set_angle)

Запрос

Поле	Тип	Описание
angle_a	int16	Угол левого уха (left)
angle_b	int16	Угол правого уха (right)
duration	float64	Время движения (сек). 0.0 — мгновенно

Ответ

Поле	Тип	Описание
data	int16	Код результата

Коды:

• 0 — успех
• -1 — angle_a вне диапазона [constraints.l_from, constraints.l_to]
• -2 — angle_b вне диапазона [constraints.r_from, constraints.r_to]
• -3 — duration < 0

Примеры вызова

# Поворот левого уха вперед на 70° и правого назад на 55° за 1.5 секунды
ros2 service call /ears_driver/ears_set_angle robohead_interfaces/srv/Move \
  "{angle_a: 70, angle_b: -55, duration: 1.5}"

# Вернуться в нейтраль за 0.8 сек
ros2 service call /ears_driver/ears_set_angle robohead_interfaces/srv/Move \
  "{angle_a: 0, angle_b: 0, duration: 0.8}"

# Поворот левого уха назад на 90° за 1.5 секунды
ros2 service call /ears_driver/ears_set_angle robohead_interfaces/srv/Move \
  "{angle_a: -90, angle_b: 0, duration: 1.5}"

# Мгновенно (осторожно)
ros2 service call /ears_driver/ears_set_angle robohead_interfaces/srv/Move \
  "{angle_a: 0, angle_b: 0, duration: 0.0}"

---

2) Сервис is_idle

Тип: robohead_interfaces/srv/SimpleCommand
Имя: задаётся параметром srv_is_idle_name (по умолчанию is_idle)

Логика:

• data = 1 — текущие углы равны целевым (нода в режиме ожидания команды)
• data = 0 — движение ещё продолжается

Пример:

# Во время отсутсвия движения вернет 1
ros2 service call /ears_driver/is_idle robohead_interfaces/srv/SimpleCommand "{}"

# Запустите долгое движение (duration=10.0) и успейте повторить запрос is_idle
ros2 service call /ears_driver/ears_set_angle robohead_interfaces/srv/Move \
  "{angle_a: 90, angle_b: -90, duration: 10.0}"
ros2 service call /ears_driver/is_idle robohead_interfaces/srv/SimpleCommand "{}"
# is_idle во время движения должен вернуть 0

---

Конфигурация (ears_driver.yaml)

Файл: ears_driver/config/ears_driver.yaml

Описание параметров

Имена сервисов

Параметр	Тип	По умолчанию	Описание
srv_ears_set_angle_name	string	ears_set_angle	Имя сервиса управления ушами
srv_is_idle_name	string	is_idle	Имя сервиса “занят/свободен”

Начальное положение

Параметр	Тип	По умолчанию	Описание
std_left_angle	int	0	Начальный угол левого уха
std_right_angle	int	0	Начальный угол правого уха

При старте нода выставляет начальные углы (с паузой ~1 сек между сервоприводами).

PCA9685 / I2C

Параметр	Тип	По умолчанию	Описание
i2c_address	int	64	I2C-адрес PCA9685 (обычно 0x40)
servo_1_channel	int	7	Канал PCA9685 для левого уха
servo_2_channel	int	6	Канал PCA9685 для правого уха

Каналы PCA9685: 0..15. На плате RPI-HEAD доступны 0..7.

Калибровка нулей (механика)

Параметр	Тип	Описание
servo_1_coef	int	Поправка (градусы) для левого сервопривода
servo_2_coef	int	Поправка (градусы) для правого сервопривода

Используется для того, чтобы при std_left_angle=0, std_right_angle=0 уши стояли симметрично/ровно.

Диапазон PWM (критично для безопасности)

Параметр	Тип	Описание
servo_pulse_low	int	PWM count для 0°
servo_pulse_high	int	PWM count для 180°

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

Ограничения пользовательских углов

Параметр	Тип	По умолчанию	Описание
constraints.l_from	int	-90	Минимальный угол левого уха
constraints.l_to	int	90	Максимальный угол левого уха
constraints.r_from	int	-90	Минимальный угол правого уха
constraints.r_to	int	90	Максимальный угол правого уха

Ограничения задаются в координатах ушей (left/right), а не в абсолютных 0..180 сервопривода.

---

Сборка и запуск

Сборка

cd ~/robohead_ws
colcon build --symlink-install --packages-select robohead_interfaces ears_driver
source install/setup.bash

Запуск

ros2 launch ears_driver ears_driver.launch.py

Ожидаемый лог:

[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [ears_driver_node-1]: process started with pid [3931]
[ears_driver_node-1] [INFO] [1776438542.730476308] [ears_driver.ears_driver]: INITED

---

Режим отладки

Если система запускает драйверы через systemd (например, robohead.service), для ручного теста:

sudo systemctl stop robohead.service

После этого запускайте ears_driver вручную через ros2 launch ....

---

Утилита калибровки: servo_pulse_check

Внимание: при обычном использовании Робоголовы нет необходимости изменять параметры ниже.

Зачем нужна

servo_pulse_low и servo_pulse_high в конфиге — это “границы” PWM, соответствующие 0° и 180°. Для разных моделей сервоприводов и реальной механики безопасные границы могут отличаться.

servo_pulse_check позволяет:

• выбрать канал PCA9685,
• вручную подавать “сырые” значения PWM count,
• наблюдать, где начинаются упоры/перегрузка,
• подобрать безопасный диапазон и записать его в конфиг.

Важные меры безопасности

Перед калибровкой:

1. Обязательно снимите с сервопривода все тяги и рычаги - он должен свободно вращаться в любом направлении.
2. Начинайте с “середины” (около 400) и двигайтесь малыми шагами (с шагом в 10).
3. При появлении:
   • сильного упора,
   • характерного “жужжания”,
   • заметного нагрева,
   • деформации механики
     немедленно вернитесь назад и уменьшите шаг.

Запуск

ros2 run ears_driver servo_pulse_check

Работа с утилитой

Убедитесь, что пакет ears_driver (и robohead_controller) выключены, иначе утилита не сможет управлять сервоприводами

1. Сначала вводится номер канала PCA9685:

Input servo channel [0..15]:

Например, для левого серво (если он на 4 канале):

Input servo channel [0..15]: 4

В случае корректного ввода утилита выведет

OK. Now input PWM 'off' count [0..4095]. Ctrl+D to exit.
Input pulse:

2. Потом бесконечно вводите значение pulse (0..4095):

Input pulse: 400

Утилита будет отвечать реально установленным значением.

Set channel=4 pulse=350

Если введенное значение выходит из дапазона 0..4095, то оно будет округлено до ближайшей границы.

Для завершения работы нажмите Ctrl+D на клавиатуре.

Пример работы:

ros2 run ears_driver servo_pulse_check
Input servo channel [0..15]: 4
OK. Now input PWM 'off' count [0..4095]. Ctrl+D to exit.
Input pulse: 400
Set channel=4 pulse=400
Input pulse: 350
Set channel=4 pulse=350
Input pulse: 450
Set channel=4 pulse=450
Input pulse: -100
Set channel=4 pulse=0
Input pulse: 10000
Set channel=4 pulse=4095
Input pulse: 400
Set channel=4 pulse=400
Input pulse: 
Exit.

Рекомендуемая процедура подбора servo_pulse_low и servo_pulse_high

1. Выберите канал одного серво.
2. Первой командой поставьте безопасную середину, например 400.
3. Уменьшайте pulse шагом 5–10, пока:
   • серво ещё уверенно держит,
   • механика серво не упирается,
   • нет сильного жужжания.
   • серво действительно поворачивается. Можно попеременно задавать pulse: 400, 350, 400, 330, 400, 300, 400, 280, ... - так даже при небольшом шаге вы будете видеть, что поворот действительно совершается.
4. Зафиксируйте найденную минимальную безопасную границу как кандидата для servo_pulse_low.
5. Вернитесь к середине и увеличивайте pulse, аналогично найдите максимум → кандидат для servo_pulse_high.
6. Повторите для всех сервоприводов.
7. Возьмите общие границы (обычно выбирают более “узкий” диапазон. Пример: у серво 1 получился диапазон 112...560, у серво 2 - 107...547. Тогда возьмите дипазон 112..547. Рекомендуется уменьшить найденный диапазон на 10-20 пунктов с каждого конца для устойчивой работы: в примере получим 130...530
8. Запишите в ears_driver.yaml:

   servo_pulse_low: 130
   servo_pulse_high: 530

---

Частые проблемы

1) Cannot open I2C device

• Проверьте /dev/i2c-1
• Проверьте включение I2C в системе
• Запустите с sudo или настройте права/группу i2c

2) Cannot set I2C slave address

• Неверный i2c_address
• PCA9685 на другом адресе (проверьте i2cdetect -y 1)

3) Уши “дёргаются”, упираются или нейтраль не совпадает

• Сначала подберите servo_pulse_low/high
• Затем подстройте servo_1_coef/servo_2_coef

4) Перепутаны левое/правое ухо

• Поменяйте servo_1_channel и servo_2_channel в конфиге или физическое подключение к PCA9685.

---