Интерфейс приводов Herkulex

===========================

Это новая версия на асинхронных операциях, отличется от старой унификацией интерфейсов herkulex_driver и herkulex_sched и переносом функции котроля timeout в планировщик подсистему НиМ.

Требования

Подсистема взаимодействия с приводами должна удовлетворять следующим целям:

  1. Доступ к приводу по имени, преобразования внутренних единиц измерения в единицы СИ.
  2. Настройки приводов: передача параметров конфигурации в память приводов.
  3. Мониторинг: считывание регистров, статус привода, сброс статуса и пр. по требованию пользователя.
  4. Считывание позы при инициализации системы управления, возможность ее считывания в (?)
  5. Взаимодействие в реальном времени: периодическая посылка задания всем или группе приводов, считывание параметров заданной группы приводов. Интерфейсы мониторинга и настройки не должны мешать работе интерфейса реального времени.

Желательно обеспечить максимальное повторное использование кода, поддержку нескольких веток приводов на разных последовательных портах.

Структура системы взаимодействия с приводами

На основе требований выделяется и характера протокола выделяется 4 элемента:

  1. Канальный уровень: прием/посылка и проверка целостности кадров. (herkulex_driver)

  2. Реализация уровня представления (протокол): именование, кодирование/декодирование кадров. Например, преобразование операции назначения/чтения параметра привода в сообщение, преобразование задания на приводы в сообщение, декодирование ответа привода.

    В настоящий момент он реализован в виде программных объектов типа HerkulexServo.

  3. Подсистема реального времени (РВ): передача целевой позиции, считывание состояния (скорость, позиция и т.п.) группы приводов с заданной частотой и минимальной задержкой, кэширование регистров приводов и т.п.

  4. Подсистема настройки и мониторинга (НиМ): назначение произвольного набора параметров, чтение произвольного набора параметров, опрос статусов, считывание состояния группы приводов.

    В настоящий момент он реализован в виде компонента herkulex_array.

Принцип разделения времени. Система управления работает по циклам. В начале каждого цикла подсистеме РВ выделяется интервал времени, в течении которого она осуществляет обмен. По завершения обмена, либо по истечению времени канал передается подсистеме настройки и мониторинга, которая использует оставшийся промежуток времени. За осуществление этого разделения отвечает планировщик. Его логично сделать частью подсистемы РВ из-за тесной связи между ними: цикл планировщика тождественен циклу управления, набор операций РВ за цикл фиксирован.

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

Канальный уровень вынесен в отдельный компонент. Решение относительно спорное, т.к. при использовании синхронных операций ничем не отличается от вызова функций.

В случае нескольких портов каждый из них должен снабжаться своим herkulex_driver и планировщиком.

Альтернативные варианты реализации

Разделение интерфейса РВ и НиМ

Для работы подсистемы РВ и НиМ требуется реализация протокола. Однако вынести реализацию протокола в отдельный компонент затруднительно из-за слишком тесной связи с обоими подсистемами. Возможны реализации:

  1. Один компонент для подсистемы РВ, подсистемы НиМ, протокола и планировщика.
  2. Два компонента с двумя копиями протокола:
  3. Подсистема РВ, планировщик, протокол.
  4. Подсистема НиМ, протокол.
  5. Два компонента с одной копией протокола:
  6. Подсистема РВ, планировщик, требует операции кодирования JOG и запроса состояния, декодировании ответа на запрос состояния.
  7. Подсистема НиМ, протокол, предоставляет операции кодирования JOG и запроса состояния, декодировании ответа на запрос состояния.
  8. Два статически связанных компонента с одной общий копией протокола:
  9. Подсистема НиМ, протокол, в конструкторе создается еще один TaskContext для подсистемы РВ.
  10. Подсистема РВ, планировщик, доступ к протоколу осуществляется через ссылку на его класс.

Первый вариант потребует нескольких нитей (ExecutionEngine) в одном компоненте и их ручной синхронизации. Фактически это будет два компонента в одной обертке.

Второй вариант приводит к двум экземплярам реализации протокола. Это означает, что часть настройки (списки приводов, аппаратные ID, параметры калибровки) должны дублироваться. С другой стороны, могут быть параметры, используемые только в одной конфигурации.

Третий вариант осуществим только с реализацией подсистемы НиМ в виде компонента OROCOS с приоритетом РВ или с версиями операций ClientTread.

Четвертый вариант представляет хороший компромисс. Однако он предполагает специальные требования на методы протокола: потокобезопасность, (Вообще говоря, указатель на протокол можно передать и операцией, если компоненты исполняются в одном адресном пространстве. Но статическое связывание гарантирует такую ситуацию).

Далее описан четвертый вариант.

