Теперь, когда в вашей клиентской библиотеке появились сообщения, можно перейти к созданию ROS2 ноды и связанных с ней элементов (издателей, сервисов и т.п.). На самом деле процесс добавления этих элементов мало отличается от создания обёртки для таймера, описанный в первой части, поскольку всё сводится к надстройке над функциями библиотеки rcl. Поэтому я не буду рассматривать их подробно, а остановлюсь только на отдельных особенностях.

Нода

Нода (node) так названа, потому что является узлом ROS2 графа, ребрами же являются топики - именованные каналы обмена сообщениями. На уровне rcl её функции сводятся к вызовам типа rcl_node_get_name, rcl_count_publishers, rcl_action_get_client_names_and_types_by_node, т.е. к получению информации о графе и своей роли в нем. Если же переходить на уровень реализации клиентской библиотеки на целевом языке программирования, функция ноды расширяется: теперь она хранит в себе все элементы, связанные с обменом сообщениями, а также часы, таймеры, логгеры и всё что вы захотите в неё добавить. Причин такой трансформации я вижу две. Во-первых, большинство этих элементов требуют указатель на объект ноды при инициализации. Во-вторых, в функцию spin() проще передать один контейнер, чем каждый элемент по отдельности. В результате, конструктор ноды (в имплементации на Lua) приобретает следующую структуру.

function Node.__call (self, ...)
  local o = {}
  -- rcl нода
  o._node__object = rclbind.new_node(self.name, self.namespace)
  -- имя
  o._node__name = self.name
  -- добавляемые элементы
  o._clock__object = rclbind.new_clock()
  o._timer__list = {}
  o._publisher__list = {}
  o._subscription__list = {}
  o._client__list = {}
  o._service__list = {}
  o._action__list = {}
  o._guard__list = {}
  -- наследуем методы ноды
  return setmetatable(o, Node)
end

Издатели и подписчики

Издатель (publisher) и подписчик (subscriber) служат для широковещательной трансляции данных: для каждого топика число издателей и подписчиков может быть произвольным. При создании они требуют указывать связанную ноду, тип сообщения, имя топика и QoS канала связи.

rcl_publisher_options_t publisher_opt = rcl_publisher_get_default_options();
rcl_publisher_t publisher =  rcl_get_zero_initialized_publisher();
// инициализация издателя, аналогично для подписчика
rcl_ret_t ret = rcl_publisher_init(publisher, node, message_type, topic, &publisher_opt);

Публикация сообщения сводится к вызову функции rcl_publish(publisher, message). Что касается подписчика, нужно дополнительно связать callback-функцию с указателем на объект на уровне целевого языка программирования, поскольку в rcl такой функционал не предусмотрен.

Сервисы и клиенты

Как понятно из названия, логика работы стандартная: клиент посылает запрос, сервис его обрабатывает и возвращает ответ. Сервис может обрабатывать запросы многих клиентов, но последовательно; клиент работает с одним сервисом, но может послать несколько запросов, не дожидаясь ответа. Отсюда вытекают требования к реализации: сервис должен понимать, кому возвращать результат, а клиент должен знать, которому из запросов соответствует полученный ответ. Первая задача решается сохранением id клиента, вторая - id запроса. В обоих случаях для хранения данных между вызовами используются какие-то сущности (классы, контейнеры), которые должны освобождаться после передачи/получения ответа. Для обмена данными служат следующие функции rcl.

// сервис
  // получение запроса
  rmw_service_info_t header;  // сохранение информации о клиенте
  rcl_ret_t ret = rcl_take_request_with_info(srv, &header, request);
  // передача результата
  rcl_ret_t ret = rcl_send_response(srv, &header->request_id, response);

  // -------------------

  // клиент
  // передача запроса
  int64_t seq_num = 0;
  rcl_ret_t ret = rcl_send_request(cli, request, &seq_num);
  // получение результата
  rmw_service_info_t header;
  rcl_ret_t ret = rcl_take_response_with_info(cli, &header, response);
  // идентификатор находится в header.request_id.sequence_number

