Merge branch 'jazzy-rmw-wrapper' into emerson/jazzy-latest

Author: emersonknapp

.github/workflows/build.yml

@@ -0,0 +1,38 @@
+---
+name: Build and test
+"on":
+  pull_request:
+  push:
+    branches:
+      - jazzy-rmw-wrapper
+
+jobs:
+  build_and_test:
+    runs-on: ubuntu-latest
+    strategy:
+      fail-fast: false
+      matrix:
+        include:
+          - ros: jazzy
+            ubuntu: noble
+    name: ROS 2 ${{ matrix.ros }}
+    container:
+      image: ghcr.io/ros-tooling/setup-ros-docker/setup-ros-docker-ubuntu-${{ matrix.ubuntu }}:latest
+    env:
+      ROS_DISTRO: ${{ matrix.ros }}
+      RMW_IMPLEMENTATION: rmw_cyclonedds_cpp
+      RMW_IMPLEMENTATION_WRAPPER: rmw_stats_shim
+      DISABLE_GROUPS_WORKAROUND: 1
+    steps:
+      - uses: actions/checkout@v4
+      - shell: bash
+        run: |
+          apt-get update && apt-get install -y -q --no-install-recommends ros-${{ matrix.ros }}-rmw-cyclonedds-cpp
+      - uses: ros-tooling/[email protected]
+        with:
+          target-ros2-distro: ${{ matrix.ros }}
+          package-name: rmw_implementation test_rmw_implementation
+      - uses: actions/upload-artifact@v4
+        with:
+          name: colcon-logs-${{ matrix.ros }}
+          path: ros_ws/log

README.md

@@ -0,0 +1,37 @@
+# RMW Implementation Fork for RMW Wrapper
+
+This repository is a fork of https://github.com/ros2/rmw_implementation , maintained to provide an extra feature of a `RMW_IMPLEMENTATION_WRAPPER` shim library that can be loaded in addition to the `RMW_IMPLEMENTATION` (DDS, etc.).
+
+This enables e.g. the https://github.com/ros-tooling/graph-monitor/tree/main/rmw_stats_shim to intercept calls to RMW and provide statistics about every node/subscription/publisher in the system.
+
+
+## Development
+
+1. All live rosdistros are supported
+1. Branches are called `${ROS_DISTRO}-rmw-wrapper`
+2. As few modifications are added here as possible
+3. All changes here are backported to all live distros
+4. Upstream branches are periodically merged in
+
+
+### Updating to latest from upstream
+
+Get sources set up
+
+```bash
+git clone https://github.com/robograph-project/rmw_implementation
+cd rmw_implementation
+git remote add ros2 https://github.com/ros2/rmw_implementation
+```
+
+Merge in latest and open PR
+
+```bash
+git checkout ${ROS_DISTRO}-rmw-wrapper
+git checkout -b ${ROS_DISTRO}-merge-latest
+git fetch ros2
+git merge ros2/${ROS_DISTRO}
+# push that branch and open PR
+```
+
+Perform for every distro.

rmw_implementation/package.xml

@@ -21,15 +21,12 @@
   <depend>rcpputils</depend>
   <depend>rcutils</depend>
   <build_depend>rmw</build_depend>
-  <!--
-  Bloom does not support group_depend so entries below duplicate the group rmw_implementation_packages.
-  This ensures that binary packages have support for all of these rmw impl. enabled.
-  -->
-  <build_depend>rmw_connextdds</build_depend>
-  <build_depend>rmw_cyclonedds_cpp</build_depend>
-  <build_depend>rmw_fastrtps_cpp</build_depend>
-  <build_depend>rmw_fastrtps_dynamic_cpp</build_depend>
-  <!-- end of group dependencies added for bloom -->
+
+  <!-- Explicit group resolution - see ros-infrastructure/catkin_pkg#369 -->
+  <build_depend condition="$DISABLE_GROUPS_WORKAROUND != 1">rmw_connextdds</build_depend>
+  <build_depend condition="$DISABLE_GROUPS_WORKAROUND != 1">rmw_cyclonedds_cpp</build_depend>
+  <build_depend condition="$DISABLE_GROUPS_WORKAROUND != 1">rmw_fastrtps_cpp</build_depend>
+  <build_depend condition="$DISABLE_GROUPS_WORKAROUND != 1">rmw_fastrtps_dynamic_cpp</build_depend>
 
   <depend>rmw_implementation_cmake</depend>
 