В любом варианте работу с несколькими серийными портами можно обеспечить дублированием всей инфраструктуры для каждого из них. Значения ServoGoal для незнакомых приводов игнорируются. Однако это решение неудобно тем, что получается два экземпляра herkulex_array.

Альтернатива --- снабжение приводов полем port (или начинать иерархию параметров с имени порта, создавать структуру HerkulexArray для каждого порта), herkulex_array порождает несколько herkulex_sched.

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

Взаимодействие с канальным уровнем

  1. Канальный уровень реализуется в виде библиотеки и становится частью РВ подсистемы. Обращение --- вызовы функций.
  2. Канальный уровень отдельный компонент, взаимодействие синхронное --- операции.
  3. Канальный уровень отдельный компонент, взаимодействие асинхронное --- порты или операции.

В первых двух вариантах обращение к канальному уровню выглядит как вызов функции, блокирующийся до завершения операции. Эта способ хорошо согласуется с текущей реализацией. Использование библиотеки быстрее, но менее универсально.

В данной версии описан вариант с синхронными операциями. Он обеспечивает более понятное разделение функций между модулями: разрешением коллизий путем введения timeout занимается только herkulex_driver. С другой стороны планировщик не обладает ни возможностью управления timeout, ни знанием его значения. Также интерфейсы "канальным уровнь--планировщик" и "подсистема НиМ--планировщик" оказываются разных типов.

Компонент herkulex_driver

Реализация канального уровня: формирование и отправка пакета по интерфейсу.

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

Нет.

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

Порт публикует пакеты, например, для целей мониторинга. Возможность опциональна.

  1. broadcast (HerkulexPacket) --- последней полученный/отправленный пакет.

Параметры

  1. port_name (string) --- имя устройства порта.
  2. baudrate (uint) --- скорость передачи данных.
  3. timeout (double) --- время ожидания ответного пакета, c.

Операции

  1. bool cmdSync(HerkulexPacket& req, HerkulexPacket& ack) (OwnThread) --- послать запрос, дождаться ответа, вернуть результат.
  2. bool cmd(HerkulexPacket& cmd, Empty) (OwnThread) --- послать команду, не ожидать ответа.
  3. uint estimate(uint data_size, bool ack_rek) (ClientTread) --- верхняя оценка времени обмена (опционально).

Замечание: можно реализовать также асинхронный интерфейс request--response, используемый herkulex_sched. Смотрите о нем позже.

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

Конфигурация. Открытие порта.

cmdSync. Синхронный запрос. Запускает таймер. Формирует кадр по HerkulexPacket, посылает его, ожидает целостного кадра-ответа. Возврат по успеху или истечению таймера. Некорректно сформированные данные игнорируются.

Для извещения запрашивающего компонента о ошибках декодирования пакета (таймаут, ошибка контрольной суммы) предлагается использовать те же сообщения HerkulexPacket, дополнив множество значений поля cmd соответствующими кодами CMD_TIMEOUT, CMD_CHECKSUM1, CMD_CHECKSUM2.

cmd. Формирует кадр, посылает его.

Замечание: Компонент не осуществляет контроль за соблюдением политики ACK (следует ли ожидать ответа от привода на данный пакет). Более высокие подсистемы обязаны выбирать правильный вызов.

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

Если не использовать asio, предлагается завести отдельную нить FileDescriptorActivity для чтения. Разбор входа --- стандартный подход на базе конечного автомата. Автомат сбрасывается по breakLoop.

Взаимодействие нитей DataObject или BufferLockFree и условная переменная или светофор.

Исключения и ошибки

Ошибки: 1. Аппаратные ошибки ввода-вывода.

Предупреждения: 2. Некорректный запрос. 3. Некорректный ответ. 3. Лишний или потерянный ответ.

Компонент herkulex_array

Подсистема настройки и мониторинга. Загружает интерфейс РВ и планировщик herkulex_sched.

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

Отсутствуют.

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

  1. joints (JointState) --- позиция и скорость приводов. Опрашивается и публикуется по запросу для целей первичной инициализации.

Параметры

  1. servos --- динамическая структура (PropertyBag) с информацией приводах в системе. Приведены основные опции.
    |- servo1_name
|  |- hw_id (uint) --- идентификатор
|  |- servo_model (string) --- модель привода
|  |- registers_init
|  |   \- register_name (uint) --- значения регистров, инициализируемых при запуске. 
|  |- reverse (bool) --- флаг обращения направления на уровне протокола
|  \- offset (double) --- смещение нуля (на уровне протокола).
\- servo2_name

Открытый вопрос: хранить регистры в uint или преобразовывать в СИ.

  1. tryouts (uint) --- Число попыток обращения к приводу.

Операции

Внешний интерфейс настройки и мониторинга.

