From 7e4b5460c2f701a3d0cf93734d5424bda843adec Mon Sep 17 00:00:00 2001 From: ATT_POWER <34850640+yglsaltfish@users.noreply.github.com> Date: Thu, 31 Oct 2024 20:03:42 +0800 Subject: [PATCH] feat: add a new plugin: echo (#51) * CI: change build worlflow image tag from v20240927 to latest * feat/plugins: Add echo plugin - Add echo plugin for message pass-through and logging - Implement loading and management of different types of support packages - Add support for multi-threaded executors - Optimize log initialization and management logic - Add support for YAML configuration files * build: Update build scripts and enable Echo plugin - Add build options for Echo plugin in build.bat, build.sh, test.bat, and test.sh scripts - Optimize JSON serialization error handling and log output in echo_plugin.cc * chore: Add Echo plugin related documentation - Update release notes with Echo plugin feature description - Add Echo plugin usage documentation - Add Echo plugin example configuration and running instructions * chore: format the code - Add thread safety checks for Echo executor - Optimize code structure to improve readability and maintainability * choro: restructure JSON parsing logic - Replace json-c with jsoncpp library - Rewrite json_to_yaml function to improve code readability and robustness * choro: adapt CMakeLists for Windows compatibility * choro : turn off the echo plugin in windows * feat(echo_plugin): Support YAML format message echo - Modify GetYamlCpp.cmake to enable yaml-cpp installation - Add yaml_convert.h and yaml_convert_test.cc in ros2_util - Update echo_plugin to support YAML format message serialization and deserialization - Add YAML serialization type support in aimrt_module_ros2_interface * choro: Fix documentation * choro : fix format and delete the extra support for echo about pb message type * choro : format * choro: add ros2 example for echo plugin and delete useless code * choro: rename the echo example shell * choro: fix some mistake * chore: refactor type support package loader and migrate to core module - Migrate TypeSupportPkgLoader from echo_plugin and record_playback_plugin to core/util directory - Optimize log output, replace printf with AIMRT_INFO * choro: replace the aimrt::common::util::AimRTException() to AIMRT_ASSERT --------- Co-authored-by: yuguanlin --- CMakeLists.txt | 1 + build.bat | 1 + build.sh | 1 + document/sphinx-cn/release_notes/v0_9_0.md | 1 + document/sphinx-cn/tutorials/index.md | 1 + .../tutorials/plugins/echo_plugin.md | 77 ++++ src/common/ros2_util/CMakeLists.txt | 3 +- src/common/ros2_util/yaml_convert.h | 232 ++++++++++ src/common/ros2_util/yaml_convert_test.cc | 402 ++++++++++++++++++ src/common/ros2_util/yaml_cpp_utils.h | 85 ++++ src/examples/plugins/CMakeLists.txt | 4 + .../plugins/echo_plugin/CMakeLists.txt | 30 ++ src/examples/plugins/echo_plugin/README.md | 61 +++ .../example_event_ts_pkg/CMakeLists.txt | 38 ++ .../type_support_pkg_main.cc | 27 ++ .../examples_plugins_echo_plugin_pb_cfg.yaml | 51 +++ ...s_plugins_echo_plugin_pb_executor_cfg.yaml | 54 +++ ...examples_plugins_echo_plugin_ros2_cfg.yaml | 54 +++ ...art_examples_plugins_echo_plugin_pb_cfg.sh | 3 + ...les_plugins_echo_plugin_pb_executor_cfg.sh | 3 + ...t_examples_plugins_echo_plugin_ros2_cfg.sh | 3 + .../util/ros2_type_support.h | 29 +- src/plugins/CMakeLists.txt | 4 + src/plugins/echo_plugin/CMakeLists.txt | 53 +++ src/plugins/echo_plugin/echo_plugin.cc | 336 +++++++++++++++ src/plugins/echo_plugin/echo_plugin.h | 85 ++++ src/plugins/echo_plugin/echo_plugin_main.cc | 16 + src/plugins/echo_plugin/global.cc | 15 + src/plugins/echo_plugin/global.h | 13 + src/plugins/echo_plugin/topic_meta_key.h | 35 ++ .../record_playback_plugin.cc | 2 +- .../record_playback_plugin.h | 7 +- .../core/util}/type_support_pkg_loader.cc | 35 +- .../core/util}/type_support_pkg_loader.h | 4 +- test.bat | 1 + test.sh | 1 + 36 files changed, 1744 insertions(+), 24 deletions(-) create mode 100644 document/sphinx-cn/tutorials/plugins/echo_plugin.md create mode 100644 src/common/ros2_util/yaml_convert.h create mode 100644 src/common/ros2_util/yaml_convert_test.cc create mode 100644 src/common/ros2_util/yaml_cpp_utils.h create mode 100644 src/examples/plugins/echo_plugin/CMakeLists.txt create mode 100644 src/examples/plugins/echo_plugin/README.md create mode 100644 src/examples/plugins/echo_plugin/example_event_ts_pkg/CMakeLists.txt create mode 100644 src/examples/plugins/echo_plugin/example_event_ts_pkg/type_support_pkg_main.cc create mode 100644 src/examples/plugins/echo_plugin/install/linux/bin/cfg/examples_plugins_echo_plugin_pb_cfg.yaml create mode 100644 src/examples/plugins/echo_plugin/install/linux/bin/cfg/examples_plugins_echo_plugin_pb_executor_cfg.yaml create mode 100644 src/examples/plugins/echo_plugin/install/linux/bin/cfg/examples_plugins_echo_plugin_ros2_cfg.yaml create mode 100755 src/examples/plugins/echo_plugin/install/linux/bin/start_examples_plugins_echo_plugin_pb_cfg.sh create mode 100755 src/examples/plugins/echo_plugin/install/linux/bin/start_examples_plugins_echo_plugin_pb_executor_cfg.sh create mode 100755 src/examples/plugins/echo_plugin/install/linux/bin/start_examples_plugins_echo_plugin_ros2_cfg.sh create mode 100644 src/plugins/echo_plugin/CMakeLists.txt create mode 100644 src/plugins/echo_plugin/echo_plugin.cc create mode 100644 src/plugins/echo_plugin/echo_plugin.h create mode 100644 src/plugins/echo_plugin/echo_plugin_main.cc create mode 100644 src/plugins/echo_plugin/global.cc create mode 100644 src/plugins/echo_plugin/global.h create mode 100644 src/plugins/echo_plugin/topic_meta_key.h rename src/{plugins/record_playback_plugin => runtime/core/util}/type_support_pkg_loader.cc (52%) rename src/{plugins/record_playback_plugin => runtime/core/util}/type_support_pkg_loader.h (88%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 412774475..63f7f67fd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -37,6 +37,7 @@ option(AIMRT_BUILD_PARAMETER_PLUGIN "AimRT build parameter plugin." OFF) option(AIMRT_BUILD_LOG_CONTROL_PLUGIN "AimRT build log control plugin." OFF) option(AIMRT_BUILD_OPENTELEMETRY_PLUGIN "AimRT build opentelemetry plugin." OFF) option(AIMRT_BUILD_GRPC_PLUGIN "AimRT build grpc plugin." OFF) +option(AIMRT_BUILD_ECHO_PLUGIN "AimRT build echo plugin." OFF) option(AIMRT_INSTALL "Enable installation of AimRT." ON) diff --git a/build.bat b/build.bat index 114211b79..5d6394645 100644 --- a/build.bat +++ b/build.bat @@ -28,6 +28,7 @@ cmake -B build ^ -DAIMRT_BUILD_LOG_CONTROL_PLUGIN=ON ^ -DAIMRT_BUILD_OPENTELEMETRY_PLUGIN=OFF ^ -DAIMRT_BUILD_GRPC_PLUGIN=OFF ^ + -DAIMRT_BUILD_ECHO_PLUGIN=OFF ^ -DAIMRT_BUILD_PYTHON_PACKAGE=ON ^ %* diff --git a/build.sh b/build.sh index b9348e0e4..ae74ff075 100755 --- a/build.sh +++ b/build.sh @@ -30,6 +30,7 @@ cmake -B build \ -DAIMRT_BUILD_LOG_CONTROL_PLUGIN=ON \ -DAIMRT_BUILD_OPENTELEMETRY_PLUGIN=ON \ -DAIMRT_BUILD_GRPC_PLUGIN=ON \ + -DAIMRT_BUILD_ECHO_PLUGIN=ON \ -DAIMRT_BUILD_PYTHON_PACKAGE=ON \ $@ diff --git a/document/sphinx-cn/release_notes/v0_9_0.md b/document/sphinx-cn/release_notes/v0_9_0.md index 7327ed7da..1d61eddeb 100644 --- a/document/sphinx-cn/release_notes/v0_9_0.md +++ b/document/sphinx-cn/release_notes/v0_9_0.md @@ -9,6 +9,7 @@ - mqtt 新增配置项以支持加密传输； - 新增了第三方库 asio，runtime::core 不再引用 boost，改为引用独立的 asio 库，以减轻依赖； - 新增 bagtrans 命令行工具，用于将 使用 aimrt record_playback 插件录制的 bag 文件转换为 ros2 的 bag 文件； +- 新增 Echo 插件，用于回显消息； **次要修改**： - 缩短了一些 examples 的文件路径长度； diff --git a/document/sphinx-cn/tutorials/index.md b/document/sphinx-cn/tutorials/index.md index c942b3dbe..723cfbd58 100644 --- a/document/sphinx-cn/tutorials/index.md +++ b/document/sphinx-cn/tutorials/index.md @@ -102,6 +102,7 @@ plugins/record_playback_plugin.md plugins/zenoh_plugin.md plugins/iceoryx_plugin.md plugins/grpc_plugin.md +plugins/echo_plugin.md ``` 如果开发者想定制开发自己的插件，可以参考以下文档。 diff --git a/document/sphinx-cn/tutorials/plugins/echo_plugin.md b/document/sphinx-cn/tutorials/plugins/echo_plugin.md new file mode 100644 index 000000000..05362d276 --- /dev/null +++ b/document/sphinx-cn/tutorials/plugins/echo_plugin.md @@ -0,0 +1,77 @@ + +# echo插件 + +## 相关链接 + +参考示例： +- {{ '[echo_plugin]({}/src/examples/plugins/echo_plugin)'.format(code_site_root_path_url) }} + +## 插件概述 + +**echo_plugin**用于对 Channel 中的消息进行回显，插件支持独立的 type_support_pkg，并支持指定执行器。 + +插件的配置项如下： + +| 节点 | 类型 | 是否可选| 默认值 | 作用 | +| ---- | ---- | ---- | ---- | ---- | +| type_support_pkgs | array | 必选 | [] | type support 包配置 | +| type_support_pkgs[i].path | string | 必选 | "" | type support 包的路径 | +| executor | string | 可选 | "" | 回显使用的执行器，要求必须是线程安全 | +| topic_meta_list | array | 必选 | [] | 要回显的 topic 和类型 | +| topic_meta_list[j].topic_name | string | 必选 | "" | 要回显的 topic | +| topic_meta_list[j].msg_type | string | 必选 | "" | 要回显的消息类型 | +| topic_meta_list[j].echo_type | string | 可选 | "json" | 回显消息的格式，ros2 支持 "json", "yaml" ， pb 只支持 "json" | + + + +### 回显消息的简单示例配置 +回显消息的存在两种配置，分别是 是否指定执行器 和 回显消息的格式： +- 是否指定执行器： 插件会使用指定的执行器来处理回显消息，如果未指定执行器，则使用默认的执行器； +- 回显消息的格式： ros2 消息类型 支持 "json", "yaml" ， pb只支持 "json" + +以下是一个带执行器的回显消息格式为 json 的简单示例配置： +```yaml +aimrt: + plugin: + plugins: + - name: echo_plugin + path: ./libaimrt_echo_plugin.so + options: + type_support_pkgs: + - path: ./libexample_event_ts_pkg.so + executor: echo_executor + topic_meta_list: + - topic_name: test_topic + msg_type: pb:aimrt.protocols.example.ExampleEventMsg + echo_type: json + executor: + executors: + - name: echo_executor + type: simple_thread + channel: + # ... +``` + + +以下是一个不带执行器的回显消息格式为 json 的简单示例配置： +```yaml +aimrt: + plugin: + plugins: + - name: echo_plugin + path: ./libaimrt_echo_plugin.so + options: + type_support_pkgs: + - path: ./libexample_event_ts_pkg.so + topic_meta_list: + - topic_name: test_topic + msg_type: pb:aimrt.protocols.example.ExampleEventMsg + echo_type: json + executor: + executors: + - name: echo_executor + type: simple_thread + channel: + # ... +``` + diff --git a/src/common/ros2_util/CMakeLists.txt b/src/common/ros2_util/CMakeLists.txt index 477dfc02d..09655f7b4 100644 --- a/src/common/ros2_util/CMakeLists.txt +++ b/src/common/ros2_util/CMakeLists.txt @@ -33,7 +33,8 @@ target_sources(${CUR_TARGET_NAME} INTERFACE FILE_SET HEADERS BASE_DIRS ${CMAKE_C target_link_libraries( ${CUR_TARGET_NAME} INTERFACE rclcpp::rclcpp - jsoncpp::jsoncpp) + jsoncpp::jsoncpp + yaml-cpp::yaml-cpp) # Set installation of target if(AIMRT_INSTALL) diff --git a/src/common/ros2_util/yaml_convert.h b/src/common/ros2_util/yaml_convert.h new file mode 100644 index 000000000..2a3e0491b --- /dev/null +++ b/src/common/ros2_util/yaml_convert.h @@ -0,0 +1,232 @@ +// Copyright (c) 2023, AgiBot Inc. +// All rights reserved. + +#pragma once + +#include +#include + +#include + +#include "rosidl_typesupport_cpp/message_type_support.hpp" +#include "rosidl_typesupport_introspection_cpp/field_types.hpp" +#include "rosidl_typesupport_introspection_cpp/identifier.hpp" +#include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" + +#include "ros2_util/rostype_mapping.h" +#include "ros2_util/type_support_util.h" +#include "ros2_util/yaml_cpp_utils.h" + +namespace aimrt::common::ros2_util { + +namespace yaml_convert_impl { + +inline bool IsSequence(const rosidl_typesupport_introspection_cpp::MessageMember &member) { + return ((member.is_array_ && member.array_size_ == 0) || member.is_upper_bound_); +} + +template +inline void WriteSequenceMemberItem(const YAML::Node &yaml, uint8_t *buffer) { + using CppType = typename TypeMappingCpp::CppType; + using SequenceType = typename TypeMappingCpp::SequenceType; + reinterpret_cast(buffer)->push_back(GetYamlValue(yaml)); +} + +template +inline void WriteSequence( + const YAML::Node &yaml, + uint8_t *buffer, + const rosidl_typesupport_introspection_cpp::MessageMember &member) { + if (member.is_upper_bound_ && yaml.size() > member.array_size_) + throw std::runtime_error("WriteSequence: upper bound exceeded"); + + for (unsigned int i = 0; i < yaml.size(); i++) { + WriteSequenceMemberItem(yaml[i], buffer); + } +} + +template +inline void WriteMemberItem(const YAML::Node &yaml, uint8_t *buffer) { + using CppType = typename TypeMappingCpp::CppType; + *reinterpret_cast(buffer) = GetYamlValue(yaml); +} + +template +inline void WriteMember( + const YAML::Node &yaml, + uint8_t *buffer, + const rosidl_typesupport_introspection_cpp::MessageMember &member) { + using CppType = typename TypeMappingCpp::CppType; + + if (IsSequence(member)) { + if (yaml[member.name_].IsSequence()) { + WriteSequence(yaml[member.name_], buffer + member.offset_, member); + return; + } + throw std::runtime_error("YamlToMessage: yaml member is not a sequence"); + } + + if (member.is_array_) { + if (yaml[member.name_].IsSequence() && member.array_size_ == yaml[member.name_].size()) { + for (unsigned int i = 0; i < member.array_size_; i++) { + WriteMemberItem(yaml[member.name_][i], buffer + member.offset_ + sizeof(CppType) * i); + } + return; + } + throw std::runtime_error("YamlToMessage: yaml member is not a sequence or size not match"); + } + + WriteMemberItem(yaml[member.name_], buffer + member.offset_); +} + +static void YamlToMessageImpl( + const YAML::Node &root, + const rosidl_typesupport_introspection_cpp::MessageMembers *member_info, + uint8_t *buffer); + +inline void WriteMemberSequenceNested( + const YAML::Node &yaml, + uint8_t *buffer, + const rosidl_typesupport_introspection_cpp::MessageMember &member) { + if (member.is_upper_bound_ && yaml.size() > member.array_size_) + throw std::runtime_error("Yaml sequence is more than capacity"); + + const auto *member_typeinfo = + reinterpret_cast(member.members_->data); + auto &seq = buffer; + member.resize_function(seq, yaml.size()); + for (unsigned int i = 0; i < yaml.size(); i++) { + YamlToMessageImpl(yaml[i], member_typeinfo, + reinterpret_cast(member.get_function(seq, i))); + } +} + +inline void WriteMemberNested( + const YAML::Node &yaml, + uint8_t *buffer, + const rosidl_typesupport_introspection_cpp::MessageMember &member) { + if (IsSequence(member)) { + if (yaml[member.name_].IsSequence()) { + WriteMemberSequenceNested(yaml[member.name_], buffer + member.offset_, member); + return; + } + throw std::runtime_error("WriteMemberNested but the yaml is not sequence!"); + } + + const auto *member_typeinfo = + reinterpret_cast(member.members_->data); + if (member.is_array_) { + for (unsigned int i = 0; i < yaml[member.name_].size(); i++) { + YamlToMessageImpl(yaml[member.name_][i], member_typeinfo, + buffer + member.offset_ + member_typeinfo->size_of_ * i); + } + } else { + YamlToMessageImpl(yaml[member.name_], member_typeinfo, buffer + member.offset_); + } +} + +static void YamlToMessageImpl( + const YAML::Node &root, + const rosidl_typesupport_introspection_cpp::MessageMembers *member_info, + uint8_t *buffer) { + for (uint32_t i = 0; i < member_info->member_count_; i++) { + const auto &member = member_info->members_[i]; + + if (!root[member.name_]) + continue; + + switch (member.type_id_) { + case rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT: + WriteMember(root, buffer, member); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_DOUBLE: + WriteMember(root, buffer, member); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_LONG_DOUBLE: + WriteMember(root, buffer, member); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR: + WriteMember(root, buffer, member); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_WCHAR: + WriteMember(root, buffer, member); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOLEAN: + WriteMember(root, buffer, member); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_OCTET: + WriteMember(root, buffer, member); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8: + WriteMember(root, buffer, member); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8: + WriteMember(root, buffer, member); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16: + WriteMember(root, buffer, member); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16: + WriteMember(root, buffer, member); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32: + WriteMember(root, buffer, member); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32: + WriteMember(root, buffer, member); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64: + WriteMember(root, buffer, member); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64: + WriteMember(root, buffer, member); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: + WriteMember(root, buffer, member); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING: + throw std::runtime_error("Not support wstring."); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE: + WriteMemberNested(root, buffer, member); + break; + default: + throw std::runtime_error("Current ros msg type is not support."); + } + } +} + +} // namespace yaml_convert_impl + +inline bool YamlToMessage( + const std::string &yaml_str, + const rosidl_message_type_support_t *typesupport, + void *message) { + using namespace yaml_convert_impl; + + if (message == nullptr) [[unlikely]] + return false; + + const auto *member_info = GetRosMembersInfo(typesupport); + if (member_info == nullptr) [[unlikely]] + return false; + + YAML::Node root; + try { + root = YAML::Load(yaml_str); + } catch (...) { + return false; + } + + uint8_t *buffer = reinterpret_cast(message); + + try { + YamlToMessageImpl(root, member_info, buffer); + } catch (...) { + return false; + } + + return true; +} + +} // namespace aimrt::common::ros2_util \ No newline at end of file diff --git a/src/common/ros2_util/yaml_convert_test.cc b/src/common/ros2_util/yaml_convert_test.cc new file mode 100644 index 000000000..de3647d64 --- /dev/null +++ b/src/common/ros2_util/yaml_convert_test.cc @@ -0,0 +1,402 @@ +// Copyright (c) 2023, AgiBot Inc. +// All rights reserved. + +#include + +#include +#include + +#include "ros2_util/yaml_convert.h" + +#include "example_interfaces/msg/bool.hpp" +#include "example_interfaces/msg/byte.hpp" +#include "example_interfaces/msg/char.hpp" +#include "example_interfaces/msg/float32.hpp" +#include "example_interfaces/msg/float64.hpp" +#include "example_interfaces/msg/int16.hpp" +#include "example_interfaces/msg/int32.hpp" +#include "example_interfaces/msg/int64.hpp" +#include "example_interfaces/msg/int8.hpp" +#include "example_interfaces/msg/string.hpp" +#include "example_interfaces/msg/u_int16.hpp" +#include "example_interfaces/msg/u_int32.hpp" +#include "example_interfaces/msg/u_int64.hpp" +#include "example_interfaces/msg/u_int8.hpp" +#include "example_interfaces/msg/w_string.hpp" +#include "rosidl_typesupport_cpp/message_type_support.hpp" +#include "test_msgs/msg/arrays.hpp" +#include "test_msgs/msg/basic_types.hpp" +#include "test_msgs/msg/bounded_sequences.hpp" +#include "test_msgs/msg/multi_nested.hpp" +#include "test_msgs/msg/nested.hpp" +#include "test_msgs/msg/strings.hpp" +#include "test_msgs/msg/unbounded_sequences.hpp" +#include "test_msgs/msg/w_strings.hpp" + +namespace aimrt::common::ros2_util { + +template +bool TestYamlToRos2Message( + const std::string& yaml_str, + const RosType& check_message, + std::function check_func = + [](const RosType& lhs, const RosType& rhs) -> bool { return lhs == rhs; }) { + RosType message; + bool ret = YamlToMessage(yaml_str, GetIntrospectionTypeSupport(), &message); + if (!ret) return false; + return check_func(message, check_message); +} + +template +typename std::enable_if::is_integer, bool>::type +AlmostEqual(T x, T y, int ulp) { + return std::fabs(x - y) <= std::numeric_limits::epsilon() * std::fabs(x + y) * ulp || + std::fabs(x - y) < std::numeric_limits::min(); +} + +TEST(YamlToRos2Message, BasicType_Bool) { + using RosType = ::example_interfaces::msg::Bool; + + { + std::string yaml_str = "data: true"; + RosType check_message; + check_message.data = true; + EXPECT_TRUE(TestYamlToRos2Message(yaml_str, check_message)); + } + + { + std::string yaml_str = "data: false"; + RosType check_message; + check_message.data = false; + EXPECT_TRUE(TestYamlToRos2Message(yaml_str, check_message)); + } +} + +TEST(YamlToRos2Message, BasicType_Float32) { + using RosType = ::example_interfaces::msg::Float32; + + auto check_func = [](const RosType& lhs, const RosType& rhs) -> bool { + return AlmostEqual(lhs.data, rhs.data, 2); + }; + + { + std::string yaml_str = "data: 3.14159"; + RosType check_message; + check_message.data = 3.14159f; + EXPECT_TRUE(TestYamlToRos2Message(yaml_str, check_message)); + } + + { + std::string yaml_str = "data: -2.71828"; + RosType check_message; + check_message.data = -2.71828f; + EXPECT_TRUE(TestYamlToRos2Message(yaml_str, check_message)); + } +} + +TEST(YamlToRos2Message, BasicType_Float64) { + using RosType = ::example_interfaces::msg::Float64; + + auto check_func = [](const RosType& lhs, const RosType& rhs) -> bool { + return AlmostEqual(lhs.data, rhs.data, 2); + }; + + { + std::string yaml_str = "data: 42424242424242.42"; + RosType check_message; + check_message.data = 42424242424242.42; + EXPECT_TRUE(TestYamlToRos2Message(yaml_str, check_message, check_func)); + } +} + +TEST(YamlToRos2Message, BasicType_Int8) { + using RosType = ::example_interfaces::msg::Int8; + { + std::string yaml_str = "data: 127"; // max value + RosType check_message; + check_message.data = std::numeric_limits::max(); + EXPECT_TRUE(TestYamlToRos2Message(yaml_str, check_message)); + } + + { + std::string yaml_str = "data: -128"; // min value + RosType check_message; + check_message.data = std::numeric_limits::min(); + EXPECT_TRUE(TestYamlToRos2Message(yaml_str, check_message)); + } +} + +TEST(YamlToRos2Message, BasicType_UInt8) { + using RosType = ::example_interfaces::msg::UInt8; + { + std::string yaml_str = "data: 255"; // max value + RosType check_message; + check_message.data = std::numeric_limits::max(); + EXPECT_TRUE(TestYamlToRos2Message(yaml_str, check_message)); + } + + { + std::string yaml_str = "data: 0"; // min value + RosType check_message; + check_message.data = std::numeric_limits::min(); + EXPECT_TRUE(TestYamlToRos2Message(yaml_str, check_message)); + } +} + +TEST(YamlToRos2Message, BasicType_Int16) { + using RosType = ::example_interfaces::msg::Int16; + + { + std::string yaml_str = "data: 32767"; + RosType check_message; + check_message.data = std::numeric_limits::max(); + EXPECT_TRUE(TestYamlToRos2Message(yaml_str, check_message)); + } + + { + std::string yaml_str = "data: -32768"; + RosType check_message; + check_message.data = std::numeric_limits::min(); + EXPECT_TRUE(TestYamlToRos2Message(yaml_str, check_message)); + } +} + +TEST(YamlToRos2Message, BasicType_UInt16) { + using RosType = ::example_interfaces::msg::UInt16; + + { + std::string yaml_str = "data: 65535"; + RosType check_message; + check_message.data = std::numeric_limits::max(); + EXPECT_TRUE(TestYamlToRos2Message(yaml_str, check_message)); + } + + { + std::string yaml_str = "data: 0"; + RosType check_message; + check_message.data = std::numeric_limits::min(); + EXPECT_TRUE(TestYamlToRos2Message(yaml_str, check_message)); + } +} + +TEST(YamlToRos2Message, BasicType_Int32) { + using RosType = ::example_interfaces::msg::Int32; + + { + std::string yaml_str = "data: 2147483647"; + RosType check_message; + check_message.data = std::numeric_limits::max(); + EXPECT_TRUE(TestYamlToRos2Message(yaml_str, check_message)); + } + + { + std::string yaml_str = "data: -2147483648"; + RosType check_message; + check_message.data = std::numeric_limits::min(); + EXPECT_TRUE(TestYamlToRos2Message(yaml_str, check_message)); + } +} + +TEST(YamlToRos2Message, BasicType_UInt32) { + using RosType = ::example_interfaces::msg::UInt32; + + { + std::string yaml_str = "data: 4294967295"; + RosType check_message; + check_message.data = std::numeric_limits::max(); + EXPECT_TRUE(TestYamlToRos2Message(yaml_str, check_message)); + } + + { + std::string yaml_str = "data: 0"; + RosType check_message; + check_message.data = std::numeric_limits::min(); + EXPECT_TRUE(TestYamlToRos2Message(yaml_str, check_message)); + } +} + +TEST(YamlToRos2Message, BasicType_Int64) { + using RosType = ::example_interfaces::msg::Int64; + + { + std::string yaml_str = "data: 9223372036854775807"; + RosType check_message; + check_message.data = std::numeric_limits::max(); + EXPECT_TRUE(TestYamlToRos2Message(yaml_str, check_message)); + } + + { + std::string yaml_str = "data: -9223372036854775808"; + RosType check_message; + check_message.data = std::numeric_limits::min(); + EXPECT_TRUE(TestYamlToRos2Message(yaml_str, check_message)); + } +} + +TEST(YamlToRos2Message, BasicType_UInt64) { + using RosType = ::example_interfaces::msg::UInt64; + + { + std::string yaml_str = "data: 18446744073709551615"; + RosType check_message; + check_message.data = std::numeric_limits::max(); + EXPECT_TRUE(TestYamlToRos2Message(yaml_str, check_message)); + } + + { + std::string yaml_str = "data: 0"; + RosType check_message; + check_message.data = std::numeric_limits::min(); + EXPECT_TRUE(TestYamlToRos2Message(yaml_str, check_message)); + } +} + +TEST(YamlToRos2Message, BasicType_String) { + using RosType = ::example_interfaces::msg::String; + const rosidl_message_type_support_t* type_support = GetIntrospectionTypeSupport(); + + { + std::string yaml_str = "data: \"Hello World\""; + RosType msg; + bool ret = YamlToMessage(yaml_str, type_support, &msg); + ASSERT_TRUE(ret); + EXPECT_EQ(msg.data, "Hello World"); + } + + { + std::string yaml_str = "data: \"\""; // empty string + RosType msg; + bool ret = YamlToMessage(yaml_str, type_support, &msg); + ASSERT_TRUE(ret); + EXPECT_TRUE(msg.data.empty()); + } +} + +TEST(YamlToRos2Message, BasicTypes_Complex) { + using RosType = ::test_msgs::msg::BasicTypes; + const rosidl_message_type_support_t* type_support = GetIntrospectionTypeSupport(); + + std::string yaml_str = R"( +bool_value: true +byte_value: 255 +char_value: 100 +float32_value: 1.125 +float64_value: -2.125 +int8_value: 127 +uint8_value: 255 +int16_value: 32767 +uint16_value: 65535 +int32_value: 2147483647 +uint32_value: 4294967295 +int64_value: 9223372036854775807 +uint64_value: 18446744073709551615 + )"; + + RosType msg; + bool ret = YamlToMessage(yaml_str, type_support, &msg); + ASSERT_TRUE(ret); + + EXPECT_TRUE(msg.bool_value); + EXPECT_EQ(msg.byte_value, 255); + EXPECT_EQ(msg.char_value, 100); + EXPECT_FLOAT_EQ(msg.float32_value, 1.125f); + EXPECT_DOUBLE_EQ(msg.float64_value, -2.125); + EXPECT_EQ(msg.int8_value, 127); + EXPECT_EQ(msg.uint8_value, 255); + EXPECT_EQ(msg.int16_value, 32767); + EXPECT_EQ(msg.uint16_value, 65535); + EXPECT_EQ(msg.int32_value, 2147483647); + EXPECT_EQ(msg.uint32_value, 4294967295); + EXPECT_EQ(msg.int64_value, 9223372036854775807); + EXPECT_EQ(msg.uint64_value, 18446744073709551615ULL); +} + +TEST(YamlToRos2Message, SingleNest) { + using RosType = ::test_msgs::msg::Nested; + const rosidl_message_type_support_t* type_support = GetIntrospectionTypeSupport(); + std::string yaml_str = R"( + basic_types_value: + int8_value: 127 + int16_value: 32767 + int32_value: 2147483647 + int64_value: 9223372036854775807 + )"; + + RosType msg; + bool ret = YamlToMessage(yaml_str, type_support, &msg); + ASSERT_TRUE(ret); + EXPECT_EQ(msg.basic_types_value.int8_value, 127); + EXPECT_EQ(msg.basic_types_value.int16_value, 32767); + EXPECT_EQ(msg.basic_types_value.int32_value, 2147483647); + EXPECT_EQ(msg.basic_types_value.int64_value, 9223372036854775807); +} + +TEST(YamlToRos2Message, MultipleNest) { + using RosType = ::test_msgs::msg::MultiNested; + const rosidl_message_type_support_t* type_support = GetIntrospectionTypeSupport(); + std::string yaml_str = R"( + array_of_arrays: + - bool_values: [true, false, true] + byte_values: [255, 0, 255] + int32_values: [2147483647, -2147483648, 0] + string_values: ["test1", "test2", "test3"] + basic_types_values: + - bool_value: true + int32_value: 42 + - bool_value: false + int32_value: -42 + - bool_value: true + int32_value: 0 + array_of_bounded_sequences: + - bool_values: [true, false] + int32_values: [42, -42] + string_values: ["bounded1", "bounded2"] + - bool_values: [false, true] + int32_values: [-1, 1] + string_values: ["bounded3", "bounded4"] + - bool_values: [true, true] + int32_values: [0, 0] + string_values: ["bounded5", "bounded6"] + )"; + + RosType msg; + bool ret = YamlToMessage(yaml_str, type_support, &msg); + ASSERT_TRUE(ret); + + // test array_of_arrays + ASSERT_EQ(msg.array_of_arrays.size(), 3u); + EXPECT_TRUE(msg.array_of_arrays[0].bool_values[0]); + EXPECT_FALSE(msg.array_of_arrays[0].bool_values[1]); + EXPECT_TRUE(msg.array_of_arrays[0].bool_values[2]); + + EXPECT_EQ(msg.array_of_arrays[0].byte_values[0], 255); + EXPECT_EQ(msg.array_of_arrays[0].byte_values[1], 0); + EXPECT_EQ(msg.array_of_arrays[0].byte_values[2], 255); + + EXPECT_EQ(msg.array_of_arrays[0].int32_values[0], 2147483647); + EXPECT_EQ(msg.array_of_arrays[0].int32_values[1], -2147483648); + EXPECT_EQ(msg.array_of_arrays[0].int32_values[2], 0); + + EXPECT_EQ(msg.array_of_arrays[0].string_values[0], "test1"); + EXPECT_EQ(msg.array_of_arrays[0].string_values[1], "test2"); + EXPECT_EQ(msg.array_of_arrays[0].string_values[2], "test3"); + + EXPECT_EQ(msg.array_of_arrays[0].basic_types_values[0].bool_value, true); + EXPECT_EQ(msg.array_of_arrays[0].basic_types_values[0].int32_value, 42); + EXPECT_EQ(msg.array_of_arrays[0].basic_types_values[1].bool_value, false); + EXPECT_EQ(msg.array_of_arrays[0].basic_types_values[1].int32_value, -42); + EXPECT_EQ(msg.array_of_arrays[0].basic_types_values[2].bool_value, true); + EXPECT_EQ(msg.array_of_arrays[0].basic_types_values[2].int32_value, 0); + + // test array_of_bounded_sequences + ASSERT_EQ(msg.array_of_bounded_sequences.size(), 3u); + EXPECT_TRUE(msg.array_of_bounded_sequences[0].bool_values[0]); + EXPECT_FALSE(msg.array_of_bounded_sequences[0].bool_values[1]); + + EXPECT_EQ(msg.array_of_bounded_sequences[0].int32_values[0], 42); + EXPECT_EQ(msg.array_of_bounded_sequences[0].int32_values[1], -42); + + EXPECT_EQ(msg.array_of_bounded_sequences[0].string_values[0], "bounded1"); + EXPECT_EQ(msg.array_of_bounded_sequences[0].string_values[1], "bounded2"); +} +} // namespace aimrt::common::ros2_util diff --git a/src/common/ros2_util/yaml_cpp_utils.h b/src/common/ros2_util/yaml_cpp_utils.h new file mode 100644 index 000000000..7b301f249 --- /dev/null +++ b/src/common/ros2_util/yaml_cpp_utils.h @@ -0,0 +1,85 @@ +// Copyright (c) 2023, AgiBot Inc. +// All rights reserved. + +#pragma once + +#include "yaml-cpp/yaml.h" + +namespace aimrt::common::ros2_util { + +template +inline T GetYamlValue(const YAML::Node& node) { + return T{}; +} + +template <> +inline int GetYamlValue(const YAML::Node& node) { + return node.as(); +} + +template <> +inline uint64_t GetYamlValue(const YAML::Node& node) { + return node.as(); +} + +template <> +inline uint32_t GetYamlValue(const YAML::Node& node) { + return node.as(); +} + +template <> +inline uint16_t GetYamlValue(const YAML::Node& node) { + return node.as(); +} + +template <> +inline uint8_t GetYamlValue(const YAML::Node& node) { + return node.as(); +} + +template <> +inline int64_t GetYamlValue(const YAML::Node& node) { + return node.as(); +} + +template <> +inline int16_t GetYamlValue(const YAML::Node& node) { + return node.as(); +} + +template <> +inline int8_t GetYamlValue(const YAML::Node& node) { + return node.as(); +} + +template <> +inline double GetYamlValue(const YAML::Node& node) { + return node.as(); +} + +template <> +inline float GetYamlValue(const YAML::Node& node) { + return node.as(); +} + +template <> +inline bool GetYamlValue(const YAML::Node& node) { + return node.as(); +} + +template <> +inline std::string GetYamlValue(const YAML::Node& node) { + return node.as(); +} + +template <> +inline char GetYamlValue(const YAML::Node& node) { + return node.as(); +} + +template +inline T GetYamlValueOr(const YAML::Node& node, const T& default_value) { + return node ? GetYamlValue(node) : default_value; +} + +} // namespace aimrt::common::ros2_util \ No newline at end of file diff --git a/src/examples/plugins/CMakeLists.txt b/src/examples/plugins/CMakeLists.txt index 55c93a85d..c1efe166d 100644 --- a/src/examples/plugins/CMakeLists.txt +++ b/src/examples/plugins/CMakeLists.txt @@ -29,6 +29,10 @@ if(AIMRT_BUILD_WITH_PROTOBUF add_subdirectory(record_playback_plugin) endif() +if(AIMRT_BUILD_ECHO_PLUGIN) + add_subdirectory(echo_plugin) +endif() + if(AIMRT_BUILD_WITH_PROTOBUF AND AIMRT_BUILD_NET_PLUGIN AND AIMRT_BUILD_TIME_MANIPULATOR_PLUGIN) diff --git a/src/examples/plugins/echo_plugin/CMakeLists.txt b/src/examples/plugins/echo_plugin/CMakeLists.txt new file mode 100644 index 000000000..d8bd8ef4e --- /dev/null +++ b/src/examples/plugins/echo_plugin/CMakeLists.txt @@ -0,0 +1,30 @@ +# Copyright (c) 2023, AgiBot Inc. +# All rights reserved. + +# Get the current folder name +string(REGEX REPLACE ".*/\(.*\)" "\\1" CUR_DIR ${CMAKE_CURRENT_SOURCE_DIR}) + +# Set namespace +set_namespace() + +# type_support_pkg +add_subdirectory(example_event_ts_pkg) + +# install +if(CMAKE_SYSTEM_NAME MATCHES "Linux") + set(CUR_INSTALL_SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/install/linux) +elseif(CMAKE_SYSTEM_NAME MATCHES "Windows") + set(CUR_INSTALL_SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/install/win) +else() + message(FATAL_ERROR "Unsupport os") +endif() + +# build all +get_namespace(CUR_SUPERIOR_NAMESPACE) +string(REPLACE "::" "_" CUR_SUPERIOR_NAMESPACE_UNDERLINE ${CUR_SUPERIOR_NAMESPACE}) +add_custom_target( + ${CUR_SUPERIOR_NAMESPACE_UNDERLINE}_${CUR_DIR}_build_all ALL + COMMAND ${CMAKE_COMMAND} -E copy_directory ${CUR_INSTALL_SOURCE_DIR}/bin ${CMAKE_BINARY_DIR} + DEPENDS aimrt::runtime::main # + aimrt::examples::cpp::pb_chn::pb_chn_pub_pkg # + aimrt::examples::cpp::pb_chn::pb_chn_sub_pkg) diff --git a/src/examples/plugins/echo_plugin/README.md b/src/examples/plugins/echo_plugin/README.md new file mode 100644 index 000000000..f330e4a84 --- /dev/null +++ b/src/examples/plugins/echo_plugin/README.md @@ -0,0 +1,61 @@ +# echo plugin examples + +## echo with executor + +一个基于 **echo_plugin** 的带执行器的回显消息示例，演示内容包括： +- 如何在启动时加载 **echo_plugin**； +- 如何回显指定 topic、msg 类型的数据； +- 如何为 **echo_plugin** 配置执行器； + + +核心代码： +- [event.proto](../../../protocols/example/event.proto) +- [normal_publisher_module.cc](../../cpp/pb_chn/module/normal_publisher_module/normal_publisher_module.cc) +- [type_support_pkg_main.cc](./example_event_ts_pkg/type_support_pkg_main.cc) + + +配置文件： +- [examples_plugins_echo_plugin_executor_cfg.yaml](./install/linux/bin/cfg/examples_plugins_echo_plugin_executor_cfg.yaml) + + +运行方式（linux）： +- 开启 `AIMRT_BUILD_EXAMPLES`、`AIMRT_BUILD_WITH_PROTOBUF`、`AIMRT_BUILD_ECHO_PLUGIN` 选项编译 AimRT； +- 直接运行 build 目录下`start_examples_plugins_echo_plugin_executor_cfg.sh`脚本启动进程； +- 键入`ctrl-c`停止进程； + + +说明： +- 此示例创建了以下模块： + - `NormalPublisherModule`：会基于 `work_thread_pool` 执行器，以配置的频率、向配置的 topic 中发布 `ExampleEventMsg` 类型的消息； +- 此示例加载了 `example_event_ts_pkg`，其中提供了 `ExampleEventMsg` 类型的 type support 工具，作为回显时的序列化工具； +- 请注意，echo 插件的原理是向 AimRT 订阅指定的 Topic，因此需要在 channel 配置中为该 topic 设置合适的后端，以保证插件能接收到数据； + + +## echo without executor + +一个基于 **echo_plugin** 的回显消息示例，演示内容包括： +- 如何在启动时加载 **echo_plugin**； +- 如何通过 curl 命令触发式回显指定 topic、msg 类型的数据； + + +核心代码： +- [event.proto](../../../protocols/example/event.proto) +- [normal_publisher_module.cc](../../cpp/pb_chn/module/normal_publisher_module/normal_publisher_module.cc) +- [type_support_pkg_main.cc](./example_event_ts_pkg/type_support_pkg_main.cc) + + +配置文件： +- [examples_plugins_echo_plugin_cfg.yaml](./install/linux/bin/cfg/examples_plugins_echo_plugin_cfg.yaml) + + +运行方式（linux）： +- 开启 `AIMRT_BUILD_EXAMPLES`、`AIMRT_BUILD_WITH_PROTOBUF`、`AIMRT_BUILD_NET_PLUGIN` 选项编译 AimRT； +- 直接运行 build 目录下`start_examples_plugins_echo_plugin_cfg.sh`脚本启动进程； +- 键入`ctrl-c`停止进程； + + +说明： +- 此示例创建了以下模块： + - `NormalPublisherModule`：会基于 `work_thread_pool` 执行器，以配置的频率、向配置的 topic 中发布 `ExampleEventMsg` 类型的消息； +- 此示例加载了 `example_event_ts_pkg`，其中提供了 `ExampleEventMsg` 类型的 type support 工具，作为回显时的序列化工具； +- 请注意，echo 插件的原理是向 AimRT 订阅指定的 Topic，因此需要在 channel 配置中为该 topic 设置合适的后端，以保证插件能接收到数据； diff --git a/src/examples/plugins/echo_plugin/example_event_ts_pkg/CMakeLists.txt b/src/examples/plugins/echo_plugin/example_event_ts_pkg/CMakeLists.txt new file mode 100644 index 000000000..efd0a0e4f --- /dev/null +++ b/src/examples/plugins/echo_plugin/example_event_ts_pkg/CMakeLists.txt @@ -0,0 +1,38 @@ +# Copyright (c) 2023, AgiBot Inc. +# All rights reserved. + +# Get the current folder name +string(REGEX REPLACE ".*/\(.*\)" "\\1" CUR_DIR ${CMAKE_CURRENT_SOURCE_DIR}) + +# Get namespace +get_namespace(CUR_SUPERIOR_NAMESPACE) +string(REPLACE "::" "_" CUR_SUPERIOR_NAMESPACE_UNDERLINE ${CUR_SUPERIOR_NAMESPACE}) + +# Set target name +set(CUR_TARGET_NAME ${CUR_SUPERIOR_NAMESPACE_UNDERLINE}_${CUR_DIR}) +set(CUR_TARGET_ALIAS_NAME ${CUR_SUPERIOR_NAMESPACE}::${CUR_DIR}) + +# Set file collection +file(GLOB_RECURSE src ${CMAKE_CURRENT_SOURCE_DIR}/*.cc) + +# Add target +add_library(${CUR_TARGET_NAME} SHARED) +add_library(${CUR_TARGET_ALIAS_NAME} ALIAS ${CUR_TARGET_NAME}) + +# Set source file of target +target_sources(${CUR_TARGET_NAME} PRIVATE ${src}) + +# Set link libraries of target +target_link_libraries( + ${CUR_TARGET_NAME} + PRIVATE aimrt::interface::aimrt_type_support_pkg_c_interface + aimrt::interface::aimrt_module_protobuf_interface + aimrt::protocols::example_pb_gencode + aimrt::interface::aimrt_module_ros2_interface + example_ros2::example_ros2__rosidl_generator_cpp + example_ros2::example_ros2__rosidl_typesupport_cpp + example_ros2::example_ros2__rosidl_typesupport_fastrtps_cpp + example_ros2::example_ros2__rosidl_typesupport_introspection_cpp) + +# Set misc of target +set_target_properties(${CUR_TARGET_NAME} PROPERTIES OUTPUT_NAME ${CUR_DIR}) diff --git a/src/examples/plugins/echo_plugin/example_event_ts_pkg/type_support_pkg_main.cc b/src/examples/plugins/echo_plugin/example_event_ts_pkg/type_support_pkg_main.cc new file mode 100644 index 000000000..1eca903ec --- /dev/null +++ b/src/examples/plugins/echo_plugin/example_event_ts_pkg/type_support_pkg_main.cc @@ -0,0 +1,27 @@ +// Copyright (c) 2023, AgiBot Inc. +// All rights reserved. + +#include "aimrt_type_support_pkg_c_interface/type_support_pkg_main.h" + +#include "aimrt_module_protobuf_interface/util/protobuf_type_support.h" + +#include "aimrt_module_ros2_interface/util/ros2_type_support.h" + +#include "example_ros2/msg/ros_test_msg.hpp" + +#include "event.pb.h" + +static const aimrt_type_support_base_t* type_support_array[]{ + aimrt::GetProtobufMessageTypeSupport(), + aimrt::GetRos2MessageTypeSupport()}; + +extern "C" { + +size_t AimRTDynlibGetTypeSupportArrayLength() { + return sizeof(type_support_array) / sizeof(type_support_array[0]); +} + +const aimrt_type_support_base_t** AimRTDynlibGetTypeSupportArray() { + return type_support_array; +} +} \ No newline at end of file diff --git a/src/examples/plugins/echo_plugin/install/linux/bin/cfg/examples_plugins_echo_plugin_pb_cfg.yaml b/src/examples/plugins/echo_plugin/install/linux/bin/cfg/examples_plugins_echo_plugin_pb_cfg.yaml new file mode 100644 index 000000000..90d2d6850 --- /dev/null +++ b/src/examples/plugins/echo_plugin/install/linux/bin/cfg/examples_plugins_echo_plugin_pb_cfg.yaml @@ -0,0 +1,51 @@ +# Copyright (c) 2023, AgiBot Inc. +# All rights reserved. + +aimrt: + plugin: + plugins: + - name: echo_plugin + path: ./libaimrt_echo_plugin.so + options: + type_support_pkgs: + - path: ./libexample_event_ts_pkg.so + topic_meta_list: + - topic_name: test_topic + msg_type: pb:aimrt.protocols.example.ExampleEventMsg + echo_type: json + log: + core_lvl: Info # Trace/Debug/Info/Warn/Error/Fatal/Off + backends: + - type: console + executor: + executors: + - name: work_thread_pool + type: asio_thread + options: + thread_num: 4 + channel: + backends: + - type: local + options: + subscriber_use_inline_executor: false + subscriber_executor: work_thread_pool + pub_topics_options: + - topic_name: "(.*)" + enable_backends: [local] + sub_topics_options: + - topic_name: "(.*)" + enable_backends: [local] + module: + pkgs: + - path: ./libpb_chn_pub_pkg.so + enable_modules: [NormalPublisherModule] + modules: + - name: NormalPublisherModule + log_lvl: INFO + +# Module custom configuration +NormalPublisherModule: + topic_name: test_topic + channel_frq: 0.5 + + diff --git a/src/examples/plugins/echo_plugin/install/linux/bin/cfg/examples_plugins_echo_plugin_pb_executor_cfg.yaml b/src/examples/plugins/echo_plugin/install/linux/bin/cfg/examples_plugins_echo_plugin_pb_executor_cfg.yaml new file mode 100644 index 000000000..287102691 --- /dev/null +++ b/src/examples/plugins/echo_plugin/install/linux/bin/cfg/examples_plugins_echo_plugin_pb_executor_cfg.yaml @@ -0,0 +1,54 @@ +# Copyright (c) 2023, AgiBot Inc. +# All rights reserved. + +aimrt: + plugin: + plugins: + - name: echo_plugin + path: ./libaimrt_echo_plugin.so + options: + type_support_pkgs: + - path: ./libexample_event_ts_pkg.so + executor: echo_executor + topic_meta_list: + - topic_name: test_topic + msg_type: pb:aimrt.protocols.example.ExampleEventMsg + echo_type: json + log: + core_lvl: Info # Trace/Debug/Info/Warn/Error/Fatal/Off + backends: + - type: console + executor: + executors: + - name: echo_executor + type: simple_thread + - name: work_thread_pool + type: asio_thread + options: + thread_num: 4 + channel: + backends: + - type: local + options: + subscriber_use_inline_executor: false + subscriber_executor: work_thread_pool + pub_topics_options: + - topic_name: "(.*)" + enable_backends: [local] + sub_topics_options: + - topic_name: "(.*)" + enable_backends: [local] + module: + pkgs: + - path: ./libpb_chn_pub_pkg.so + enable_modules: [NormalPublisherModule] + modules: + - name: NormalPublisherModule + log_lvl: Info + +# Module custom configuration +NormalPublisherModule: + topic_name: test_topic + channel_frq: 0.5 + + diff --git a/src/examples/plugins/echo_plugin/install/linux/bin/cfg/examples_plugins_echo_plugin_ros2_cfg.yaml b/src/examples/plugins/echo_plugin/install/linux/bin/cfg/examples_plugins_echo_plugin_ros2_cfg.yaml new file mode 100644 index 000000000..67f6ccc0a --- /dev/null +++ b/src/examples/plugins/echo_plugin/install/linux/bin/cfg/examples_plugins_echo_plugin_ros2_cfg.yaml @@ -0,0 +1,54 @@ +# Copyright (c) 2023, AgiBot Inc. +# All rights reserved. + +aimrt: + plugin: + plugins: + - name: ros2_plugin + path: ./libaimrt_ros2_plugin.so + options: + node_name: example_ros2msg_echo_plugin + executor_type: MultiThreaded # SingleThreaded/StaticSingleThreaded/MultiThreaded + executor_thread_num: 2 + - name: echo_plugin + path: ./libaimrt_echo_plugin.so + options: + type_support_pkgs: + - path: ./libexample_event_ts_pkg.so + topic_meta_list: + - topic_name: test_topic + msg_type: ros2:example_ros2/msg/RosTestMsg + echo_type: yaml + log: + core_lvl: Info # Trace/Debug/Info/Warn/Error/Fatal/Off + backends: + - type: console + executor: + executors: + - name: work_thread_pool + type: asio_thread + options: + thread_num: 4 + channel: + backends: + - type: ros2 + pub_topics_options: + - topic_name: "(.*)" + enable_backends: [ros2] + sub_topics_options: + - topic_name: "(.*)" + enable_backends: [ros2] + module: + pkgs: + - path: ./libros2_chn_pub_pkg.so + enable_modules: [NormalPublisherModule] + modules: + - name: NormalPublisherModule + log_lvl: INFO + +# Module custom configuration +NormalPublisherModule: + topic_name: test_topic + channel_frq: 0.5 + + diff --git a/src/examples/plugins/echo_plugin/install/linux/bin/start_examples_plugins_echo_plugin_pb_cfg.sh b/src/examples/plugins/echo_plugin/install/linux/bin/start_examples_plugins_echo_plugin_pb_cfg.sh new file mode 100755 index 000000000..431f858c6 --- /dev/null +++ b/src/examples/plugins/echo_plugin/install/linux/bin/start_examples_plugins_echo_plugin_pb_cfg.sh @@ -0,0 +1,3 @@ +#!/bin/bash + +./aimrt_main --cfg_file_path=./cfg/examples_plugins_echo_plugin_pb_cfg.yaml diff --git a/src/examples/plugins/echo_plugin/install/linux/bin/start_examples_plugins_echo_plugin_pb_executor_cfg.sh b/src/examples/plugins/echo_plugin/install/linux/bin/start_examples_plugins_echo_plugin_pb_executor_cfg.sh new file mode 100755 index 000000000..ddc26f3e4 --- /dev/null +++ b/src/examples/plugins/echo_plugin/install/linux/bin/start_examples_plugins_echo_plugin_pb_executor_cfg.sh @@ -0,0 +1,3 @@ +#!/bin/bash + +./aimrt_main --cfg_file_path=./cfg/examples_plugins_echo_plugin_pb_executor_cfg.yaml diff --git a/src/examples/plugins/echo_plugin/install/linux/bin/start_examples_plugins_echo_plugin_ros2_cfg.sh b/src/examples/plugins/echo_plugin/install/linux/bin/start_examples_plugins_echo_plugin_ros2_cfg.sh new file mode 100755 index 000000000..f88a5d7e7 --- /dev/null +++ b/src/examples/plugins/echo_plugin/install/linux/bin/start_examples_plugins_echo_plugin_ros2_cfg.sh @@ -0,0 +1,3 @@ +#!/bin/bash + +./aimrt_main --cfg_file_path=./cfg/examples_plugins_echo_plugin_ros2_cfg.yaml diff --git a/src/interface/aimrt_module_ros2_interface/util/ros2_type_support.h b/src/interface/aimrt_module_ros2_interface/util/ros2_type_support.h index e624cf6d3..46f11f730 100644 --- a/src/interface/aimrt_module_ros2_interface/util/ros2_type_support.h +++ b/src/interface/aimrt_module_ros2_interface/util/ros2_type_support.h @@ -13,7 +13,9 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp/serialization.hpp" +#include "rosidl_runtime_cpp/message_type_support_decl.hpp" #include "rosidl_runtime_cpp/traits.hpp" +#include "yaml_convert.h" namespace aimrt { @@ -23,7 +25,7 @@ concept Ros2MsgType = rosidl_generator_traits::is_message::value; template const aimrt_type_support_base_t* GetRos2MessageTypeSupport() { static const aimrt_string_view_t kChannelRos2SerializationTypesSupportedList[] = { - aimrt::util::ToAimRTStringView("ros2"), aimrt::util::ToAimRTStringView("json")}; + aimrt::util::ToAimRTStringView("ros2"), aimrt::util::ToAimRTStringView("json"), aimrt::util::ToAimRTStringView("yaml")}; static const std::string kMsgTypeName = std::string("ros2:") + rosidl_generator_traits::name(); @@ -53,6 +55,7 @@ const aimrt_type_support_base_t* GetRos2MessageTypeSupport() { aimrt_buffer_array_with_allocator_t bawa{.buffer_array = buffer_array, .allocator = allocator}; Ros2RclSerializedMessageAdapter serialized_msg_adapter(&bawa); rcl_ret_t ret = rmw_serialize(msg, ts_ptr, serialized_msg_adapter.GetRclSerializedMessage()); + return (ret == RMW_RET_OK); } if (aimrt::util::ToStdStringView(serialization_type) == "json") { @@ -63,6 +66,23 @@ const aimrt_type_support_base_t* GetRos2MessageTypeSupport() { auto buffer = allocator->allocate(allocator->impl, buffer_array, msg_data.size()); if (buffer.data == nullptr || buffer.len < msg_data.size()) return false; memcpy(buffer.data, msg_data.c_str(), msg_data.size()); + + return true; + } + if (aimrt::util::ToStdStringView(serialization_type) == "yaml") { + const MsgType* typed_msg = static_cast(msg); + std::string msg_yaml_str = to_yaml(*typed_msg); + if (msg_yaml_str.empty()) return false; + + if (msg_yaml_str.back() == '\n') { + msg_yaml_str.pop_back(); + } + msg_yaml_str.push_back('\0'); + + auto buffer = allocator->allocate(allocator->impl, buffer_array, msg_yaml_str.size()); + if (buffer.data == nullptr || buffer.len < msg_yaml_str.size()) return false; + memcpy(buffer.data, msg_yaml_str.c_str(), msg_yaml_str.size()); + return true; } } catch (const std::exception& e) { @@ -113,6 +133,13 @@ const aimrt_type_support_base_t* GetRos2MessageTypeSupport() { } bool ret = common::ros2_util::JsonToMessage(json_data, ts_ptr, msg); return ret; + } else if (aimrt::util::ToStdStringView(serialization_type) == "yaml") { + std::string yaml_data; + for (size_t ii = 0; ii < buffer_array_view.len; ++ii) { + yaml_data.append((char*)buffer_array_view.data[ii].data, buffer_array_view.data[ii].len); + } + bool ret = common::ros2_util::YamlToMessage(yaml_data, ts_ptr, msg); + return ret; } } catch (const std::exception& e) { } diff --git a/src/plugins/CMakeLists.txt b/src/plugins/CMakeLists.txt index 81952e0ed..680d1dc54 100644 --- a/src/plugins/CMakeLists.txt +++ b/src/plugins/CMakeLists.txt @@ -46,3 +46,7 @@ endif() if(AIMRT_BUILD_WITH_PROTOBUF AND AIMRT_BUILD_GRPC_PLUGIN) add_subdirectory(grpc_plugin) endif() + +if(AIMRT_BUILD_ECHO_PLUGIN) + add_subdirectory(echo_plugin) +endif() diff --git a/src/plugins/echo_plugin/CMakeLists.txt b/src/plugins/echo_plugin/CMakeLists.txt new file mode 100644 index 000000000..0fa5a932c --- /dev/null +++ b/src/plugins/echo_plugin/CMakeLists.txt @@ -0,0 +1,53 @@ +# Copyright (c) 2023, AgiBot Inc. +# All rights reserved. + +# Get the current folder name +string(REGEX REPLACE ".*/\(.*\)" "\\1" CUR_DIR ${CMAKE_CURRENT_SOURCE_DIR}) + +# Get namespace +get_namespace(CUR_SUPERIOR_NAMESPACE) +string(REPLACE "::" "_" CUR_SUPERIOR_NAMESPACE_UNDERLINE ${CUR_SUPERIOR_NAMESPACE}) + +# Set target name +set(CUR_TARGET_NAME ${CUR_SUPERIOR_NAMESPACE_UNDERLINE}_${CUR_DIR}) +set(CUR_TARGET_ALIAS_NAME ${CUR_SUPERIOR_NAMESPACE}::${CUR_DIR}) + +# Set file collection +file(GLOB_RECURSE src ${CMAKE_CURRENT_SOURCE_DIR}/*.cc) +file(GLOB_RECURSE test_files ${CMAKE_CURRENT_SOURCE_DIR}/*_test.cc) +list(REMOVE_ITEM src ${test_files}) + +# Add target +add_library(${CUR_TARGET_NAME} SHARED) +add_library(${CUR_TARGET_ALIAS_NAME} ALIAS ${CUR_TARGET_NAME}) + +# Set source file of target +target_sources(${CUR_TARGET_NAME} PRIVATE ${src}) + +# Set include path of target +target_include_directories( + ${CUR_TARGET_NAME} + PRIVATE $) + +# Set link libraries of target +target_link_libraries( + ${CUR_TARGET_NAME} + PRIVATE aimrt::interface::aimrt_core_plugin_interface + aimrt::runtime::core) + +# Add -Werror option +include(AddWerror) +add_werror(${CUR_TARGET_NAME}) + +# Set installation of target +if(AIMRT_INSTALL) + install(TARGETS ${CUR_TARGET_NAME} LIBRARY DESTINATION bin) +endif() + +# Set test of target +if(AIMRT_BUILD_TESTS AND test_files) + add_gtest_target(TEST_TARGET ${CUR_TARGET_NAME} TEST_SRC ${test_files}) +endif() + +# Set misc of target +set_target_properties(${CUR_TARGET_NAME} PROPERTIES OUTPUT_NAME "aimrt_${CUR_DIR}") diff --git a/src/plugins/echo_plugin/echo_plugin.cc b/src/plugins/echo_plugin/echo_plugin.cc new file mode 100644 index 000000000..9d714a7e3 --- /dev/null +++ b/src/plugins/echo_plugin/echo_plugin.cc @@ -0,0 +1,336 @@ +// Copyright (c) 2023, AgiBot Inc. +// All rights reserved. + +#include "echo_plugin/echo_plugin.h" + +#include +#include +#include "echo_plugin/global.h" + +namespace YAML { + +template <> +struct convert { + using Options = aimrt::plugins::echo_plugin::EchoPlugin::Options; + + static Node encode(const Options& rhs) { + Node node; + + node["type_support_pkgs"] = Node(NodeType::Sequence); + for (const auto& type_support_pkg : rhs.type_support_pkgs) { + Node type_support_pkg_node; + type_support_pkg_node["path"] = type_support_pkg.path; + node["type_support_pkgs"].push_back(type_support_pkg_node); + } + + if (!rhs.executor.empty()) { + node["executor"] = rhs.executor; + } + + node["topic_meta_list"] = Node(NodeType::Sequence); + for (const auto& topic_meta : rhs.topic_meta_list) { + Node topic_meta_node; + topic_meta_node["topic_name"] = topic_meta.topic_name; + topic_meta_node["msg_type"] = topic_meta.msg_type; + topic_meta_node["echo_type"] = topic_meta.echo_type; + node["topic_meta_list"].push_back(topic_meta_node); + } + return node; + } + + static bool decode(const Node& node, Options& rhs) { + if (!node.IsMap()) return false; + + if (node["type_support_pkgs"] && node["type_support_pkgs"].IsSequence()) { + for (const auto& type_support_pkg_node : node["type_support_pkgs"]) { + Options::TypeSupportPkg type_support_pkg; + type_support_pkg.path = type_support_pkg_node["path"].as(); + rhs.type_support_pkgs.push_back(std::move(type_support_pkg)); + } + } + + if (node["executor"] && node["executor"].IsScalar()) { + rhs.executor = node["executor"].as(); + } + + if (node["topic_meta_list"] && node["topic_meta_list"].IsSequence()) { + for (const auto& topic_meta_node : node["topic_meta_list"]) { + Options::TopicMeta topic_meta; + topic_meta.topic_name = topic_meta_node["topic_name"].as(); + topic_meta.msg_type = topic_meta_node["msg_type"].as(); + if (topic_meta_node["echo_type"] && topic_meta_node["echo_type"].IsScalar()) { + topic_meta.echo_type = topic_meta_node["echo_type"].as(); + } else { + topic_meta.echo_type = "json"; + } + rhs.topic_meta_list.push_back(std::move(topic_meta)); + } + } + return true; + } +}; + +} // namespace YAML + +namespace aimrt::plugins::echo_plugin { + +bool EchoPlugin::Initialize(runtime::core::AimRTCore* core_ptr) noexcept { + try { + core_ptr_ = core_ptr; + + YAML::Node plugin_options_node = core_ptr_->GetPluginManager().GetPluginOptionsNode(Name()); + + if (plugin_options_node && !plugin_options_node.IsNull()) { + options_ = plugin_options_node.as(); + AIMRT_TRACE("Load plugin options."); + } + + init_flag_ = true; + // type support + for (auto& type_support_pkg : options_.type_support_pkgs) { + // check duplicate pkg + auto finditr = std::find_if( + options_.type_support_pkgs.begin(), options_.type_support_pkgs.end(), + [&type_support_pkg](const auto& op) { + if (&type_support_pkg == &op) return false; + return op.path == type_support_pkg.path; + }); + AIMRT_CHECK_ERROR_THROW(finditr == options_.type_support_pkgs.end(), + "Duplicate pkg path {}", type_support_pkg.path); + + InitTypeSupport(type_support_pkg); + } + AIMRT_TRACE("Load {} pkg and {} type.", + type_support_pkg_loader_vec_.size(), type_support_map_.size()); + + RegisterGetTypeSupportFunc( + [this](std::string_view msg_type) -> aimrt::util::TypeSupportRef { + auto finditr = type_support_map_.find(msg_type); + if (finditr != type_support_map_.end()) + return finditr->second.type_support_ref; + return {}; + }); + + for (auto& topic_meta : options_.topic_meta_list) { + // check msg type + auto type_support_ref = get_type_support_func_(topic_meta.msg_type); + AIMRT_CHECK_ERROR_THROW(type_support_ref, + "Can not find type '{}' in any type support pkg!", topic_meta.msg_type); + + // check serialization type + if (!topic_meta.serialization_type.empty()) { + bool check_ret = type_support_ref.CheckSerializationTypeSupported(topic_meta.serialization_type); + AIMRT_CHECK_ERROR_THROW(check_ret, + "Msg type '{}' does not support serialization type '{}'.", + topic_meta.msg_type, topic_meta.msg_type); + } else { + topic_meta.serialization_type = type_support_ref.DefaultSerializationType(); + } + } + + // check duplicate topic + for (auto& topic_meta_option : options_.topic_meta_list) { + TopicMetaKey key{ + .topic_name = topic_meta_option.topic_name, + .msg_type = topic_meta_option.msg_type}; + + AIMRT_CHECK_ERROR_THROW( + topic_meta_map_.find(key) == topic_meta_map_.end(), + "Duplicate topic meta, topic name: {}, msg type: {}.", + topic_meta_option.topic_name, topic_meta_option.msg_type); + + TopicMeta topic_meta{ + .topic_name = topic_meta_option.topic_name, + .msg_type = topic_meta_option.msg_type, + .echo_type = topic_meta_option.echo_type, + .serialization_type = topic_meta_option.serialization_type}; + topic_meta_map_.emplace(key, topic_meta); + } + + if (!options_.executor.empty()) { + core_ptr_->RegisterHookFunc( + runtime::core::AimRTCore::State::kPostStart, + [this] { + RegisterGetExecutorFunc( + [this](std::string_view executor_name) -> aimrt::executor::ExecutorRef { + return core_ptr_->GetExecutorManager().GetExecutor(executor_name); + }); + InitExecutor(); + }); + } + + core_ptr_->RegisterHookFunc( + runtime::core::AimRTCore::State::kPostInitLog, + [this] { + SetLogger(aimrt::logger::LoggerRef( + core_ptr_->GetLoggerManager().GetLoggerProxy().NativeHandle())); + }); + core_ptr_->RegisterHookFunc( + runtime::core::AimRTCore::State::kPreInitModules, + [this] { + RegisterEchoChannel(); + }); + core_ptr_->RegisterHookFunc( + runtime::core::AimRTCore::State::kPreShutdown, + [this] { + Shutdown(); + SetLogger(aimrt::logger::GetSimpleLoggerRef()); + }); + plugin_options_node = options_; + core_ptr_->GetPluginManager().UpdatePluginOptionsNode(Name(), plugin_options_node); + return true; + } catch (const std::exception& e) { + AIMRT_ERROR("Initialize failed, {}", e.what()); + } + + return false; +} + +void EchoPlugin::InitTypeSupport(Options::TypeSupportPkg& options) { + auto loader_ptr = std::make_unique(); + loader_ptr->LoadTypeSupportPkg(options.path); + + options.path = loader_ptr->GetDynamicLib().GetLibFullPath(); + + auto type_support_array = loader_ptr->GetTypeSupportArray(); + for (const auto* item : type_support_array) { + aimrt::util::TypeSupportRef type_support_ref(item); + auto type_name = type_support_ref.TypeName(); + + // check duplicate type + auto finditr = type_support_map_.find(type_name); + if (finditr != type_support_map_.end()) { + AIMRT_WARN("Duplicate msg type '{}' in {} and {}.", + type_name, options.path, finditr->second.options.path); + continue; + } + + type_support_map_.emplace( + type_name, + TypeSupportWrapper{ + .options = options, + .type_support_ref = type_support_ref, + .loader_ptr = loader_ptr.get()}); + } + AIMRT_INFO("Load {} type support pkgs.", type_support_pkg_loader_vec_.size()); + type_support_pkg_loader_vec_.emplace_back(std::move(loader_ptr)); +} + +void EchoPlugin::RegisterEchoChannel() { + using namespace aimrt::runtime::core::channel; + using EchoFunc = std::function; + + struct Wrapper { + std::unordered_set require_cache_serialization_types; + std::vector echo_func_vec; + }; + + std::unordered_map echo_func_map; + + const auto& topic_meta_list = topic_meta_map_; + AIMRT_TRACE("Echo plugin has {} topics.", topic_meta_list.size()); + + for (const auto& topic_meta_itr : topic_meta_list) { + const auto& topic_meta = topic_meta_itr.second; + EchoFunc echo_func; + if (options_.executor.empty()) { + echo_func = [this, echo_type{topic_meta.echo_type}]( + uint64_t cur_timestamp, MsgWrapper& msg_wrapper) { + Echo(msg_wrapper, echo_type); + }; + } else { + echo_func = [this, echo_type{topic_meta.echo_type}]( + uint64_t cur_timestamp, MsgWrapper& msg_wrapper) { + executor_.Execute([this, msg_wrapper{std::move(msg_wrapper)}, echo_type]() mutable { + Echo(msg_wrapper, echo_type); + }); + }; + } + auto& item = echo_func_map[topic_meta_itr.first]; + item.require_cache_serialization_types.emplace(topic_meta.serialization_type); + item.echo_func_vec.emplace_back(std::move(echo_func)); + } + + AIMRT_TRACE("Register {} echo functions.", echo_func_map.size()); + + // subscribe + for (auto& echo_func_itr : echo_func_map) { + const auto& key = echo_func_itr.first; + auto& wrapper = echo_func_itr.second; + auto finditr = type_support_map_.find(key.msg_type); + + const auto& type_support_ref = finditr->second; + + SubscribeWrapper sub_wrapper; + sub_wrapper.info = TopicInfo{ + .msg_type = key.msg_type, + .topic_name = key.topic_name, + .pkg_path = type_support_ref.options.path, + .module_name = "core", + .msg_type_support_ref = type_support_ref.type_support_ref}; + + sub_wrapper.require_cache_serialization_types = wrapper.require_cache_serialization_types; + + sub_wrapper.callback = [echo_func_vec{std::move(wrapper.echo_func_vec)}]( + MsgWrapper& msg_wrapper, std::function&& release_callback) { + auto cur_timestamp = aimrt::common::util::GetCurTimestampNs(); + + for (const auto& echo_func : echo_func_vec) + echo_func(cur_timestamp, msg_wrapper); + + release_callback(); + }; + + bool ret = core_ptr_->GetChannelManager().Subscribe(std::move(sub_wrapper)); + AIMRT_CHECK_ERROR_THROW(ret, "Subscribe failed!"); + } +} + +void EchoPlugin::Shutdown() noexcept { + try { + if (!init_flag_) return; + } catch (const std::exception& e) { + AIMRT_ERROR("Shutdown failed, {}", e.what()); + } +} + +void EchoPlugin::Echo(runtime::core::channel::MsgWrapper& msg_wrapper, std::string_view echo_type) { + auto buffer_view_ptr = aimrt::runtime::core::channel::TrySerializeMsgWithCache(msg_wrapper, echo_type); + if (!buffer_view_ptr) [[unlikely]] { + AIMRT_ERROR("Can not serialize msg type '{}' with echo type '{}'.", + msg_wrapper.info.msg_type, echo_type); + return; + } + if (buffer_view_ptr && buffer_view_ptr->Data() && buffer_view_ptr->Size() > 0) { + const char* data = static_cast(buffer_view_ptr->Data()[0].data); + AIMRT_INFO("\n{}\n---------------\n", std::string_view(data)); + } else { + AIMRT_ERROR("Invalid buffer, topic_name: {}, msg_type: {}", msg_wrapper.info.topic_name, msg_wrapper.info.msg_type); + } +} + +void EchoPlugin::RegisterGetTypeSupportFunc( + const std::function& get_type_support_func) { + get_type_support_func_ = get_type_support_func; +} + +void EchoPlugin::InitExecutor() { + AIMRT_CHECK_ERROR_THROW( + get_executor_func_, + "Get executor function is not set before initialize."); + + executor_ = get_executor_func_(options_.executor); + + AIMRT_CHECK_ERROR_THROW( + executor_, "Can not get executor {}.", options_.executor); + + AIMRT_CHECK_ERROR_THROW( + executor_.ThreadSafe(), "Echo executor {} is not thread safe!", options_.executor); +} + +void EchoPlugin::RegisterGetExecutorFunc( + const std::function& get_executor_func) { + get_executor_func_ = get_executor_func; +} + +} // namespace aimrt::plugins::echo_plugin diff --git a/src/plugins/echo_plugin/echo_plugin.h b/src/plugins/echo_plugin/echo_plugin.h new file mode 100644 index 000000000..d3ec30df0 --- /dev/null +++ b/src/plugins/echo_plugin/echo_plugin.h @@ -0,0 +1,85 @@ +#pragma once + +#include +#include +#include "aimrt_core_plugin_interface/aimrt_core_plugin_base.h" +#include "aimrt_module_cpp_interface/executor/executor.h" +#include "aimrt_module_cpp_interface/util/type_support.h" +#include "core/aimrt_core.h" +#include "core/channel/channel_backend_tools.h" +#include "core/util/type_support_pkg_loader.h" + +#include "echo_plugin/global.h" +#include "echo_plugin/topic_meta_key.h" +#include "yaml-cpp/yaml.h" + +namespace aimrt::plugins::echo_plugin { + +class EchoPlugin : public AimRTCorePluginBase { + public: + struct Options { + std::string executor; + + struct TopicMeta { + std::string topic_name; + std::string msg_type; + std::string serialization_type; + std::string echo_type; + }; + std::vector topic_meta_list; + struct TypeSupportPkg { + std::string path; + }; + std::vector type_support_pkgs; + }; + + public: + EchoPlugin() = default; + ~EchoPlugin() override = default; + + std::string_view Name() const noexcept override { return "echo_plugin"; } + + bool Initialize(runtime::core::AimRTCore* core_ptr) noexcept override; + void Shutdown() noexcept override; + + const auto& GetTypeSupportMap() const { return type_support_map_; } + + private: + void InitTypeSupport(Options::TypeSupportPkg& options); + void InitExecutor(); + + void RegisterEchoChannel(); + void RegisterGetTypeSupportFunc( + const std::function& get_type_support_func); + + void RegisterGetExecutorFunc( + const std::function& get_executor_func); + + aimrt::executor::ExecutorRef executor_; + + runtime::core::AimRTCore* core_ptr_ = nullptr; + + Options options_; + + bool init_flag_ = false; + + std::vector> + type_support_pkg_loader_vec_; + + struct TypeSupportWrapper { + const Options::TypeSupportPkg& options; + aimrt::util::TypeSupportRef type_support_ref; + runtime::core::util::TypeSupportPkgLoader* loader_ptr; + }; + + std::unordered_map type_support_map_; + + std::unordered_map topic_meta_map_; + + std::function get_type_support_func_; + std::function get_executor_func_; + + void Echo(runtime::core::channel::MsgWrapper& msg_wrapper, std::string_view serialization_type); +}; + +} // namespace aimrt::plugins::echo_plugin diff --git a/src/plugins/echo_plugin/echo_plugin_main.cc b/src/plugins/echo_plugin/echo_plugin_main.cc new file mode 100644 index 000000000..90efb620a --- /dev/null +++ b/src/plugins/echo_plugin/echo_plugin_main.cc @@ -0,0 +1,16 @@ +// Copyright (c) 2023, AgiBot Inc. +// All rights reserved. + +#include "aimrt_core_plugin_interface/aimrt_core_plugin_main.h" +#include "echo_plugin/echo_plugin.h" + +extern "C" { + +aimrt::AimRTCorePluginBase* AimRTDynlibCreateCorePluginHandle() { + return new aimrt::plugins::echo_plugin::EchoPlugin(); +} + +void AimRTDynlibDestroyCorePluginHandle(const aimrt::AimRTCorePluginBase* plugin) { + delete plugin; +} +} diff --git a/src/plugins/echo_plugin/global.cc b/src/plugins/echo_plugin/global.cc new file mode 100644 index 000000000..d3b1f3c2e --- /dev/null +++ b/src/plugins/echo_plugin/global.cc @@ -0,0 +1,15 @@ +// Copyright (c) 2023, AgiBot Inc. +// All rights reserved. + +#include "echo_plugin/global.h" + +namespace aimrt::plugins::echo_plugin { + +aimrt::logger::LoggerRef global_logger; + +void SetLogger(aimrt::logger::LoggerRef logger) { global_logger = logger; } +aimrt::logger::LoggerRef GetLogger() { + return global_logger ? global_logger : aimrt::logger::GetSimpleLoggerRef(); +} + +} // namespace aimrt::plugins::echo_plugin diff --git a/src/plugins/echo_plugin/global.h b/src/plugins/echo_plugin/global.h new file mode 100644 index 000000000..5a68c2ea3 --- /dev/null +++ b/src/plugins/echo_plugin/global.h @@ -0,0 +1,13 @@ +// Copyright (c) 2023, AgiBot Inc. +// All rights reserved. + +#pragma once + +#include "aimrt_module_cpp_interface/logger/logger.h" + +namespace aimrt::plugins::echo_plugin { + +void SetLogger(aimrt::logger::LoggerRef); +aimrt::logger::LoggerRef GetLogger(); + +} // namespace aimrt::plugins::echo_plugin diff --git a/src/plugins/echo_plugin/topic_meta_key.h b/src/plugins/echo_plugin/topic_meta_key.h new file mode 100644 index 000000000..061414d54 --- /dev/null +++ b/src/plugins/echo_plugin/topic_meta_key.h @@ -0,0 +1,35 @@ +// Copyright (c) 2023, AgiBot Inc. +// All rights reserved. + +#pragma once + +#include +#include + +namespace aimrt::plugins::echo_plugin { + +struct TopicMetaKey { + std::string topic_name; + std::string msg_type; + + bool operator==(const TopicMetaKey& rhs) const { + return topic_name == rhs.topic_name && msg_type == rhs.msg_type; + } + + struct Hash { + std::size_t operator()(const TopicMetaKey& k) const { + return (std::hash()(k.topic_name)) ^ + (std::hash()(k.msg_type)); + } + }; +}; + +struct TopicMeta { + uint64_t id; + std::string topic_name; + std::string msg_type; + std::string echo_type; + std::string serialization_type; +}; + +} // namespace aimrt::plugins::echo_plugin diff --git a/src/plugins/record_playback_plugin/record_playback_plugin.cc b/src/plugins/record_playback_plugin/record_playback_plugin.cc index 11d4eb1ad..8f2e76aaf 100644 --- a/src/plugins/record_playback_plugin/record_playback_plugin.cc +++ b/src/plugins/record_playback_plugin/record_playback_plugin.cc @@ -246,7 +246,7 @@ void RecordPlaybackPlugin::Shutdown() noexcept { } void RecordPlaybackPlugin::InitTypeSupport(Options::TypeSupportPkg& options) { - auto loader_ptr = std::make_unique(); + auto loader_ptr = std::make_unique(); loader_ptr->LoadTypeSupportPkg(options.path); options.path = loader_ptr->GetDynamicLib().GetLibFullPath(); diff --git a/src/plugins/record_playback_plugin/record_playback_plugin.h b/src/plugins/record_playback_plugin/record_playback_plugin.h index a4c09a3a1..c31dbdd25 100644 --- a/src/plugins/record_playback_plugin/record_playback_plugin.h +++ b/src/plugins/record_playback_plugin/record_playback_plugin.h @@ -5,11 +5,10 @@ #include "aimrt_core_plugin_interface/aimrt_core_plugin_base.h" #include "aimrt_module_cpp_interface/util/type_support.h" +#include "core/util/type_support_pkg_loader.h" #include "record_playback_plugin/playback_action.h" #include "record_playback_plugin/record_action.h" #include "record_playback_plugin/service.h" -#include "record_playback_plugin/type_support_pkg_loader.h" - namespace aimrt::plugins::record_playback_plugin { class RecordPlaybackPlugin : public AimRTCorePluginBase { @@ -61,12 +60,12 @@ class RecordPlaybackPlugin : public AimRTCorePluginBase { std::unique_ptr service_ptr_; - std::vector> type_support_pkg_loader_vec_; + std::vector> type_support_pkg_loader_vec_; struct TypeSupportWrapper { const Options::TypeSupportPkg& options; aimrt::util::TypeSupportRef type_support_ref; - TypeSupportPkgLoader* loader_ptr; + runtime::core::util::TypeSupportPkgLoader* loader_ptr; }; std::unordered_map type_support_map_; diff --git a/src/plugins/record_playback_plugin/type_support_pkg_loader.cc b/src/runtime/core/util/type_support_pkg_loader.cc similarity index 52% rename from src/plugins/record_playback_plugin/type_support_pkg_loader.cc rename to src/runtime/core/util/type_support_pkg_loader.cc index 9d95cb40e..92135fc3e 100644 --- a/src/plugins/record_playback_plugin/type_support_pkg_loader.cc +++ b/src/runtime/core/util/type_support_pkg_loader.cc @@ -1,10 +1,12 @@ // Copyright (c) 2023, AgiBot Inc. // All rights reserved. -#include "record_playback_plugin/type_support_pkg_loader.h" -#include "record_playback_plugin/global.h" +#include "core/util/type_support_pkg_loader.h" +#include +#include "util/exception.h" +#include "util/format.h" -namespace aimrt::plugins::record_playback_plugin { +namespace aimrt::runtime::core::util { using DynlibGetTypeSupportArrayLengthFunc = size_t (*)(); using DynlibGetTypeSupportArray = const aimrt_type_support_base_t** (*)(); @@ -15,25 +17,28 @@ static constexpr const char* kDynlibGetTypeSupportArrayFuncName = "AimRTDynlibGe void TypeSupportPkgLoader::LoadTypeSupportPkg(std::string_view path) { path_ = path; - AIMRT_CHECK_ERROR_THROW(dynamic_lib_.Load(path_), - "Load dynamic lib failed, lib path {}, error info {}", - path_, aimrt::common::util::DynamicLib::GetErr()); + AIMRT_ASSERT(dynamic_lib_.Load(path_), + "Load dynamic lib failed, lib path {}, error info {}", + path_, aimrt::common::util::DynamicLib::GetErr()); auto* get_length_func = dynamic_lib_.GetSymbol(kDynlibGetTypeSupportArrayLengthFuncName); - AIMRT_CHECK_ERROR_THROW(get_length_func != nullptr, - "Cannot find symbol '{}' in lib {}.", - kDynlibGetTypeSupportArrayLengthFuncName, path_); + + AIMRT_ASSERT(get_length_func != nullptr, + "Cannot find symbol '{}' in lib {}.", + kDynlibGetTypeSupportArrayLengthFuncName, path_); auto* get_array_func = dynamic_lib_.GetSymbol(kDynlibGetTypeSupportArrayFuncName); - AIMRT_CHECK_ERROR_THROW(get_array_func != nullptr, - "Cannot find symbol '{}' in lib {}.", - kDynlibGetTypeSupportArrayFuncName, path_); + + AIMRT_ASSERT(get_array_func != nullptr, + "Cannot find symbol '{}' in lib {}.", + kDynlibGetTypeSupportArrayFuncName, path_); size_t array_size = ((DynlibGetTypeSupportArrayLengthFunc)get_length_func)(); const aimrt_type_support_base_t** array_ptr = ((DynlibGetTypeSupportArray)get_array_func)(); - AIMRT_CHECK_ERROR_THROW(array_ptr != nullptr, - "Cannot get type support array in lib {}.", path_); + AIMRT_ASSERT(array_ptr != nullptr, + "Cannot get type support array in lib {}.", + path_); type_support_array_ = std::span{array_ptr, array_size}; } @@ -44,4 +49,4 @@ void TypeSupportPkgLoader::UnLoadTypeSupportPkg() { dynamic_lib_.Unload(); } -} // namespace aimrt::plugins::record_playback_plugin +} // namespace aimrt::runtime::core::util diff --git a/src/plugins/record_playback_plugin/type_support_pkg_loader.h b/src/runtime/core/util/type_support_pkg_loader.h similarity index 88% rename from src/plugins/record_playback_plugin/type_support_pkg_loader.h rename to src/runtime/core/util/type_support_pkg_loader.h index fbc415ca0..d83fd1fc3 100644 --- a/src/plugins/record_playback_plugin/type_support_pkg_loader.h +++ b/src/runtime/core/util/type_support_pkg_loader.h @@ -10,7 +10,7 @@ #include "core/util/dynamic_lib.h" #include "util/log_util.h" -namespace aimrt::plugins::record_playback_plugin { +namespace aimrt::runtime::core::util { class TypeSupportPkgLoader { public: @@ -35,4 +35,4 @@ class TypeSupportPkgLoader { std::span type_support_array_; }; -} // namespace aimrt::plugins::record_playback_plugin +} // namespace aimrt::runtime::core::util diff --git a/test.bat b/test.bat index c44e7e5d5..d27d64eac 100644 --- a/test.bat +++ b/test.bat @@ -28,6 +28,7 @@ cmake -B build ^ -DAIMRT_BUILD_LOG_CONTROL_PLUGIN=ON ^ -DAIMRT_BUILD_OPENTELEMETRY_PLUGIN=OFF ^ -DAIMRT_BUILD_GRPC_PLUGIN=OFF ^ + -DAIMRT_BUILD_ECHO_PLUGIN=OFF ^ -DAIMRT_BUILD_PYTHON_PACKAGE=ON ^ %* diff --git a/test.sh b/test.sh index 05038be93..547d8c2e3 100755 --- a/test.sh +++ b/test.sh @@ -30,6 +30,7 @@ cmake -B build \ -DAIMRT_BUILD_LOG_CONTROL_PLUGIN=ON \ -DAIMRT_BUILD_OPENTELEMETRY_PLUGIN=ON \ -DAIMRT_BUILD_GRPC_PLUGIN=ON \ + -DAIMRT_BUILD_ECHO_PLUGIN=ON \ -DAIMRT_BUILD_PYTHON_PACKAGE=ON \ $@