Задатчик FollowJointState

Задатчик получает sensor_msg::JointState от высшего уровня и передает его агрегатору. Возможно сглаживание движений. Основной вариант использования --- управление роботом при помощи joint_state_publisher.

Входные порты

  1. sync (TimerEvent, EventPort) --- синхронизация таймера.

  2. in_joints_sorted (JointState) --- состояние робота в угловой СК (желаемое от агрегатора или реальное по датчикам), сообщение отсортировано по кинематическим цепочкам.

  3. in_joints_ref (JointState) --- входной порт для желаемой позы от верхнего уровня, произвольный порядок звеньев.

Выходные порты

  1. out_joints_ref (JointState) --- задающее воздействие в угловой системе координат.

  2. out_joints_src_reset (JointState) --- текущая полная поза. Она передается верхнему уровню в течении activation_delay после активации задатчика. Таким образом источник, формирующий JointState (например, GUI joint_state_publisher) может подстроится под текущую позу до того, как его сообщения будут переданы в систему уравления движением.

Параметры

  1. controlled_chains (strings) --- набор ресурсов, управляемых компонентом.
  2. activation_delay (double) --- задержка активации. В течение этого времени верхний уровень может подстроиться к текущей позе робота.
  3. stay_operational (bool) --- задатчик остается в активном состоянии, даже если обладает не всеми запрошенными ресурсами.

Операции

ROS-совместимые: 1. Предоставляет: rosSetOperational (std_srv::SetBool/sweetie_bot_resource_control_msg::SetOperational) --- управление компонентом из ROS.

Сервисы

  1. Требует: сервис robot_model.
  2. Требует (опционально): сервис сглаживания траекторий с интерфейсом TransientJointStateInterface.

Семантика исполнения

Вледение ресурсами: компонент запрашивает ресурсы заданные в параметре controlled_chains. Если установлен флаг stay_operational он остается активен, если обладает любым подмножеством запрошенных ресурсов (в том числе и пустым). Прагматика такого решения следующая: т.к. задатчик всегда активен, арбитр возвращает ему отнятые ресурсы, когда они снова освобождаются. Таким образом задатчик может играть роль фонового процесса, управляющего конечностями по умолчанию. В совокупности с GUI joint_state_publisher это дает возможность ручного посуставного управления.

Исполнение. В любых ситуациях формируемое задающее воздействие содержит полный список суставов, соответствующих ресурсам, которыми управляет компонент. Если с момента активации прошло меньше activation_delay секунд, то публикует позу c in_joints_sorted (текущая поза робота) на out_joints_src_reset. В качестве задающей позы используется текущая поза робота с обнуленной скоростью. Если с момента активации прошло больше activation_delay секунд, то в качестве задающей позы используется сообщение, приходящее на in_joints_ref. Из него удаляются все приводы, которыми компонент не управет. В качестве задающего воздействия по умолчанию используется текущая поза с обнуленной скоростью. * Перед передачей задающего воздействие оно сглаживается с использованием TransientJointStateInterface плагина сглаживания.

Детали реализации

По списку ресурсов удобно составить map, позволяющую быстро по имени находить положение сустава в joints_sorted и out_joints_ref.

joint_state_publisher может реагировать на получаемую с out_joints_src_reset сообщения с существенной задержкой (1-2 секунды). Рекомендуется устанавливать activation_delay достаточной длительности.

Ошибки и исключения

Ошибки: 1. Несуществующий ресурс.