feat: add a new plugin: echo (AimRT#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 <yuguanlin@agibot.com>
whjgwf · Oct 31, 2024 · 7e4b546 · 7e4b546
1 parent 929a105
commit 7e4b546
Show file tree

Hide file tree

Showing 36 changed files with 1,744 additions and 24 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -0,0 +1,232 @@
+// Copyright (c) 2023, AgiBot Inc.
+// All rights reserved.
+
+#pragma once
+
+#include <cassert>
+#include <stdexcept>
+
+#include <yaml-cpp/yaml.h>
+
+#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 <int RosTypeId>
+inline void WriteSequenceMemberItem(const YAML::Node &yaml, uint8_t *buffer) {
+  using CppType = typename TypeMappingCpp<RosTypeId>::CppType;
+  using SequenceType = typename TypeMappingCpp<RosTypeId>::SequenceType;
+  reinterpret_cast<SequenceType *>(buffer)->push_back(GetYamlValue<CppType>(yaml));
+}
+
+template <int RosTypeId>
+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<RosTypeId>(yaml[i], buffer);
+  }
+}
+
+template <int RosTypeId>
+inline void WriteMemberItem(const YAML::Node &yaml, uint8_t *buffer) {
+  using CppType = typename TypeMappingCpp<RosTypeId>::CppType;
+  *reinterpret_cast<CppType *>(buffer) = GetYamlValue<CppType>(yaml);
+}
+
+template <int RosTypeId>
+inline void WriteMember(
+    const YAML::Node &yaml,
+    uint8_t *buffer,
+    const rosidl_typesupport_introspection_cpp::MessageMember &member) {
+  using CppType = typename TypeMappingCpp<RosTypeId>::CppType;
+
+  if (IsSequence(member)) {
+    if (yaml[member.name_].IsSequence()) {
+      WriteSequence<RosTypeId>(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<RosTypeId>(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<RosTypeId>(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<const rosidl_typesupport_introspection_cpp::MessageMembers *>(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<uint8_t *>(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<const rosidl_typesupport_introspection_cpp::MessageMembers *>(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<rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT>(root, buffer, member);
+        break;
+      case rosidl_typesupport_introspection_cpp::ROS_TYPE_DOUBLE:
+        WriteMember<rosidl_typesupport_introspection_cpp::ROS_TYPE_DOUBLE>(root, buffer, member);
+        break;
+      case rosidl_typesupport_introspection_cpp::ROS_TYPE_LONG_DOUBLE:
+        WriteMember<rosidl_typesupport_introspection_cpp::ROS_TYPE_LONG_DOUBLE>(root, buffer, member);
+        break;
+      case rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR:
+        WriteMember<rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR>(root, buffer, member);
+        break;
+      case rosidl_typesupport_introspection_cpp::ROS_TYPE_WCHAR:
+        WriteMember<rosidl_typesupport_introspection_cpp::ROS_TYPE_WCHAR>(root, buffer, member);
+        break;
+      case rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOLEAN:
+        WriteMember<rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOLEAN>(root, buffer, member);
+        break;
+      case rosidl_typesupport_introspection_cpp::ROS_TYPE_OCTET:
+        WriteMember<rosidl_typesupport_introspection_cpp::ROS_TYPE_OCTET>(root, buffer, member);
+        break;
+      case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8:
+        WriteMember<rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8>(root, buffer, member);
+        break;
+      case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8:
+        WriteMember<rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8>(root, buffer, member);
+        break;
+      case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16:
+        WriteMember<rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16>(root, buffer, member);
+        break;
+      case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16:
+        WriteMember<rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16>(root, buffer, member);
+        break;
+      case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32:
+        WriteMember<rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32>(root, buffer, member);
+        break;
+      case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32:
+        WriteMember<rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32>(root, buffer, member);
+        break;
+      case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64:
+        WriteMember<rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64>(root, buffer, member);
+        break;
+      case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64:
+        WriteMember<rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64>(root, buffer, member);
+        break;
+      case rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING:
+        WriteMember<rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING>(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<uint8_t *>(message);
+
+  try {
+    YamlToMessageImpl(root, member_info, buffer);
+  } catch (...) {
+    return false;
+  }
+
+  return true;
+}
+
+}  // namespace aimrt::common::ros2_util