Dispatcher Executor

rclc/include/rclc/executor.h

@@ -83,6 +83,8 @@ typedef struct
   void * trigger_object;
   /// data communication semantics
   rclc_executor_semantics_t data_comm_semantics;
+  /// interface for multi-threaded executor
+  void * multi_threaded;
 } rclc_executor_t;
 
 /**
@@ -273,6 +275,7 @@ rclc_executor_add_subscription_with_context(
   void * context,
   rclc_executor_handle_invocation_t invocation);
 
+
 /**
  *  Adds a timer to an executor.
  * * An error is returned, if {@link rclc_executor_t.handles} array is full.

rclc/include/rclc/executor_handle.h

@@ -27,9 +27,6 @@ extern "C"
 #include <rclc/action_client.h>
 #include <rclc/action_server.h>
 
-/// TODO (jst3si) Where is this defined? - in my build environment this variable is not set.
-// #define ROS_PACKAGE_NAME "rclc"
-
 /// Enumeration for timer, subscription, guard conditions etc to be waited on.
 typedef enum
 {
@@ -165,6 +162,8 @@ typedef struct
   /// Interval variable. Flag, which is true, if new data is available from DDS queue
   /// (is set after calling rcl_take)
   bool data_available;
+  /// interface for multi-threaded data
+  void * multi_threaded;
 } rclc_executor_handle_t;
 
 /// Information about total number of subscriptions, guard_conditions, timers, subscription etc.

rclc/src/rclc/executor.c

@@ -15,6 +15,7 @@
 // limitations under the License.
 
 #include "rclc/executor.h"
+#include <unistd.h>
 #include <rcutils/time.h>
 
 #include "./action_generic_types.h"
@@ -33,32 +34,6 @@
 // default timeout for rcl_wait() is 1000ms
 #define DEFAULT_WAIT_TIMEOUT_NS 1000000000
 
-// declarations of helper functions
-/*
-/// get new data from DDS queue for handle i
-static
-rcl_ret_t
-_rclc_check_for_new_data(rclc_executor_t * executor, rcl_wait_set_t * wait_set, size_t i);
-
-/// get new data from DDS queue for handle i
-static
-rcl_ret_t
-_rclc_take_new_data(rclc_executor_t * executor, rcl_wait_set_t * wait_set, size_t i);
-
-
-/// execute callback of handle i
-static
-rcl_ret_t
-_rclc_execute(rclc_executor_t * executor, rcl_wait_set_t * wait_set, size_t i);
-
-static
-rcl_ret_t
-_rclc_let_scheduling(rclc_executor_t * executor, rcl_wait_set_t * wait_set);
-*/
-
-// rationale: user must create an executor with:
-// executor = rclc_executor_get_zero_initialized_executor();
-// then handles==NULL or not (e.g. properly initialized)
 static
 bool
 _rclc_executor_is_valid(rclc_executor_t * executor)
@@ -110,6 +85,7 @@ rclc_executor_init(
     return RCL_RET_INVALID_ARGUMENT;
   }
 
+
   rcl_ret_t ret = RCL_RET_OK;
   (*executor) = rclc_executor_get_zero_initialized_executor();
   executor->context = context;
