Robossembler Teleoperation Library

Мотивация

• Объединить разные устройства ввода (геймпад, VR/IMU, клавиатура, лидер‑манипулятор) под единой абстракцией и единым набором топиков управления.
• Дать модульную архитектуру: провайдеры управления подключаются плагинами (pluginlib) и могут отдавать Twist, JointTrajectory, команды хватателя и сервисные команды.
• Стабильно работать локально и через сеть. Поддерживается ROS 2 поверх Zenoh RMW для удалённого телеуправления.

См. «Инструкция по Zenoh» — zenoh_run.md

Поддерживаемые устройства и их статус

• Геймпад (DualShock 4): стабильная поддержка. На вход — sensor_msgs/Joy (любая нода драйвера), маппинг осей/кнопок задан статически.
• Клавиатура: базовая поддержка через топики keyboard_twist и keyboard_gripper (нужен внешний паблишер). Для команд записи есть узел keyboard_adapter.py.
• VR/IMU: экспериментальная поддержка. Из IMU интегрируется ориентация и компенсируется гравитация, на выходе — Twist.
• Лидер‑манипулятор GELLO (Dynamixel): плагин‑провайдер, читает углы лидера и строит JointTrajectory для ведомого по URDF‑ограничениям; публикует gello/joint_states. Статус: beta.
• Лидер‑манипулятор Minipulator (AS5600 + PCA9548A прошивка): плагин‑драйвер, считывающий магнитные энкодеры по сериалу, публикующий /minipulator/joint_states и доступный через rbs_teleop_provider/MinipulatorProvider.
• RViz панель: отображает сравнение позиций сочленений «Лидер vs Ведомый» и позволяет менять знаки направлений.

Сводка пакетов

• rbs_teleop_interface: интерфейсы провайдеров (Twist/Pose/Gripper/Recorder/JointTrajectory) + утилиты Publisher/Subscription.
• rbs_teleop_provider: встроенные провайдеры — keyboard, joy (с маппингами), vr, фабрика ProviderFactory и инфраструктура маппинга геймпадов.
• gello_provider: плагин‑провайдер для лидерского манипулятора GELLO (Dynamixel) + простой планировщик траекторий по URDF‑лимитам (Ruckig, joint_limits, URDF); фоновая публикация /gello/joint_states при неактивном контроллере; калибровка знаков/смещений с сохранением в parameters_file.
• minipulator_provider: плагин‑провайдер для Minipulator (AS5600 энкодеры через прошивку minipulator_provider/pio), драйвер читает сериал‑фреймы R,<t>,encoders, публикует /minipulator/joint_states и предоставляет те же сервисы калибровки/знаков.
• virtual_frame_controller: Lifecycle‑нода мостит провайдеры в контроллеры робота: публикует Twist/Pose/JointTrajectory, вызывает action хватателя, управляет сервисом рекордера; умеет работать в базовой или ИМ рамке.
• rbs_rviz_plugins: RViz дисплей и панель калибровки/наблюдения «Leader/Follower» по JointState с прогресс‑барами и инверсией направлений.
• repos/: .repos файл с внешними зависимостями для vcs import.

rbs_rviz_panel (панель RViz)

• Дисплей RbsTeleop (пакет rbs_rviz_plugins) добавляет док‑панель для наблюдения и калибровки.
• Свойства дисплея:
  • URDF topic: /robot_description (latched std_msgs/String с URDF).
  • Leader JointState: например /gello/joint_states или /minipulator/joint_states.
  • Follower JointState: /joint_states.
• Панель показывает: значения суставов лидера/ведомого, прогресс в пределах лимитов и колонку Direction для инверсии знаков.
• Кнопки:
  • Link/Unlink — активировать/деактивировать Lifecycle‑узел virtual_frame_controller через /<ns>/virtual_frame_controller/change_state.
  • Calibrate — сервис /<ns>/(gello|minipulator)/calibrate: вычисляет joint_signs и best_offsets, сохраняет в parameters_file (или ~/.ros/(gello|minipulator)_offsets.yaml).
• Смена знаков: публикует список в /<ns>/(gello|minipulator)/signs_cmd (std_msgs/Int64MultiArray) и вызывает /<ns>/(gello|minipulator)/reload_signs; начальное чтение через /<ns>/(gello|minipulator)/get_signs (Trigger, строка [1,-1,...]).

Примечание: при деактивации контроллера (VirtualFrameController inactive) лидерские JointState продолжают обновляться для удобной калибровки.

Добавление новых устройств

Руководство: docs/ADDING_DEVICES.md

Зависимости