(Преимущественно OwnTread, в порядке важности, все не нужны).

  1. strings listRegister(servo)
  2. uint getRegister(servo, reg) --- (что делать с ошибочным именем?)
  3. bool setRegister(servo, reg, val)
  4. printRegisterList(servo)
  5. printRegisters(servo, regs)
  6. printAllRegisters(servo)
  7. getStatus(servo)
  8. clearStatus(servo)
  9. publishState(servos) --- опросить позицию и скорость, опубликовать ее.
  10. writeRegProperties() --- обновить значения параметров в registers_init текущими значениями регистров (новые не создавать).
  11. addRegProperties() --- добавить новые параметры, инициализировать их регистрами.
  12. readRegProperties() --- перечитать параметры в регистры.

Интерфейс с herkulex_sched.

  1. Требует: void cmdReqCP(HerkulexPacket req) --- извещает herkulex_sched через BufferLockFree о новом запросе.
  2. Предоставляет: void cmdAckCP(HerkulexPacket req) (ClientTread) записывает в BufferLockFree результат запроса, посылает сигнал условной переменной.

Это механизм позволяет преобразовать синхронный вызов функции sendRequest в herkulex_array в асинхронный запрос на уровне herkulex_sched. Фактически он реализует семантику портов на операциях, поскольку порты невозможно использовать, т.к. OROCOS не пердоставляет возможности ожидания прихода сообщения в теле операции.

Методы

  1. cmdSync(HerkulexPacket req, HerkulexPacket ack) --- синхронный запрос к приводам. Вызывает cmdReqCP, блокируется на условии до истечения timeout или вызова cmdAckCP.

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

Конфигурация. Считываются опции, создается HerkulexArray, содержащий объекты HerkulexServo. Проводится инициализация приводов и регистров по параметрам.

Исполнение. updateHook пуст, основной код в операциях. Операции транслируют запрос в вызовы cmdSync.

В некоторой степени открыт вопрос кеширования регистров и приводов. Для ускорения операций требующий чтения множества регистров они очень желательны.

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

Операции длительные и блокирующие. Следует предусмотреть реализацию breakLoop, прерывающую ожидание (сигнализирует условную переменную).

Взаимодействие c herkulex_sched

Механизм описывается псевдокодом ниже. Он позволяет обрабатывать запросы от подсистемы НиМ в updateHook планировщика herkulex_sched по его усмотрению.

В herkulex_array:

cmdSync(HerkulexPacket req, HerkulexPacket ack) {
   request(req)
   ждем условия: !buffer_ack_CP.Empty()
   buffer_ack_CP.Pop(ack)
} 
responseCM(HerkulexPacket ack) {
   buffer_ack_CP.Push(ack)
   сигнализируем условие
}

В herkulex_sched (если планировщик не запущен, то он просто исполняет запросы):

updateHook() {
   ...
   if (!buffer_lock_free2.empty()) {
      buffer_lock_free2.Pop(req)
       обработка
       response(ack)
    }
    ...
}
cmdReqCP(HerkulexPacket req) {
    if (this->isRunning())  
        buffer_CP_req.Push(req)
    else {
        herkulex_driver.cmdSync(req, ack);
        response(ack);
    }
}

Реализация протокола

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

Семантика протокола состояит в трансляции запросов в HerkulexPacket и обратном декодировании ответов в заданные структуры данных. Иного варианта быть не может: один объект-протокол используется одновременно двумя нитями, использующими разные методы обращения к приводам, что не позволяет удобно включить в протокол синхронное взаимодействие с приводом. Единственное решение --- передовать функциям-запросам сылку на requestSync.

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

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

  • RegisterMapper(vector<HerkulexRegister>) --- описание набора регистров, преобразования адреса в имя и обратно в любом направлении.

  • RegisterValues : map<string, uint> --- используется для передачи и хранения состояние группы регистров.

  • HerkulexServo(name, hw_id, RegisterMapper, reverse, offset) Запись о существовании привода. Не хранит никаких состояний привода, кроме неизменных параметров: scale, offset, hw_id, reverse. Реализует типовые операции c приводом. Связывается c RegisterMapper по ссылке.

    Типовые методы. .convert* --- преобразование значений в СИ к внутреннему представлению привода и обратно (учет scale, reverse). .reqRead(HerkulexPacket& req, string register) --- запрос на чтение регистра .reqRead(HerkulexPacket& req, reg_start, reg_end) --- запрос на чтение регистров .reqRead_epp(HerkulexPacket& req, string register) --- запрос на чтение регистра .reqStatus(HerkulexPacket& req) bool .ackRead(HerkulexPacket& ack, RegisterValues regs) --- декодирование запроса, false при несовпадении типа или привода * ...

    Заглушки для методов чтения РВ. Не ясно, как сделать лучше: для привода с произвольным набором регистров нельзя гарантировать, что нужные регистры есть и их можно прочитать за одно обращение к приводам. .reqPosVel --- запрос скорости и позиции bool .ackPosVel --- декодирование результата в структуру .reqState --- запрос регистров 53--59 .ackState --- декодирование результата в структуру

  • HerkulexServoDRS0101(name, hw_id, reverse, offset) : HerkulexServo Наследник HerkulexServo: жестко задан RegisterMapper, реализованы жестко зависящие от набора регистров методы

    • .reqPosVel --- запрос скорости и позиции
    • bool .ackPosVel --- декодирование результата в структуру
    • .reqState --- запрос регистров 53--59
    • .ackState --- декодирование результата в структуру

  • HerkulexArray Агрегирует набор HerkulexServo (реально HerkulexServoDRS0101) доступ по имени, итерация по элементам

    • .reqIJOG(HerkulexPacket& req, HerkulexGoal& goal)
    • .reqSJOG(HerkulexPacket& req, HerkulexGoal& goal, playtime)
    • const HerkulexServo * GetServo(const string& name), const HerkulexServo * GetServo(uint hw_id) --- получить указатель на привод. <!--- * bool .ackRead(HerkulexPacket& ack, string name, RegisterValues regs) --- декодирование запроса, false при несовпадении типа или привода ->

