From dd431cbb88011c95a07f073a32ff990e39b6702a Mon Sep 17 00:00:00 2001 From: paulagb8894 Date: Tue, 19 Jul 2022 14:17:03 +0200 Subject: [PATCH] pendulum executor ROS2 refactorization menu for pendulum demo to choose between the different threaded executors kpsr executor in pendulum demo included --- pendulum_control/CMakeLists.txt | 10 ++++- .../include/pendulum_control/rtt_executor.hpp | 29 +++++++++++-- pendulum_control/src/pendulum_demo.cpp | 42 +++++++++++++++++-- 3 files changed, 74 insertions(+), 7 deletions(-) diff --git a/pendulum_control/CMakeLists.txt b/pendulum_control/CMakeLists.txt index f04bd876e..58387ab50 100644 --- a/pendulum_control/CMakeLists.txt +++ b/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) diff --git a/pendulum_control/include/pendulum_control/rtt_executor.hpp b/pendulum_control/include/pendulum_control/rtt_executor.hpp index da35f3537..9712f66f7 100644 --- a/pendulum_control/include/pendulum_control/rtt_executor.hpp +++ b/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 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_ \ No newline at end of file diff --git a/pendulum_control/src/pendulum_demo.cpp b/pendulum_control/src/pendulum_demo.cpp index 4efbf7a97..6aafcd88d 100644 --- a/pendulum_control/src/pendulum_demo.cpp +++ b/pendulum_control/src/pendulum_demo.cpp @@ -36,6 +36,8 @@ #include "pendulum_control/pendulum_motor.hpp" #include "pendulum_control/rtt_executor.hpp" +#include +#include static bool running = false; @@ -96,6 +98,7 @@ using TLSFAllocator = tlsf_heap_allocator; 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>(); + std::make_shared>(); auto command_msg_strategy = - std::make_shared>(); + std::make_shared>(); auto setpoint_msg_strategy = std::make_shared>(); @@ -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(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(); + break; + case '2': + std::cout << "Option 2: MultiThreadedExecutor" << std::endl; + threadedExecutor = std::make_shared(); + break; + case '3': + std::cout << "Option 3: StaticSingleThreadedExecutor" << std::endl; + threadedExecutor = std::make_shared(); + 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(); + } + + auto executor = std::make_shared(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;