Action. Пример сервера

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

В стандартной установке РОС есть пример, который рассчитывает последовательность Фибоначчи до введенного клиентом порядкового номера.

С расчетом числа Фибоначчи мы уже сталкивались когда изучали основы python. Но кратко напомним, что такое ряд Фибоначчи. Это такая последовательность чисел, при которой каждое следующее число является суммой двух предыдущих. При этом вся последовательность начинается с нуля и единицы. Т.е. третий член последовательности: ноль плюс один - один. Четвертый: один плюс один - два. Следующий: два плюс один - три, потом два плюс три - пять, потом три плюс пять - восемь и т.д.

Формула вычисления числа

f(0) = 0
f(1) = 1
f(n) = f(n-1) + f(n-2)

Вот такой вот сервер, считающий ряд Фибоначчи до заданного порядкового номера мы и рассмотрим. При этом мы позаимствуем уже созданные сообщений из примеров, чтобы упростить нашу задачу. Более детально о создании сообщений для Actions мы поговорим в разделе Администрирование ROS.

Action Server

Рассмотрим код Action сервера

import rospy
import actionlib
from actionlib_tutorials.msg import FibonacciAction, FibonacciFeedback, 
                                    FibonacciResult

class FibonacciActionServer(object):

    _feedback = FibonacciFeedback()
    _result = FibonacciResult()

    def __init__(self, name):
        self._action_name = name
        self._as = actionlib.SimpleActionServer(self._action_name, FibonacciAction,
                                 execute_cb=self.execute_cb, auto_start = False)
        self._as.start()

    def execute_cb(self, goal):

        r = rospy.Rate(1)
        success = True

        # append the seeds for the fibonacci sequence
        self._feedback.sequence = []
        self._feedback.sequence.append(0)
        self._feedback.sequence.append(1)

        # publish info to the console for the user
        rospy.loginfo('%s: Executing, creating fibonacci 
                        sequence of order %i with seeds %i, 
                        %i' % (self._action_name, goal.order, 
                        self._feedback.sequence[0], 
                        self._feedback.sequence[1]))

        # start executing the action
        for i in range(1, goal.order):
            # check that preempt has not been requested by the client
            if self._as.is_preempt_requested():
                rospy.loginfo('%s: Preempted' % self._action_name)
                self._as.set_preempted()
                success = False
                break
            self._feedback.sequence.append(self._feedback.sequence[i] + 
                                    self._feedback.sequence[i-1])
            # publish the feedback
            self._as.publish_feedback(self._feedback)
            # this step is not necessary, the sequence is computed at 1 Hz for demo
            r.sleep()

        if success:
            self._result.sequence = self._feedback.sequence
            rospy.loginfo('%s: Succeeded' % self._action_name)
            self._as.set_succeeded(self._result)

if __name__ == '__main__':
    rospy.init_node('fibonacci')
    server = FibonacciActionServer(rospy.get_name())
    rospy.spin()

Начало, как всегда, состоит из подключения различных модулей

import rospy
import actionlib
from actionlib_tutorials.msg import FibonacciAction, FibonacciFeedback, 
                                FibonacciResult

Так как модуль работы с Action не является часть rospy, нам необходимо подключить его отдельно. Также по аналогии с топиками, которые требуют импорта используемой в них структуры данных, экшн-серверы тоже требуют импорта структуры данных используемых сообщений.

В данном случае FibonacciActionServer - это класс, описывающий используемые сообщения для работы, а также FibonacciFeedback и FibonacciResult, объекты для использования feedback (промежуточного результата) и result (результата выполнения задания).

Давайте посмотрим, из чего они состоят

roscd actionlib_tutorials/msg/
cat FibonacciAction.msg
======
FibonacciActionGoal action_goal
FibonacciActionResult action_result
FibonacciActionFeedback action_feedback

Видно, что структура экшна состоит из трех других структур: запроса (goal), результата (result) и промежуточного результата (feedback). Мы не будем останавливаться подробно на том, как именно она собирается, а просто примем к сведению, что она создается автоматически при сборке пакета.

Давайте также посмотрим на структуры запроса, результата и фидбэка:

cat FibonacciGoal.msg
# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
#goal definition
int32 order

Переменная в структуре всего одна int32 order, это и есть порядковый номер указанного члена, до которого нам надо провести расчет ряда. Обратите внимание, мы не импортируем эту структуру в нашу программу, хотя и неявно будем пользоваться ей. Это произойдет потому, что FibonacciGoal уже есть в составе структуры FibonacciAction.

Давайте также посмотрим на FibonacciFeedback и FibonacciResult

cat FibonacciFeedback.msg      
====  
int32[] sequence

cat FibonacciResult.msg

int32[] sequence

Это очень "похожие" объекты, оба содержат массивы int32. Но один ответ необходимо использовать как Feedback, другой как Result. И понятно, что в общем случае эти сообщения могут быть разными, разной структуры и состава, но в нашем случае они одинаковые.