Для извещения о ошибках следует использовать исключения, однако надо помнить, что это не РВ средство. Т.е. в случае кода РВ лучше сделать проверки явно.

Типовое использование:

 const HerkulexServo& servos = array->getServo(name); // исключения по имени привода
 servo->reqRead(req_packet, "velocity");  // исключение по имени регистра
 requestSync(req_packet, ack_packet); 
 if (! servo->ackRead(ack_packet, cache))
     if (servo->getHWID() ~= ack_packet.hw_id) ....
     else ...
 }
 else {
     ...
 }

Исключения и ошибки

Уточнить 1. Привод не отвечает на STAT 2. Превышен лимит числа попыток обращения к приводу: привод не отвечает 2. Превышен лимит числа попыток обращения к приводу: привод не вернул плохой ответ. 4. Неверное имя привод/регистр.

Компонент herkulex_sced

Подсистема обмена реального времени.

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

  1. sync (TimerEvent, EventPort) --- тактирующий сообщения таймера
  2. goal (HerkulexGoal) --- задания на приводы

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

  1. joints (JointState) --- позиции и скорости (прочитанные).
  2. states (HerkulexState) --- расширенное состояние привода.

Параметры

  1. period_RT (double) --- длительность периода РВ (секунды)
  2. period_CM (double) --- длительность периода НиМ (секунды)
  3. poll_list (strings) --- список приводов, которые опрашиваются.

Операции

Интерфейс с herkulex_array.

  1. Требует: void responseCM(HerkulexPacket req) передать подсистеме НиМ (configuration and monitoring) результат запроса.
  2. Предоставляет: void requestCM(HerkulexPacket req) (ClientTread) --- извещает herkulex_sched через BufferLockFree о новом запросе.

Интерфейс с herkulex_driver.

  1. Требует: bool cmdSync(HerkulexPacket& req, HerkulexPacket& ack) --- послать запрос, дождаться ответа, вернуть результат.
  2. Требует: bool cmd(HerkulexPacket& cmd, Empty) --- послать команду, не ожидать ответа.
  3. Требует: uint estimate(uint data_size, bool ack_rek) (ClientTread) --- верхняя оценка времени обмена (опционально).

Методы

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

Операция cmdReqCP. Поведение детализировано выше. В зависимоси от состояни компонента (запущен, остановлен) помещает запрос в буфер, либо передает его на исполнение канальному уровню немедленно.

Исполнение. updateHook():

  1. Ожидаем сообщение TimerEvent
  2. Запускаем таймер на длительность period_RT
  3. Читаем порт goal, посылаем JOG
  4. В цикле читаем poll_list и опрашиваем приводы.
  5. Выход из цикла: иcтек таймер или кончился список.
  6. Публикуем результат опроса.
  7. Запускаем таймер на длительность period_CM
  8. Ждем запроса через вызов cmdReqCP, обрабатываем его.
  9. Цикл и ожидание прерываем по истечению таймера.
  10. Переход в состояние 1.

По идее, можно опросить состояние всех приводов за несколько циклов.

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

Ожидание запроса НиМ (cmdReqCP) и ответа приводов можно осуществить выходом из updateHook. По событию (исполнение операции, или getActivity()->trigger()) снова начнет исполнятся пользовотельский код. Надо только проверить тип события: запрос, ответ, таймер. При такой реализации будет не цикл, а конечный автомат.

В OROCOS есть реализация таймера Timer, буфера BufferLockFree и средств синхронизации.

Исключения и ошибки

Уточнить 1. TimerEvent пришло раньше выхода из updateHook(). 2. Ошибка разбора ответа привода. 3. Имена приводов.