При работе с клиентом у пользователя есть выбор: остановить выполнение программы до получения ответа или продолжить работу. На уровне rcl нет синхронного или асинхронного вызова, логика работы с Wait Set будет одинаковая в обоих случаях, поэтому данный функционал реализуется с помощью целевого языка программирования.

Action сервисы и клиенты

Экшн сервисы отличаются от обычных продолжительностью действия: запрос клиента запускает процесс, за которым можно следить с помощью сообщений обратной связи. Этот процесс может завершиться успешно или не успешно, а также может быть прерван по инициативе клиента. Одно из ключевых особенностей реализации заключается в том, что нода должна иметь возможность запустить несколько action сервисов одновременно, т.е. они должны выполняться в параллельных потоках, в то время как весь прочий функционал ROS2 по умолчанию однопоточный.

После того как экшн клиент послал запрос серверу, логика его работы сводится к следующему циклу. Wait Set имеет отдельный метод для работы с экшн клиентом и сервисом. При завершении ожидания возвращаются флаги, которые указывают, какое из сообщений было получено: ответ сервиса регистрации задания (goal), ответ сервиса результата (result), ответ сервиса прерывания исполнения (cancel), сообщение обратной связи (feedback), сообщение о состоянии работы сервера (status). Исходя из этого запускается соответствующая логика обработки.

while rclbind.context_ok() do
  -- добавляем в Wait Set
  wait_set:clear()
  act_cli:add_to_waitset(wait_set)
  -- ждем ответа action сервиса
  wait_set:wait(-1)

  -- извлекаем тип сообщения
  local is_feedback, is_status, is_goal, is_cancel, is_result = act_cli:is_ready(wait_set)

  -- ответ сервера на задание
  if is_goal then
    local resp, fn, seq = act_cli:take_goal_response()
    if resp.accepted then
      -- посылаем запрос на получения результата
    else
      -- сервер отклонил наше задание
    end
  end

  -- если клиент прервал работу сервера
  if is_cancel then
    local resp, fn, seq = act_cli:take_cancel_response()
    -- здесь можно вызвать callback для обработки
  end

  -- сервер прислал результат
  if is_result then
    local resp, fn, seq = act_cli:take_result_response()
    -- вызваем callback для полученных данных
  end

  -- сообщение обратной связи
  if is_feedback then
    local resp, fn = act_cli:take_feedback()
    -- следим за ходом работы
  end

  -- статус сервера (задание принято, выполняется, отменено и т.д.)
  if is_status then
    local resp, fn = act_cli:take_status()
    -- следим за сервером
  end
end

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

while rclbind.context_ok() do
  -- добавляем в Wait Set
  wait_set:clear()
  act_srv:add_to_waitset(wait_set)

  -- ждем новый запрос или заданное время
  wait_set:wait(time_ns)

  -- проверка входящих сообщений
  local is_goal, is_cancel, is_result, is_expired = act_srv:is_ready(wait_set)

  -- новое задание
  if is_goal then
    local req, _, header = act_srv:take_goal_request()
    -- проверяем req.goal_id.uuid чтобы избежать повторных запусков
    -- если задание новое, создаем поток для выполнения
    -- посылаем ответ клиенту (принято/не принято)
  end

  -- прерывание процесса
  if is_cancel then
    local req, _, header = act_srv:take_cancel_request()
    -- завершаем требуемые задачи
    -- посылаем ответ клиенту (список завершенных задач)
  end

  -- запрос результата
  if is_result then
    local req, _, header = act_srv:take_result_request()
    -- сохраняем информацию о клиенте
    -- чтобы позже вернуть результат
  end

  if is_expired then
    local lst = act_srv:expire_goals(handle_num)
    -- очистить список задач
  end

  -- в процессе выполнения нужно передавать клиенту
  -- сообщения обратной связи и статус задачи
end

Executor и spin