rmw_implementation/src/functions.cpp

@@ -136,6 +136,29 @@ get_library()
   return g_rmw_lib;
 }
 
+std::shared_ptr<rcpputils::SharedLibrary>
+get_wrap_library()
+{
+  static bool tried_load = false;
+  static std::shared_ptr<rcpputils::SharedLibrary> wrap_lib = nullptr;
+  if (!wrap_lib && !tried_load) {
+    tried_load = true;
+    std::string wrapper_var;
+    try {
+      wrapper_var = rcpputils::get_env_var("RMW_IMPLEMENTATION_WRAPPER");
+    } catch (const std::exception & e) {
+      RMW_SET_ERROR_MSG_WITH_FORMAT_STRING(
+        "failed to fetch RMW_IMPLEMENTATION_WRAPPER "
+        "from environment due to %s", e.what());
+      return nullptr;
+    }
+    if (!wrapper_var.empty()) {
+      wrap_lib = attempt_to_load_one_rmw(wrapper_var);
+    }
+  }
+  return wrap_lib;
+}
+
 void *
 lookup_symbol(std::shared_ptr<rcpputils::SharedLibrary> lib, const std::string & symbol_name)
 {
@@ -175,6 +198,25 @@ get_symbol(const char * symbol_name)
   }
 }
 