• ROS 2 (рекомендуется Jazzy) + базовые пакеты: rclcpp, rclcpp_lifecycle, pluginlib, geometry_msgs, sensor_msgs, trajectory_msgs, control_msgs, tf2, tf2_ros, tf2_geometry_msgs.
• Для геймпада: любой драйвер, публикующий sensor_msgs/Joy.
• Для RViz панели: rviz2, rviz_common, rviz_default_plugins.
• Для GELLO: dynamixel_sdk, joint_limits, urdfdom, ruckig, generate_parameter_library.
• Внешние интерфейсы: rbs_utils_interfaces и утилиты из Robossembler Runtime (см. repos/aubo.repos).
• Опционально для сети: rmw_zenoh_cpp (см. zenoh_run.md).

Установка

1. Клонируйте репозиторий в src вашего ROS 2 workspace:

mkdir -p ~/ws/src && cd ~/ws/src
git clone <this_repo_url> rbs_teleop

2. Подтяните внешние зависимости через VCS (при необходимости):

vcs import . < rbs_teleop/repos/aubo.repos

3. Установите системные зависимости и зависимости ROS:

rosdep install --from-paths . -iry

4. Соберите workspace:

cd ~/ws
colcon build --symlink-install
source install/setup.bash

Базовая настройка

Минимальный пример параметров для геймпада (DualShock 4) с публикацией Twist:

virtual_frame_controller:
  ros__parameters:
    provider_type: joy
    controller_type: dualshock4
    base_link: base_link
    ee_link: tool0
    use_ee_frame: false
    twist_gain_array: [6.0, 6.0, 6.0, 10.0, 10.0, 10.0]
    twist_topic: cartesian_twist_controller/target_twist
    pose_topic: cartesian_motion_controller/target_frame
    use_gripper: true
    gripper_open_length: 0.025
    gripper_joint_names: ["right_finger_joint"]

Пример для GELLO, построения JointTrajectory и публикации gello/joint_states:

virtual_frame_controller:
  ros__parameters:
    provider_type: "rbs_teleop_provider/GelloProvider"
    joint_trajectory_topic: joint_trajectory_controller/joint_trajectory
    gello_provider:
      joint_names: [shoulder_joint, upperArm_joint, foreArm_joint, wrist1_joint, wrist2_joint, wrist3_joint]
      topic_name: "/gello/joint_states"
      # com_port: "/dev/serial/by-id/..."    # опционально, авто‑детект
      # joint_signs: [1,1,1,1,1,1]
      # best_offsets: [0,0,0,0,0,0]

Пример для Minipulator (AS5600), использующего сериал‑драйвер:

virtual_frame_controller:
  ros__parameters:
    provider_type: "rbs_teleop_provider/MinipulatorProvider"
    joint_trajectory_topic: joint_trajectory_controller/joint_trajectory
    minipulator_provider:
      joint_names: [shoulder_joint, upperArm_joint, foreArm_joint, wrist1_joint, wrist2_joint, wrist3_joint, right_finger_joint]
      topic_name: "/minipulator/joint_states"
      gripper_joint: "right_finger_joint"
      publisher_topic: "/minipulator/joint_states"
      baud_rate: 115200
      read_timeout_ms: 100
      # com_port: "/dev/ttyACM0"            # авто‑детект при пустой строке
      # joint_signs/best_offsets задаются калибровкой

Полный перечень параметров: docs/SETTINGS.md

Все настройки

Полная справка по параметрам и топикам: docs/SETTINGS.md

Запуск и использование

• Локально с геймпадом:
  • Запустите драйвер геймпада, публикующий sensor_msgs/Joy (например, joy/joy_linux).
  • Запустите контроллер:

    ros2 run virtual_frame_controller vfc_node \
      --ros-args \
      -p provider_type:=joy -p controller_type:=dualshock4 \
      -p base_link:=base_link -p ee_link:=tool0 -p use_ee_frame:=false
    

• С плагинами лидерских манипуляторов и демонстрацией (launch):

  ros2 launch virtual_frame_controller virtual_frame_controller.launch.py \
    provider_type:=rbs_teleop_provider/GelloProvider \
    parameters_file:=$(ros2 pkg prefix virtual_frame_controller)/share/virtual_frame_controller/config/parameters.yaml
  

  ros2 launch virtual_frame_controller virtual_frame_controller.launch.py \
    provider_type:=rbs_teleop_provider/MinipulatorProvider \
    parameters_file:=$(ros2 pkg prefix virtual_frame_controller)/share/virtual_frame_controller/config/parameters.yaml
  

• RViz панель:
  • Откройте rviz2, добавьте дисплей «TeleopDisplay» (пакет rbs_rviz_plugins).
  • Выберите топики: robot_description, Leader JointState (например, /gello/joint_states) и Follower JointState (/joint_states).
• Удалённое телеуправление через Zenoh: см. zenoh_run.md