Продолжаем разбираться с тем, как научить ROS2 понимать ваш язык программирования. В прошлый раз мы рассмотрели создание и запуск минимальной программы, теперь поговорим про работу с сообщениями. Свою библиотеку я разрабатывал для Lua, поэтому далее в примерах будет встречаться упоминание этого языка.
Обмен данными играет в ROS2 ключевую роль. К счастью, практически все задачи, связанные с передачей и приемом сообщений берут на себя библиотеки rcl и rmw, нам «всего лишь» необходимо обеспечить возможность их создания и обработки.
Структура сообщения в ROS2 описывается в файле с расширением msg или idl. При сборке пакета выполняются следующие действия:
-
генерация исходного кода на C
-
генерация исходного кода для других языков (C++, Python), который ссылается на C код
-
компиляция динамических библиотек, настройка путей
Рассмотрим подробнее, как эта генерация устроена.
Представление сообщений в C
Прежде чем переходить к другим языкам полезно посмотреть, какую структуру имеют сообщения на C. Вообще, если ваш язык программирования предоставляет возможность взаимодействовать с полями C структур напрямую, вам очень повезло. В противном случае, придётся писать функции для доступа к данным.
Для каждого сообщения генератор C кода формирует файлы нескольких видов. Непосредственно структура сообщений описана в файле тип__struct.h. В тип__functions.c хранятся функции для инициализации, освобождения, копирования, проверки равенства, а в тип__description.c — детальная информация о структуре сообщений.
Поля C структуры расположены в том же порядке, в каком они описаны в файле msg. Для элементарных типов (таких как uint8, float64, bool) используется соответствующий тип из C. В случае объекта формируется отдельная структура, которая затем добавляется в сообщение. Если поле представляет собой статический массив, т.е. число элементов известно заранее, аналогичный массив закладывается и в C структуру. Динамический массив описывается указателем на выделяемую память, текущим числом элементов и размером выделенной памяти. Для примитивных типов и строк динамические массивы используются повсеместно, поэтому они заранее определены, и генератор просто добавляет в хэдеры библиотеку rosidl_runtime_c.
Генератор сообщений
Генерацию сообщений выполняет отдельный ROS2 пакет, написанный на Python. Он включает в себя шаблоны для генерации кода, исполняемые и библиотечные файлы, а также CMake скрипты. Обычно данный пакет содержит следующие папки.
Папка resource
Здесь лежат шаблоны для функций сериализации и десериализации сообщений под конкретный язык программирования. Они имеют расширение em и синтаксически являются макросами над языком Python, позволяющими комбинироавть сырой текст с исполняемым кодом. Можно разделить эти макросы на 4 типа: для управления генерацией кода (@[ ]), определения промежуточных переменных (@{ }), подстановки значения в генерируемый текст (@( )) и комментариев (@#).
@# это строчный комментарий@# генерация в цикле или по условию@[инициализация блока]@текст программы@[ опциональное промежуточное условие, со смещением]@альтернативный текст программы@[конец блока]@@# объявление переменных, функций, прочий код на Python@{var = 42}@продолжение программы, значение var равно @(var)
Основная логика преобразования сообщений определена в файле msg.c.em (название может быть произвольным), который служит для генерации C кода. Здесь можно реализовать все необходимые функции для работы с сообщениями, но прежде всего необходимы методы сериализации и десериализации. Они используют следующий шаблон.
@# загрузка зависимостей@{from rosidl_generator_lua import NUMERIC_LUA_TYPES, sequence_metatable, make_prefixfrom rosidl_parser.definition import EMPTY_STRUCTURE_REQUIRED_MEMBER_NAME}@@# формирование имени функции@[for member in message.structure.members]@@[ if len(message.structure.members) == 1 and member.name == EMPTY_STRUCTURE_REQUIRED_MEMBER_NAME]@@[ continue]@@[ end if]@@{msg_prefix = make_prefix(message)setter_ = '_'.join((msg_prefix, '_set', member.name))}@static int @(setter_) (lua_State* L) { // подготовительные операции ...@# генерация наименования типа@{type_ = member.typeif isinstance(type_, AbstractNestedType): type_ = type_.value_type}@ // значение считывается из поля с именем // @(member.name)@# комплексный тип данных@[ if isinstance(type_, NamespacedType)]@@# массив@[ if isinstance(member.type, AbstractNestedType)]@@# динамический массив@[ if isinstance(member.type, AbstractSequence)]@ // копирование данных в динамический массив@# статический массив@[ else]@ // копирование данных в массив@[ end if]@@# объект@[ else]@ // копирование объекта@[ end if]@@# последовательность примитивных типов@[ elif isinstance(member.type, AbstractNestedType)]@@# динамический массив@[ if isinstance(member.type, AbstractSequence)]@ // копирование данных в динамический массив@# статический массив@[ else]@ // копирование данных в массив@[ end if]@@# литерал@[ elif isinstance(member.type, BasicType) and member.type.typename == 'char']@ // копирование одиночного символа@# логическая переменная@[ elif isinstance(member.type, BasicType) and member.type.typename == 'boolean']@ // копирование логической переменной@# число@[ elif isinstance(member.type, BasicType) and member.type.typename in NUMERIC_LUA_TYPES]@@{type_dict = NUMERIC_LUA_TYPES[member.type.typename]}@@# проверка беззнакового числа@[ if member.type.typename.startswith('u') ]@ // проверка диапазона беззнаковых чисел@[ else]@ // проверка диапазона с учетом знака@[ end if]@ // копирование значения@# строка в 8-битной кодировке@[ elif isinstance(member.type, AbstractString)]@ // копирование строки@# строка в 16-битной кодировке@[ elif isinstance(member.type, AbstractWString)]@ // копирование строки@[ else]@@# здесь мы не должны оказаться@{assert False, ("unknown type " + member.type.typename)}@@[ end if]@ return 0;}@[end for]@
Список полей сообщения определен в переменной message.structure.members. Для каждого поля шаблон проверяет тип переменной, а также является ли она примитивом, объектом или массивом. В последнем случае дополнительно проверяется тип массива (статический или динамический), и какие элементы в нём содержатся (примитивы или объекты). Исходя из этого строится логика преобразования в C и обратно.
Предыдущий шаблон определяет структуру кода, который должен быть записан в файл. Генерацией файлов для заданного пакета управляет шаблон idl.c.em. Он итеративно вызывает функцию TEMPLATE для каждого найденного сообщения.
@{from rosidl_parser.definition import Messageinclude_directives = set()}@@[for message in content.get_elements_of_type(Message)]@@{TEMPLATE( 'msg.c.em', package_name=package_name, interface_path=interface_path, message=message, include_directives=include_directives)}@@[end for]@
Для ROS2 сервисов генерируются 2 сообщения, соответствующие полям request_message и response_message. В случае action сервисов число сообщений увеличивается до 8 (поля goal, result, feedback, send_goal_service.request_message, send_goal_message.response_message, get_result_service.request_message, get_result_service.response_message, action.feedback_message).
В папке resource можно найти и другие em файлы. Они служат для оборачивания сгенерированных сообщений в одну или несколько динамических библиотек.
Папка rosidl_generator
Скрипт init.py определяет функцию для запуска генерации сообщений, а также вспомогательный функционал, используемый внутри генератора.
from rosidl_cmake import expand_template, generate_filesfrom rosidl_cmake import get_newest_modification_time, read_generator_argumentsfrom rosidl_parser.definition import IdlContent, IdlLocatorfrom rosidl_parser.definition import Message, Service, Actionfrom rosidl_parser.parser import parse_idl_file# переменные и функции для обработки числовых полей сообщенийNUMERIC_LUA_TYPES = { 'float': {'min': 'FLT_MIN', 'max': 'FLT_MAX', 'var': 'lua_Number', 'fn': 'luaL_checknumber', 'ifn': 'lua_pushnumber', 'ctype': 'float'}, # etc.}# основная функция для генерации C кода библиотекиdef generate_lua(generator_arguments_file, typesupport_impls): mapping = {'idl.c.em': '%s.c'} generated_files = generate_files(generator_arguments_file, mapping) args = read_generator_arguments(generator_arguments_file) template_dir = args['template_dir'] # разбор idl файлов modules = {} idl_content = IdlContent() for idl_tuple in args.get('idl_tuples', []): idl_parts = idl_tuple.rsplit(':', 1) idl_rel_path = pathlib.Path(idl_parts[1]) idl_stems = modules.setdefault(str(idl_rel_path.parent), set()) idl_stems.add(idl_rel_path.stem) locator = IdlLocator(*idl_parts) idl_file = parse_idl_file(locator) idl_content.elements += idl_file.content.elements # разделяем по типам в соответствии с реализованными шаблонами obj_list = [ ('msg', idl_content.get_elements_of_type(Message)), ('srv', idl_content.get_elements_of_type(Service)), ('action', idl_content.get_elements_of_type(Action)), ] # формирование файлов библиотеки latest_target_timestamp = get_newest_modification_time(args['target_dependencies']) for msg_type, idl_group in obj_list: template_file = msg_type + '_lib.c.em' out_name = msg_type + '_lib.c' package_name = args['package_name'] data = { 'package_name': args['package_name'], 'content': idl_group, } generated_file = os.path.join( args['output_dir'], msg_type, out_name) template = os.path.join(template_dir, template_file) # развертывание шаблона expand_template( template, data, generated_file, minimum_timestamp=latest_target_timestamp) generated_files.append(generated_file) return generated_files# здесь можно разместить вспомогательные функции для шаблонов
Аргументом функции является JSON файл с параметрами пакета и списком сообщений. В данном примере для какждого типа (Message, Action, Service) определен свой em шаблон. Генератор проходит по списку сообщений, определяет их тип и генерирует соответствующий набор файлов динамической библиотеки.
Папка bin
В данной папке находится Python скрипт, который вызывает написанную выше функцию, передавая ей аргументы командной строки: путь к файлу с агрументами генерации и список типов сообщений.
Папка cmake
Для того чтобы генерация была запущена на этапе сборки ROS2 окружения, нужно настроить CMake. Основную работу выполняет скрипт rosidl_generator_lua_generate_interfaces.cmake.
find_package(rmw REQUIRED)find_package(rosidl_runtime_c REQUIRED)find_package(rosidl_typesupport_c REQUIRED)find_package(rosidl_typesupport_interface REQUIRED)find_package(Python3 REQUIRED COMPONENTS Interpreter)# локальные переменныеset(_output_path "${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_lua/${PROJECT_NAME}")set(_generated_c_files "")# список имен файловforeach(_abs_idl_file ${rosidl_generate_interfaces_ABS_IDL_FILES}) get_filename_component(_parent_folder "${_abs_idl_file}" DIRECTORY) get_filename_component(_parent_folder "${_parent_folder}" NAME) get_filename_component(_idl_name "${_abs_idl_file}" NAME_WE) string_camel_case_to_lower_case_underscore("${_idl_name}" _module_name) set(_src_c "${_output_path}/${_parent_folder}/${_module_name}.c") list(APPEND _generated_c_files ${_src_c}) # msg / srv / action в разные списки if(${_parent_folder} STREQUAL "msg") list(APPEND _msg_list ${_src_c}) endif()endforeach()file(MAKE_DIRECTORY "${_output_path}")# поиск зависимостейset(_dependency_files "")set(_dependencies "")foreach(_pkg_name ${rosidl_generate_interfaces_DEPENDENCY_PACKAGE_NAMES}) foreach(_idl_file ${${_pkg_name}_IDL_FILES}) set(_abs_idl_file "${${_pkg_name}_DIR}/../${_idl_file}") normalize_path(_abs_idl_file "${_abs_idl_file}") list(APPEND _dependency_files "${_abs_idl_file}") list(APPEND _dependencies "${_pkg_name}:${_abs_idl_file}") endforeach()endforeach()# создание файла настроек генератораset(generator_arguments_file "${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_lua__arguments.json")rosidl_write_generator_arguments( "${generator_arguments_file}" PACKAGE_NAME "${PROJECT_NAME}" IDL_TUPLES "${rosidl_generate_interfaces_IDL_TUPLES}" ROS_INTERFACE_DEPENDENCIES "${_dependencies}" OUTPUT_DIR "${_output_path}" TEMPLATE_DIR "${rosidl_generator_lua_TEMPLATE_DIR}" TARGET_DEPENDENCIES ${target_dependencies})set_property( SOURCE ${_generated_c_files} PROPERTY GENERATED 1)# команда вызова функции скрипта из папки binadd_custom_command( OUTPUT ${_generated_c_files} COMMAND Python3::Interpreter ARGS ${rosidl_generator_lua_BIN} --generator-arguments-file "${generator_arguments_file}" --typesupport-impls "${_typesupport_impls}" DEPENDS ${target_dependencies} COMMENT "Generating code for ROS interfaces" VERBATIM)# генерация C файловadd_library(${_target_name_lib} SHARED ${_generated_c_files})target_link_libraries(${_target_name_lib} ${rosidl_generate_interfaces_TARGET}__rosidl_generator_c)add_dependencies( ${_target_name_lib} ${rosidl_generate_interfaces_TARGET}${_target_suffix} ${rosidl_generate_interfaces_TARGET}__rosidl_typesupport_c)rosidl_get_typesupport_target(c_typesupport_target "${rosidl_generate_interfaces_TARGET}" "rosidl_typesupport_c")# сборка сообщенийif(NOT _msg_list STREQUAL "") add_library(msg SHARED ${_msg_list}) set_target_properties(msg PROPERTIES PREFIX "" LIBRARY_OUTPUT_DIRECTORY ${_output_path} ) target_link_libraries(msg ${c_typesupport_target}) ament_target_dependencies(msg "rosidl_runtime_c")endif()# аналогично для сервисов и экшенов
Данный скрипт формирует список сообщений и их зависимостей, на основе собранных данных формирует JSON файл с описанием пакета и передаёт его генератору кода, после чего запускается компиляция динамических библиотек.
Сборка
Осталось сделать ещё несколько шагов, чтобы выполнить сборку. Во-первых, нужно создать в корне файл rosidl_generator_lua-extras.cmake.in с текстом
include("${CMAKE_CURRENT_LIST_DIR}/register_lua.cmake")rosidl_generator_lua_extras( "${rosidl_generator_lua_DIR}/../../../lib/rosidl_generator_lua/rosidl_generator_lua" "${rosidl_generator_lua_DIR}/../../../@PYTHON_INSTALL_DIR@/rosidl_generator_lua/__init__.py" "${rosidl_generator_lua_DIR}/../resource")
Здесь прописаны пути к файлам и папкам, используемым в процессе генерации. Во-вторых, в package.xml добавляем зависимости:
<buildtool_depend>ament_cmake_export_assemblies</buildtool_depend> <buildtool_export_depend>ament_cmake</buildtool_export_depend> <buildtool_export_depend>rosidl_cmake</buildtool_export_depend> <buildtool_export_depend>rosidl_generator_c</buildtool_export_depend> <buildtool_export_depend>rosidl_typesupport_c</buildtool_export_depend> <buildtool_export_depend>rosidl_typesupport_interface</buildtool_export_depend> <build_depend>rosidl_runtime_c</build_depend> <exec_depend>rmw_implementation</exec_depend> <exec_depend>rmw_implementation_cmake</exec_depend> <exec_depend>rosidl_runtime_c</exec_depend> <exec_depend>rosidl_generator_c</exec_depend> <exec_depend>rosidl_parser</exec_depend> <member_of_group>rosidl_generator_packages</member_of_group>
Наконец, настраиваем сборку через CMakeLists.txt.
find_package(ament_cmake REQUIRED)find_package(ament_cmake_python REQUIRED)ament_export_dependencies(rosidl_cmake)ament_export_dependencies(rmw)ament_index_register_resource("rosidl_generator_packages")ament_python_install_package(${PROJECT_NAME})install( PROGRAMS bin/rosidl_generator_lua DESTINATION lib/rosidl_generator_lua)install( DIRECTORY cmake resource DESTINATION share/${PROJECT_NAME})# добавляем свои сценарии сборкиament_package( CONFIG_EXTRAS "cmake/rosidl_generator_lua_get_typesupports.cmake" "cmake/register_lua.cmake" "rosidl_generator_lua-extras.cmake.in")
Стандартные сообщения
Представленный выше код справляется с генерацией сообщений, но есть нюанс. Он ориентирован на работу с кастомными сообщениями, т.е. описание которых лежит в вашем локальном рабочем окружении. Но ROS включает в себя множество стандартных типов, таких как std_msgs, nav_msgs, sensor_msgs и т.д. И нужно как-то обеспечить возможность работы с ними.
Можно попытаться собрать стандартные библиотеки в своём локальном рабочем окружении. Однако в этом случае возникнет конфликт с глобальным окружением и ROS завершит работу. Я решил проблему следующим образом. Описания стандартных сообщений, т.е. msg и idl файлы, лежат в папке ros_distro_name/share. Я добавил отдельный пакет, который считывает эти описания, генерирует нужные динамические библиотеки и добавляет пути в LUA_CPATH. Это позволило иметь локальные версии библиотек не конфликтуя с глобальным окружением. Правда, пришлось использовать непубличные функции из ament_cmake, поэтому при обновлении версии ROS2 этот код, скорее всего, придётся дорабатывать.
Заключение
Мы рассмотрели, что нужно следать для того, чтобы генератор ROS2 сообщений подхватил ваши шаблоны и на их основе сформировал библиотеку под заданный язык программирования. Теперь можно переходить к имплементации издателей, подписчиков и прочего функционала, тесно связанного с хранением и обменом данными.
ссылка на оригинал статьи https://habr.com/ru/articles/1033622/