@@ -121,16 +97,16 @@ rclc_executor_init(
   // allocate memory for the array
   executor->handles =
     executor->allocator->allocate(
-    (number_of_handles * sizeof(rclc_executor_handle_t)),
+    (executor->max_handles * sizeof(rclc_executor_handle_t)),
     executor->allocator->state);
   if (NULL == executor->handles) {
     RCL_SET_ERROR_MSG("Could not allocate memory for 'handles'.");
     return RCL_RET_BAD_ALLOC;
   }
 
   // initialize handle
-  for (size_t i = 0; i < number_of_handles; i++) {
-    rclc_executor_handle_init(&executor->handles[i], number_of_handles);
+  for (size_t i = 0; i < executor->max_handles; i++) {
+    rclc_executor_handle_init(&executor->handles[i], executor->max_handles);
   }
 
   // initialize #counts for handle types
@@ -251,7 +227,6 @@ rclc_executor_add_subscription(
   return ret;
 }
 
-
 rcl_ret_t
 rclc_executor_add_subscription_with_context(
   rclc_executor_t * executor,
@@ -1773,7 +1748,7 @@ rclc_executor_prepare(rclc_executor_t * executor)
   // (2) executor_add_timer() or executor_add_subscription() has been called.
   //     i.e. a new timer or subscription has been added to the Executor.
   if (!rcl_wait_set_is_valid(&executor->wait_set)) {
-    // calling wait_set on zero_initialized wait_set multiple times is ok.
+    // calling wait_set_fini on zero_initialized wait_set multiple times is ok.
     rcl_ret_t rc = rcl_wait_set_fini(&executor->wait_set);
     if (rc != RCL_RET_OK) {
       PRINT_RCLC_ERROR(rclc_executor_spin_some, rcl_wait_set_fini);

rclc/src/rclc/executor_handle.c

@@ -60,6 +60,7 @@ rclc_executor_handle_init(
   handle->index = max_handles;
   handle->initialized = false;
   handle->data_available = false;
+
   return RCL_RET_OK;
 }
 

rclc_dispatcher_executor/CHANGELOG.rst

@@ -0,0 +1,7 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package rclc_dispatcher_executor
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.0.1 (2022-11-24)
+------------------
+* Initial release

rclc_dispatcher_executor/CMakeLists.txt

@@ -0,0 +1,162 @@
+cmake_minimum_required(VERSION 3.5)
+
+project(rclc_dispatcher_executor)
+
+set(CMAKE_VERBOSE_MAKEFILE ON)
+
+#################################################
+# compiler settings
+#################################################
+
+# Default to C11
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 11)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+set(CMAKE_VERBOSE_MAKEFILE ON)
+
+#################################################
+# package dependencies
+#################################################
+find_package(ament_cmake_ros REQUIRED)
+find_package(rcl REQUIRED)
+find_package(rclc REQUIRED)
+find_package(rcl_action REQUIRED)
+find_package(rcutils REQUIRED)
+find_package(rosidl_generator_c REQUIRED)
+find_package(std_msgs REQUIRED)
+
+if("${rcl_VERSION}" VERSION_LESS "1.0.0")
+  message(STATUS
+    "Found rcl version ${rcl_VERSION}, which belongs to Dashing or Eloquent")
+  # Later, with CMake 3.12+ use:
+  # add_compile_definitions(USE_RCL_WAIT_SET_IS_VALID_BACKPORT)
+  add_definitions(-DUSE_RCL_WAIT_SET_IS_VALID_BACKPORT)
+else()
+  message(STATUS
+    "Found rcl version ${rcl_VERSION}, which belongs to Foxy or later")
+  find_package(rosidl_runtime_c REQUIRED)
+endif()
+
+#################################################
+# create library
+#################################################
+
+add_library(${PROJECT_NAME}
+  src/rclc_dispatcher_executor/dispatcher_executor.c
+)
+
+target_include_directories(${PROJECT_NAME}
+    PUBLIC
+        $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>
+    PRIVATE
+        $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/src>
+)
+
+target_link_libraries(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
+# specific order: dependents before dependencies
+ament_target_dependencies(${PROJECT_NAME}
+  rcl
+  rclc
+  rcl_action
+  rcutils
+  rosidl_generator_c
+)
+
+#################################################
+# install
+#################################################
+#install(DIRECTORY include/ DESTINATION include)
+#install(TARGETS ${PROJECT_NAME} DESTINATION lib)
+
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(${PROJECT_NAME}
+  PRIVATE "RCLC_DISPATCHER_EXECUTOR_BUILDING_LIBRARY")
+
+install(
+  TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+# specific order: dependents before dependencies
+ament_export_include_directories(include)
+ament_export_libraries(${PROJECT_NAME})
+
+if("${rcl_VERSION}" VERSION_LESS "1.1.10")
+# ament_export_targets command is not available for Dashing
+else()
+ament_export_targets(${PROJECT_NAME})
+endif()
+
+
+#################################################
+# gtest
+#################################################
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(ament_lint_auto REQUIRED)
+  find_package(osrf_testing_tools_cpp REQUIRED)
+  find_package(std_msgs REQUIRED)
+  find_package(example_interfaces REQUIRED)
+  find_package(rclcpp REQUIRED)
+  find_package(rclcpp_action REQUIRED)
+
+  # No copyright header check since link to NOTICE file is not recognized properly.
+  set(ament_cmake_copyright_FOUND TRUE)
+
+  ament_lint_auto_find_test_dependencies()
+
+  ament_add_gtest(${PROJECT_NAME}_test
+    test/rclc_dispatcher_executor/test_dispatcher_executor.cpp
+
+  )
+
+  target_include_directories(${PROJECT_NAME}_test PRIVATE include src)
+  target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME})
+  ament_target_dependencies(${PROJECT_NAME}_test
+    rclcpp
+    rclcpp_action
+    rcl
+    rclc
+    rcutils
+    rosidl_generator_c
+    osrf_testing_tools_cpp
+    std_msgs
+    test_msgs
+    example_interfaces
+  )
+
+
+endif()
+
+#################################################
+# export dependencies
+#################################################
+# specific order: dependents before dependencies
+ament_export_include_directories(include)
+ament_export_libraries(${PROJECT_NAME})
+ament_export_dependencies(ament_cmake)
+ament_export_dependencies(rcl)
+ament_export_dependencies(rclc)
+ament_export_dependencies(rcl_action)
+ament_export_dependencies(rcutils)
+ament_export_dependencies(rosidl_generator_c)
+ament_package()

rclc_dispatcher_executor/README.md

@@ -0,0 +1,60 @@
+# The rclc dispatcher executor
+
+Supports multi-threaded executor and prioritization of callbacks.
+
+TODO
+- Functionality
+  - rclc_dispatcher_executor_spin_some() is necessary only spin once for testing
+- Unit tests:
+  - initialization
+  - functional test
+  - how to check that executor has been initialized by rclc_executor_init?
+
+- Example:
+  - check priority of the thread
+  - design meaningful functional test (two prios one gets more work done)
+  - priority is not set correctly for worker thread
+
+- Clean-up
+  - reduce dependencies in CmakeLists.txt
+
+- Write documentation here in README
+  - call only one _init function?
+  - introspection, display which is the actual priority of the thread
+
+## Table of contents
+*   [Initialization](#initialization)
+*   [Configuration](#configuration)
+*   [Runtime](#runtime)
+*   [Cleaning up](#cleaning-up)
+
+## Initialization
+
+- Default initialization:
+    ```c
+TODO
+    ```
+
+## Configuration
+
+
+TODO
+
+## Runtime
+
+TODO
+
+```c
+// xxx
+TODO
+```
+
+
+## Cleaning up
+
+To destroy an initialized executor:
+
+```c
+// Delete dispatching executor
+TODO
+```