Компонент `agregator`

Минимальная реализация агрегатора. Компонент буферизует входящие сообщение с частичной позой в угловой системе координат, что позволяет сформировать отсортированную позу.

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

in_joints (JointState, EventPort) --- состояние отдельных элементов робота в угловой системе координат (первичная инициализация).
in_support (SupportState) --- опорные коэффициенты.

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

out_joints_sorted (JointState) --- состояние робота в угловой системе координат, суставы отсортированы по цепочкам.
out_support (SupportState) --- опорные коэффициенты.

Параметры

Плагины

Требует: robot_model --- URDF модель робота и его кинематических цепочек RobotModelURDF. Определяет порядок суставов в отсортированной JointState.

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

Конфигурация. По модели робота и набору кинематических цепочек создать переменные для хранения позы (JointState) и средства быстрого доступа к ним для осуществления эффективной сортировки.

Получение сообщения. Обновление буфера с отсортированным JointState посредством полученного сообщения. Входной сообщение проверяется на корректность: неизвестные суставы, наличие одновременно скорости и позиции. Новая поза публикуется через порт joints_sorted. Также обновляется и публикуется SupportState.

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

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

Вероятно, для ускорения поиска по имени сочленения следует использовать std::unsorted_map. (Для приводов и для цепочек и индексами или ссылками нужных структур и элементов: сквозной номер привода, номер цепочки, номер привода в цепочке).

Буфер логичнее строить на JointState, поскольку могут быть приводы не принадлежащие ни одной цепочке.

Входной порт должен читаться в коде компонента в цикле, пока не будет получено OldValue.

Варианты использования

Перед использованием требуется первичная инициализация позы.

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

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

Предупреждения: 2. Выход за лимиты. 1. Незнакомый привод, цепочка.

Ошибки: 1. Целостность входных данных нарушена: несовпадение размеров массивов с требуемым значением.

Компонент `agregator_odom`

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

-->

<!--* Если в наличии скорость и позиция,  сбросить фильтры, присвоить буферным переменным новые значения.-->
<!--* Если в наличии скорость или позиция, применить соответствующий фильтр, сбросить другой, присвоить буферам новые значения.-->
<!--Сообщения сохраняются в буфер `JointState` в порядке установленном `robot_model`.-->

Компонент agregator