Adds support for runtime configuration of actions and services via code generation

CMakeLists.txt

@@ -16,7 +16,10 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
+
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(rcutils REQUIRED)
 # Leverage rosbag2's generic type support utilities
@@ -27,6 +30,16 @@ find_package(yaml_cpp_vendor REQUIRED)
 find_package(zstd_vendor REQUIRED)
 find_package(zstd REQUIRED)
 
+find_package(Python3 REQUIRED COMPONENTS Interpreter)
+
+set(GENERATED_CODE "${CMAKE_CURRENT_BINARY_DIR}/generated_code.cpp")
+
+add_custom_command(
+  OUTPUT ${GENERATED_CODE}
+  COMMAND Python3::Interpreter ${CMAKE_CURRENT_SOURCE_DIR}/bin/generate_code.py --output ${GENERATED_CODE}
+  DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/bin/generate_code.py ${CMAKE_CURRENT_SOURCE_DIR}/resources/code_template.cpp.em
+)
+
 rosidl_generate_interfaces(${PROJECT_NAME}
   msg/CompressedMsg.msg
 )
@@ -40,8 +53,26 @@ add_library(${PROJECT_NAME}_lib SHARED
   src/${PROJECT_NAME}/qos_options.cpp
   src/${PROJECT_NAME}/service_bridge_options.cpp
   src/${PROJECT_NAME}/topic_bridge_options.cpp
+  src/${PROJECT_NAME}/action_bridge_options.cpp
+
+  ${GENERATED_CODE}
 )
 