+void * get_wrap_symbol(const char * symbol_name)
+{
+  try {
+    auto lib = get_wrap_library();
+    if (!lib) {
+      return nullptr;
+    }
+    if (!lib->has_symbol(symbol_name)) {
+      return nullptr;
+    }
+    return lib->get_symbol(symbol_name);
+  } catch (const std::exception & e) {
+    RMW_SET_ERROR_MSG_WITH_FORMAT_STRING(
+      "failed to get wrapper symbol '%s' due to %s",
+      symbol_name, e.what());
+    return nullptr;
+  }
+}
+
 #ifdef __cplusplus
 extern "C"
 {
@@ -202,6 +244,27 @@ extern "C"
 #define ARGS_6(t6, ...) t6 v6, EXPAND(ARGS_5(__VA_ARGS__))
 #define ARGS_7(t7, ...) t7 v7, EXPAND(ARGS_6(__VA_ARGS__))
 
+// Macros for "wrapped function" args, allow us to prepend one extra argument more than _NR value
+// "tX" params are "type" for the type declaration of the argument
+// "vX" params are "variable" for the variable name declaration
+#define WARGS_0(t0, tvoid) t0 v0
+#define WARGS_1(t1, t0) t1 v1, t0 v0
+#define WARGS_2(t2, ...) t2 v2, EXPAND(WARGS_1(__VA_ARGS__))
+#define WARGS_3(t3, ...) t3 v3, EXPAND(WARGS_2(__VA_ARGS__))
+#define WARGS_4(t4, ...) t4 v4, EXPAND(WARGS_3(__VA_ARGS__))
+#define WARGS_5(t5, ...) t5 v5, EXPAND(WARGS_4(__VA_ARGS__))
+#define WARGS_6(t6, ...) t6 v6, EXPAND(WARGS_5(__VA_ARGS__))
+#define WARGS_7(t7, ...) t7 v7, EXPAND(WARGS_6(__VA_ARGS__))
+
+#define WARG_VALUES_0(impl, ...) impl
+#define WARG_VALUES_1(impl, ...) impl, EXPAND(ARG_VALUES_1(__VA_ARGS__))
+#define WARG_VALUES_2(impl, ...) impl, EXPAND(ARG_VALUES_2(__VA_ARGS__))
+#define WARG_VALUES_3(impl, ...) impl, EXPAND(ARG_VALUES_3(__VA_ARGS__))
+#define WARG_VALUES_4(impl, ...) impl, EXPAND(ARG_VALUES_4(__VA_ARGS__))
+#define WARG_VALUES_5(impl, ...) impl, EXPAND(ARG_VALUES_5(__VA_ARGS__))
+#define WARG_VALUES_6(impl, ...) impl, EXPAND(ARG_VALUES_6(__VA_ARGS__))
+#define WARG_VALUES_7(impl, ...) impl, EXPAND(ARG_VALUES_7(__VA_ARGS__))
+
 #define CALL_SYMBOL(symbol_name, ReturnType, error_value, ArgTypes, arg_values) \
   if (!symbol_ ## symbol_name) { \
     /* only necessary for functions called before rmw_init */ \
@@ -218,8 +281,14 @@ extern "C"
 // cppcheck-suppress preprocessorErrorDirective
 #define RMW_INTERFACE_FN(name, ReturnType, error_value, _NR, ...) \
   void * symbol_ ## name = nullptr; \
+  void * symbol_wrap_ ## name = nullptr; \
   ReturnType name(EXPAND(ARGS_ ## _NR(__VA_ARGS__))) \
   { \
+    if (symbol_wrap_ ## name) { \
+      typedef ReturnType (* WrapFunctionSignature)(EXPAND (WARGS_ ## _NR(void *, __VA_ARGS__))); \
+      auto wrap_func = reinterpret_cast<WrapFunctionSignature>(symbol_wrap_ ## name); \
+      return wrap_func(EXPAND(WARG_VALUES_ ## _NR(symbol_ ## name, __VA_ARGS__))); \
+    } \
     CALL_SYMBOL( \
       name, ReturnType, error_value, ARG_TYPES(__VA_ARGS__), \
       EXPAND(ARG_VALUES_ ## _NR(__VA_ARGS__))); \
@@ -788,8 +857,9 @@ RMW_INTERFACE_FN(
   3, ARG_TYPES(
     const char *, rcutils_allocator_t *, rosidl_dynamic_typesupport_serialization_support_t *))
 
-
-#define GET_SYMBOL(x) symbol_ ## x = get_symbol(#x);
+#define GET_SYMBOL(x) \
+  symbol_ ## x = get_symbol(#x); \
+  symbol_wrap_ ## x = get_wrap_symbol(STRINGIFY(wrap_ ## x));
 
 void prefetch_symbols(void)
 {
@@ -891,6 +961,7 @@ void prefetch_symbols(void)
 }
 
 void * symbol_rmw_init = nullptr;
+void * symbol_wrap_rmw_init = nullptr;
 
 rmw_ret_t
 rmw_init(const rmw_init_options_t * options, rmw_context_t * context)
@@ -902,10 +973,19 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context)
   if (!symbol_rmw_init) {
     return RMW_RET_ERROR;
   }
+  if (!symbol_wrap_rmw_init) {
+    symbol_wrap_rmw_init = get_wrap_symbol("wrap_rmw_init");
+  }
+  rmw_ret_t ret = RMW_RET_OK;
+  if (symbol_wrap_rmw_init) {
+    typedef rmw_ret_t (* WrapperInitSignature)(void *, const rmw_init_options_t *, rmw_context_t *);
+    auto wrap_init_func = reinterpret_cast<WrapperInitSignature>(symbol_wrap_rmw_init);
+    ret = wrap_init_func(get_library().get(), options, context);
+  }
 
   typedef rmw_ret_t (* FunctionSignature)(const rmw_init_options_t *, rmw_context_t *);
   FunctionSignature func = reinterpret_cast<FunctionSignature>(symbol_rmw_init);
-  return func(options, context);
+  return ret != RMW_RET_OK ? ret : func(options, context);
 }
 
 #ifdef __cplusplus