С сообщениями мы разобрались, можем перейти и к самой программе.

Создаем наш основной класс. И инициализируем переменные _feedback и _result

class FibonacciAction(object):
    # create messages that are used to publish feedback/result
    _feedback = FibonacciFeedback()
    _result = FibonacciResult()

Конструктор класса

    def __init__(self, name):
        self._action_name = name
        self._as = actionlib.SimpleActionServer(self._action_name, FibonacciAction,
                    execute_cb=self.execute_cb, auto_start = False)
        self._as.start()

Переменная self._as - это переменная с объектом нашего ActionServer. При его инициализации мы передаем параметрами:

Имя Action сервера
Тип используемого сообщения (FibonacciAction)
параметр execute_cb название функции обратного вызова для запуска после получения goal

Далее мы запускаем сервис командой self._as.start()

Мы обозначили при инициализации функцию обратного вызова execute_cb, рассмотрим ее.

Инициализация вспомогательных переменных

r = rospy.Rate(1)
success = True

rospy.Rate(1) Задает частоту "генерации" последовательности. success флаг завершения расчета последовательности.

self._feedback.sequence = []
self._feedback.sequence.append(0)
self._feedback.sequence.append(1)

Инициализация пустой последовательности для заполнения и "генерация" первый двух элементов.

rospy.loginfo('%s: Executing, creating fibonacci 
                sequence of order %i with seeds %i, 
                %i' % (self._action_name, goal.order, 
                self._feedback.sequence[0], 
                self._feedback.sequence[1]))

Вывод отладочного сообщения

Ну и сам цикл расчета числа

# start executing the action
for i in range(1, goal.order):
    # check that preempt has not been requested by the client
    if self._as.is_preempt_requested():
        rospy.loginfo('%s: Preempted' % self._action_name)
        self._as.set_preempted()
        success = False
        break
    self._feedback.sequence.append(self._feedback.sequence[i] + 
                                    self._feedback.sequence[i-1])
    # publish the feedback
    self._as.publish_feedback(self._feedback)
    # this step is not necessary, the sequence is computed at 1 Hz for demo
    r.sleep()

По общей логике этого блока мы "крутим" цикл от 1 до заданной цели goal.order

Если клиент запросил "отмену" операции if self._as.is_preempt_requested(): то мы меняем статус сервера на "preempted", убирая флаг о успешном завершении, прерываем цикл for

Если с программой ничего не случилось, то продолжаем "работать". Заполняем последовательность новым элементом self._feedback.sequence[i] + self._feedback.sequence[i-1]

Публикуем последовательность в feedback.

Метод r.sleep() создает паузу выполнения (r = rospy.Rate(1))

if success:
    self._result.sequence = self._feedback.sequence
    rospy.loginfo('%s: Succeeded' % self._action_name)
    self._as.set_succeeded(self._result)

Если цикл for с расчётом последовательности закончился, и мы все еще в статусе "все хорошо", то стоит ответить заполненным `result

На этом работа по расчету последовательности закончена, как и функция execute_cb

Далее нам необходимо только правильно запустить сервер

if __name__ == '__main__':
    rospy.init_node('fibonacci')
    server = FibonacciActionServer(rospy.get_name())
    rospy.spin()

Уже привычный для нас код, инициализируем ROS ноду, инициализируем сервер и переходим в бесконечное ожидание функцией rospy.spin()

Запуск и проверка

Ранее мы уже запускали аналогичный Action Server, поэтому тут для нас все знакомо.

Запустим сервер

python3 action_server.py

Проверим, что появилась наша нода

rosnode list 

/fibonacci
/rosout

Проверим, что появились необходимые топики

rostopic list
/fibonacci/cancel
/fibonacci/feedback
/fibonacci/goal
/fibonacci/result
/fibonacci/status

Запустим просмотр топиков result и feedback в разных консолях

rostopic echo /fibonacci/feedback

rostopic echo /fibonacci/result

Запустим расчет последовательности для числа 5

rostopic pub /fibonacci/goal actionlib_tutorials/FibonacciActionGoal "header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: ''
goal_id:
  stamp:
    secs: 0
    nsecs: 0
  id: ''
goal:
  order: 5"

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

feedback: 
  sequence: [0, 1, 1]
===
feedback: 
  sequence: [0, 1, 1, 2]
===
feedback: 
  sequence: [0, 1, 1, 2, 3]  
===
feedback: 
  sequence: [0, 1, 1, 2, 3, 5]

А в топике result увидим только одну запись

result: 
  sequence: [0, 1, 1, 2, 3, 5]

Мы убедились, что наш ActionServer работает.

При получении Goal начинается расчет, а по мере расчета публикуются промежуточные данные Feedback и при завершении расчета публикуется результат Result

Action. Пример сервера

Action. Пример сервера

Action Server

Запуск и проверка

results matching ""

No results matching ""