Заметки по ROS: работа с параметрами и dynamic_reconfigure

Это четвертая из серии статей о робототехнической платформе ROS ^[1]. Первую статью можно найти здесь ^[2]. В предыдущей ^[3]статье я рассказывал о параметрах в ROS. В этой статье я расскажу о том как можно динамически управлять параметрами в ROS с помощью dynamic_reconfigure ^[4] . Кому интересно прошу под кат.

Пакет dynamic_reconfigure позволяет изменять параметры, которые хранятся на Parameter server в рантайме. Он отвечает за отправку нового значения параметра на сервер параметров и оперативное уведомление всех узлов, которые используют это параметр. Таким образом наш узел узнает об изменениях значения параметра без необходимости его перезапуска.

Например, у нас может быть GUI приложение, которое позволяет настраивать параметры в робототехнической системе. Также dynamic_reconfigure позволяет значительно упростить процесс файнтюнинга параметров с целью поиска оптимальной комбинации значений параметров.

Создание cfg файлов

Для начала нужно научиться создавать cfg файлы.

Перейдем в воркспейс и создадим пакет ROS

cd ~/catkin_ws/src
catkin_create_pkg parameter_server_basics std_msgs roscpp rospy dynamic_reconfigure

Скомпилируем воркспейс

cd ~/catkin_ws/ && catkin_make

Теперь нужно создать внутри нашего пакета папку cfg, в которой будут лежат файлы cfg.

Создадим внутри папки cfg новый файл parameter_server_basics.cfg с таким содержимым:

#!/usr/bin/env python3

PACKAGE = "parameter_server_basics"

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

gen.add('hor_angle', int_t, 0, "A horizontal angle", 10, 0, 90)
gen.add('status', str_t, 0, "A state of device", "Active")

exit(gen.generate(PACKAGE, 'parameter_server_basics', 'parameter_server_'))

Более подробно о синтаксисе файлов cfg можно прочитать здесь ^[5].

Если вкратце то с помощью метода add() мы добавляем параметр. Метод принимает следующие аргументы: название, тип, битовую маску level которая используется в колбеке (здесь ^[6] обсуждают этот аргумент), описание параметра, значение по умолчанию. Последние два параметры в данном случае для численных типов это минимальное и максимальное значения.

Сделаем файл исполняемым:

chmod +x parameter_server_basics.cfg

Добавим такие строки в файл CMakeLists.txt:

#add dynamic reconfigure api
#find_package(catkin REQUIRED dynamic_reconfigure)
generate_dynamic_reconfigure_options(
  cfg/parameter_server_basics.cfg
  #...
)

и такую строку в самый конец файла:

add_dependencies(parameter_server_basics ${PROJECT_NAME}_gencfg)

Теперь добавим узел который будет использовать файл cfg. Создадим файл parameter_server_basics.cpp в папке src:

#include <ros/ros.h>

#include <dynamic_reconfigure/server.h>
#include <parameter_server_basics/parameter_server_Config.h>


void callback(parameter_server_basics::parameter_server_Config &config, uint32_t level)
{
    ROS_INFO("Reconfigure Request: %d %s",
        config.hor_angle, config.status.c_str());
}

int main(int argc, char* argv[])
{
    ros::init(argc, argv, "parameter_server_basics");

    dynamic_reconfigure::Server<parameter_server_basics::parameter_server_Config> server;
    dynamic_reconfigure::Server<parameter_server_basics::parameter_server_Config>::CallbackType f;

    f = boost::bind(&callback, _1, _2);
    server.setCallback(f);

    ROS_INFO("Spinning node");
    ros::spin();

    return 0;
    
}

Теперь добавим узел в конец файла CMakeLists.txt:

add_executable(parameter_server_basics src/parameter_server_basics.cpp)
target_link_libraries(parameter_server_basics ${catkin_LIBRARIES})

Нам нужно сделать так чтобы узел rqt_reconfigure запускался с Python 3.

Выполним команду:

sudo ln -s /usr/bin/python3 /usr/bin/python

Соберем воркспейс и запустим узел:

cd ~/catkin_ws && catkin_make 

Запустим наш узел:

rosrun parameter_server_basics parameter_server_basics

В другом терминале запустим rqt_reconfigure

rosrun rqt_reconfigure rqt_reconfigure

Мы увидим приложение

Изменим значение для параметра hor_angle или статус и получим такой вывод в терминале с запущенным узлом:

Теперь у вас есть простой способ легко настраивать параметры и все узлы, которые используют эти параметры будут автоматически осведомлены. На этом все.