+
+# generate per interface compilation units to keep the memory usage low
+ament_index_get_resources(ros2_interface_packages "rosidl_interfaces")
+# actionlib_msgs is deprecated, but we will quiet the warning until the bridge has support for
+# ROS actions: https://github.com/ros2/design/issues/195
+foreach(package_name ${ros2_interface_packages})
+  find_package(${package_name} QUIET REQUIRED)
+  message(STATUS "Found ${package_name}: ${${package_name}_VERSION} (${${package_name}_DIR})")
+  if("${package_name}" STREQUAL "builtin_interfaces")
+    continue()
+  endif()
+  list(APPEND interface_packages ${package_name})
+endforeach()
+
+
 target_include_directories(${PROJECT_NAME}_lib PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>
@@ -53,12 +84,14 @@ endif()
 
 ament_target_dependencies(${PROJECT_NAME}_lib
   rclcpp
+  rclcpp_action
   rclcpp_components
   rcutils
   rosbag2_cpp
   rosidl_typesupport_cpp
   yaml_cpp_vendor
   zstd
+  ${interface_packages}
 )
 
 rosidl_get_typesupport_target(cpp_typesupport_target
@@ -130,6 +163,7 @@ install(DIRECTORY examples launch
 ament_export_targets(export_${PROJECT_NAME})
 ament_export_dependencies(
   rclcpp
+  rclcpp_action
   rclcpp_components
   rosbag2_cpp
   rcutils
@@ -150,6 +184,7 @@ if(BUILD_TESTING)
   )
   ament_target_dependencies(${PROJECT_NAME}_test_component_lib
     rclcpp
+    rclcpp_action
     rclcpp_components
     test_msgs
   )

bin/generate_code.py

@@ -0,0 +1,79 @@
+#!/usr/bin/env python3
+
+import argparse
+import os
+from ament_index_python.packages import get_packages_with_prefixes
+from rosidl_cmake import expand_template
+import re
+
+
+def camel_case_to_lower_case_underscore(value):
+    # insert an underscore before any upper case letter
+    # which is not followed by another upper case letter
+    value = re.sub('(.)([A-Z][a-z]+)', '\\1_\\2', value)
+    # insert an underscore before any upper case letter
+    # which is preseded by a lower case letter or number
+    value = re.sub('([a-z0-9])([A-Z])', '\\1_\\2', value)
+    return value.lower()
+
+def type_name_to_include(type_name):
+    package, interface_type, type = type_name.split("/")
+    type = camel_case_to_lower_case_underscore(type)
+    return "#include <" + package + "/" + interface_type + "/" + type + ".hpp>"
+
+def type_name_to_cpp_type(type_name):
+    package, interface_type, type = type_name.split("/")
+    return package + "::" + interface_type + "::" + type
+
+def get_types():
+    ros2_packages = get_packages_with_prefixes()
+
+    message_types = []
+    service_types = []
+    action_types = []
+
+    for package_name, prefix_path in ros2_packages.items():
+        share_path = os.path.join(prefix_path, 'share', package_name)
+        # Collect message types
+        msg_path = os.path.join(share_path, 'msg')
+        if os.path.exists(msg_path):
+            for msg_file in os.listdir(msg_path):
+                if msg_file.endswith('.msg'):
+                    msg_type = f"{package_name}/msg/{msg_file[:-4]}"
+                    message_types.append(msg_type)
+        # Collect service types
+        srv_path = os.path.join(share_path, 'srv')
+        if os.path.exists(srv_path):
+            for srv_file in os.listdir(srv_path):
+                if srv_file.endswith('.srv'):
+                    srv_type = f"{package_name}/srv/{srv_file[:-4]}"
+                    service_types.append(srv_type)
+        # Collect action types
+        action_path = os.path.join(share_path, 'action')
+        if os.path.exists(action_path):
+            for action_file in os.listdir(action_path):
+                if action_file.endswith('.action'):
+                    action_type = f"{package_name}/action/{action_file[:-7]}"
+                    action_types.append(action_type)
+
+    return message_types, service_types, action_types
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument('--output', required=True)
+    args = parser.parse_args()
+
+    message_types, service_types, action_types = get_types()
+
+    template_file = os.path.join(os.path.dirname(__file__), '../resources/code_template.cpp.em')
+    expand_template(template_file, {
+        'message_types': message_types,
+        'service_types': service_types,
+        'action_types': action_types,
+        'camel_case_to_lower_case_underscore': camel_case_to_lower_case_underscore,
+        'type_name_to_include': type_name_to_include,
+        'type_name_to_cpp_type': type_name_to_cpp_type
+    }, args.output)
+
+if __name__ == '__main__':
+    main()

examples/example_bridge_config.yaml

@@ -8,30 +8,43 @@ topics:
     type: rosgraph_msgs/msg/Clock
   # Bridge "/chatter" topic from doman ID 2 to domain ID 3
   chatter:
-    type: example_interfaces/msg/String
+    type: std_msgs/msg/String
     # Override QoS reliablity setting to be best effort
     # This affects the domain bridge subscription and publisher
     # This does NOT affect the other "chatter" bridge below, which is for a different domain
     qos:
       reliability: best_effort
   # Also bridge "/chatter" topic from doman ID 2 to domain ID 4
   chatter:
-    type: example_interfaces/msg/String
+    type: std_msgs/msg/String
     to_domain: 4
   # Bridge reversed "/chatter" topic
   chatter:
-    type: example_interfaces/msg/String
+    type: std_msgs/msg/String
     # Reverse 'from' and 'to' domains (bridge from domain ID 3 to domain ID 2)
     reversed: True
   # Bridge bidirectional "/chatter" topic
   chatter:
-    type: example_interfaces/msg/String
+    type: std_msgs/msg/String
     from_domain: 7
     to_domain: 8
     # Bridge from domain ID 7 to domain ID 8 AND from domain ID 8 to domain ID 7
     bidirectional: True
 
   # Bridge "/chatter" topic from doman ID 2 to domain ID 3, but as "/chitter"
   chatter:
-    type: example_interfaces/msg/String
+    type: std_msgs/msg/String
     remap: chitter
+
+services:
+  add_two_ints:
+    type: example_interfaces/srv/AddTwoInts
+    remap: add_two_numbers
+    from_domain: 1
+    to_domain: 2
+
+actions:
+  fibonacci:
+    type: action_tutorials_interfaces/action/Fibonacci
+    from_domain: 1
+    to_domain: 2

include/domain_bridge/action_bridge.hpp

@@ -0,0 +1,63 @@
+#ifndef DOMAIN_BRIDGE__ACTION_BRIDGE_HPP_
+#define DOMAIN_BRIDGE__ACTION_BRIDGE_HPP_
+
+#include <cstddef>
+#include <string>
+
+namespace domain_bridge
+{
+
+/// Info and ROS entities related to a action bridge
+struct ActionBridge
+{
+  /// Name of the bridged action
+  std::string action_name;
+
+  /// Name of the message type sent on the action
+  std::string type_name;
+
+  /// Domain ID that the subscription uses
+  std::size_t from_domain_id;
+
+  /// Domain ID that the publisher uses
+  std::size_t to_domain_id;
+
+  /// Less-than operator.
+  /**
+   * Sort by 'from_domain_id',
+   *   then by 'to_domain_id',
+   *   then by 'action_name',
+   *   then by 'type_name'
+   */
+  bool operator<(const ActionBridge & other) const
+  {
+    if (from_domain_id < other.from_domain_id) {
+      return true;
+    }
+    if (from_domain_id > other.from_domain_id) {
+      return false;
+    }
+    if (to_domain_id < other.to_domain_id) {
+      return true;
+    }
+    if (to_domain_id > other.to_domain_id) {
+      return false;
+    }
+    int name_compare = action_name.compare(other.action_name);
+    if (name_compare < 0) {
+      return true;
+    }
+    if (name_compare > 0) {
+      return false;
+    }
+    int type_compare = type_name.compare(other.type_name);
+    if (type_compare < 0) {
+      return true;
+    }
+    return false;
+  }
+};
+
+}  // namespace domain_bridge
+
+#endif  // DOMAIN_BRIDGE__ACTION_BRIDGE_HPP_