Для удобства работы с Wait Set в клиентской библиотеке ROS2 вводится класс Executor. Он выполняет всю работу по регистрации событий, их ожиданию и последующей обработке. Данного класса в rcl нет, он реализуется на уровне целевого языка программирования.

Объект Executor хранит ссылки на одну или несколько ROS2 нод. При вызове метода spin (или его вариаций spin_once, spin_until_future_complete) запускается цикл, на каждой итерации которого из всех нод собираются связанные с ними элементы (издатели, таймеры, сервисы и т.д.), добавляются в Wait Set, и после разблокировки выполняются соответствующие callback функции. Данный алгоритм может быть вынесен в следующую функцию.

function _wait_for_ready_callbacks (executor, timeout_sec)
  -- подготовка элементов для Wait Set
  for _, node in ipairs(executor._nodes) do
    -- извлечение списка подписчиков, таймеров, сервисов, клиентов, защитников, событий

    -- отдельно для action сервисов/клиентов
    for _, act in ipairs(node._action__list) do
      local sub_no, guard_no, timer_no, cli_no, srv_no = act:get_num_entities()
      sub_cnt   = sub_cnt + sub_no
      timer_cnt = timer_cnt + timer_no
      cli_cnt   = cli_cnt + cli_no
      srv_cnt   = srv_cnt + srv_no
      guard_cnt = guard_cnt + guard_no
    end
  end

  -- инициализация
  executor._wait_set = rclbind.new_wait_set(
    sub_cnt,
    guard_cnt,
    timer_cnt,
    cli_cnt,
    srv_cnt,
    ev_cnt)

  -- добавление событий
  local wait_set = executor._wait_set
  wait_set:clear()

  for i = 1, #subscriptions do wait_set:add_subscription(subscriptions[i]) end
  for i = 1, #timers do wait_set:add_timer(timers[i]) end
  for i = 1, #clients do wait_set:add_client(clients[i]) end
  for i = 1, #services do wait_set:add_service(services[i]) end

  for i = 1, #actions do actions[i]:add_to_waitset(wait_set) end

  -- ожидание
  wait_set:wait(timeout_sec)
  if not rclbind.context_ok() then return end

  -- проверка событий
  subscriptions = wait_set:ready_subscriptions()
  timers = wait_set:ready_timers()
  clients = wait_set:ready_clients()
  services = wait_set:ready_services()

  -- обработка
  for _, act in ipairs(actions) do
    -- извлечение сообщения для action сервера/клиента
    -- возврат функции обработки
  end

  for i = 1, #subscriptions do
    -- возврат callback функции подписчика
  end

  for i = 1, #timers do
    -- возврат callback функции таймера
    -- вызов метода call таймера
  end

  for i = 1, #services do
    -- возврат функции сервиса
    -- передача результата клиенту
  end

  for i = 1, #clients do
    -- возврат callback функций клиентов
  end
end

Обычно данная функция заворачивается в корутину, которая для каждого объекта в цикле возвращает исполняемый handle. Это позволяет отследить промежуточные события, такие как нажатие пользователем Ctrl+C или прерывание текущего цикла обработки через вызов триггера объекта guard.

-- однократное исполнение
function Executor.spin_once (self, timeout_sec)
  -- создание корутины (если требуется)
  self._cb_iter = coroutine.create(_wait_for_ready_callbacks)
  -- вызов
  local ok, handle = coroutine.resume(self._cb_iter, self, timeout_sec)
  -- обработка
  if ok and handle then handle() end
  -- прочие операции
end

-- непрерывное исполнение
function Executor.spin (self)
  while rclbind.context_ok() do
    Executor.spin_once(self, -1)
  end
end

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

Заключение

Функционал, рассмотренный в этих 3х статьях, не покрывает все возможности ROS2. В частности, я не написал про сервис параметров и lifecycle ноды. Однако, данный материал может помочь с реализаций большинства функций при интеграции нового языка программирования в экосистему ROS2. А также послужить путеводителем для тех, кто захочет погрузиться в исходные коды этого фреймворка.

Создаем клиентскую библиотеку ROS2. Элементы ноды