KPE-330-pendulum executor ROS2 refactorization

pendulum_control/CMakeLists.txt

@@ -22,6 +22,12 @@ find_package(rclcpp REQUIRED)
 find_package(pendulum_msgs REQUIRED)
 find_package(rttest)
 find_package(tlsf_cpp)
+find_package(kpsr_ros2_executor REQUIRED)
+find_package(Klepsydra REQUIRED)
+find_package(KlepsydraAdmin REQUIRED)
+find_package(KlepsydraStreaming REQUIRED)
+
+set(CMAKE_THREAD_LIBS_INIT "-lpthread")
 
 include_directories(include)
 
@@ -32,11 +38,13 @@ endif()
 
 add_executable(pendulum_demo
   src/pendulum_demo.cpp)
+
 ament_target_dependencies(pendulum_demo
   "pendulum_msgs"
   "rclcpp"
   "rttest"
-  "tlsf_cpp")
+  "tlsf_cpp"
+  "kpsr_ros2_executor")
 
 add_executable(pendulum_logger
   src/pendulum_logger.cpp)

pendulum_control/include/pendulum_control/rtt_executor.hpp

@@ -41,8 +41,11 @@ class RttExecutor : public rclcpp::Executor
    * Extends default Executor constructor
    */
   RttExecutor(
-    const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions())
-  : rclcpp::Executor(options), running(false)
+    const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions(),
+    rclcpp::Executor::SharedPtr decorableExecutor = nullptr)
+  : rclcpp::Executor(options)
+  , running(false)
+  , decorableExecutor(decorableExecutor)
   {
     rttest_ready = rttest_running();
     memset(&start_time_, 0, sizeof(timespec));
@@ -120,6 +123,25 @@ class RttExecutor : public rclcpp::Executor
     return 0;
   }
 
+  void spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0)) override
+  {
+    if (decorableExecutor) {
+      decorableExecutor->spin_some(max_duration);
+    } else {
+      rclcpp::Executor::spin_some();
+    }
+
+  }
+
+  void add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true) override
+  {
+    if (decorableExecutor) {
+      decorableExecutor->add_node(node_ptr, notify);
+    } else {
+      rclcpp::Executor::add_node(node_ptr);
+    }
+  }
+
   // For storing accumulated performance statistics.
   rttest_results results;
   bool results_available{false};
@@ -135,9 +157,10 @@ class RttExecutor : public rclcpp::Executor
   timespec start_time_;
 
 private:
+  rclcpp::Executor::SharedPtr decorableExecutor;
   RCLCPP_DISABLE_COPY(RttExecutor)
 };
 
 }  // namespace pendulum_control
 
-#endif  // PENDULUM_CONTROL__RTT_EXECUTOR_HPP_
+#endif  // PENDULUM_CONTROL__RTT_EXECUTOR_HPP_

pendulum_control/src/pendulum_demo.cpp

@@ -36,6 +36,8 @@
 #include "pendulum_control/pendulum_motor.hpp"
 #include "pendulum_control/rtt_executor.hpp"
 
+#include <kpsr_ros2_executor/streaming_executor_factory.hpp>
+#include <kpsr_ros2_executor/streaming_executor.hpp>
 
 static bool running = false;
 
@@ -96,6 +98,7 @@ using TLSFAllocator = tlsf_heap_allocator<T>;
 
 int main(int argc, char * argv[])
 {
+  char threadedExecutorOption = *argv[1];
   // Initialization phase.
   // In the initialization phase of a realtime program, non-realtime-safe operations such as
   // allocation memory are permitted.
@@ -127,10 +130,11 @@ int main(int argc, char * argv[])
   // message pool is determined by the number of threads (the maximum number of concurrent accesses
   // to the subscription).
   // Since this example is single-threaded, we choose a message pool size of 1 for each strategy.
+
   auto state_msg_strategy =
-    std::make_shared<MessagePoolMemoryStrategy<pendulum_msgs::msg::JointState, 1>>();
+    std::make_shared<MessagePoolMemoryStrategy<pendulum_msgs::msg::JointState, 16>>();
   auto command_msg_strategy =
-    std::make_shared<MessagePoolMemoryStrategy<pendulum_msgs::msg::JointCommand, 1>>();
+    std::make_shared<MessagePoolMemoryStrategy<pendulum_msgs::msg::JointCommand, 16>>();
   auto setpoint_msg_strategy =
     std::make_shared<MessagePoolMemoryStrategy<pendulum_msgs::msg::JointCommand, 1>>();
 
@@ -219,7 +223,36 @@ int main(int argc, char * argv[])
   options.memory_strategy = memory_strategy;
   // RttExecutor is a special single-threaded executor instrumented to calculate and record
   // real-time performance statistics.
-  auto executor = std::make_shared<pendulum_control::RttExecutor>(options);
+
+  rclcpp::Executor::SharedPtr threadedExecutor;
+
+  switch(threadedExecutorOption) {
+    case '0':
+      std::cout << "Option 0: Parent executor" << std::endl;
+      threadedExecutor = nullptr;
+      break;
+    case '1':
+      std::cout << "Option 1: SingleThreadedExecutor" << std::endl;
+      threadedExecutor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+      break;
+    case '2':
+      std::cout << "Option 2: MultiThreadedExecutor" << std::endl;
+      threadedExecutor = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
+      break;
+    case '3':
+      std::cout << "Option 3: StaticSingleThreadedExecutor" << std::endl;
+      threadedExecutor = std::make_shared<rclcpp::executors::StaticSingleThreadedExecutor>();
+      break;
+    case '4':
+      std::cout << "Option 4: KlepsydraExecutor" << std::endl;
+      threadedExecutor = kpsr::ros2::StreamingExecutorFactory::createExecutor(false, options);
+      break;
+    default:
+      std::cout << "No option provided for the ThreadedExecutorType. Default value: SingleThreadedExecutor" << std::endl;
+      threadedExecutor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+  }
+
+  auto executor = std::make_shared<pendulum_control::RttExecutor>(options, threadedExecutor);
 
   // Add the motor and controller nodes to the executor.
   executor->add_node(motor_node);
@@ -306,6 +339,9 @@ int main(int argc, char * argv[])
   printf("PendulumMotor received %zu messages\n", pendulum_motor->messages_received);
   printf("PendulumController received %zu messages\n", pendulum_controller->messages_received);
 
+  if (threadedExecutorOption == '4') {
+    kpsr::ros2::StreamingExecutorFactory::deleteAllExecutors();
+  }
   rclcpp::shutdown();
 
   return 0;