From f5c8593b534f0ebea0837b9eb596fcbbecec5e7d Mon Sep 17 00:00:00 2001 From: Niklas Halle Date: Wed, 30 Jul 2025 16:27:34 +0200 Subject: [PATCH 1/2] updated submodules --- .gitmodules | 4 ---- src/cyclonedds | 2 +- src/rcl | 2 +- src/rclcpp | 2 +- src/rmw_cyclonedds | 2 +- src/ros2_tracing | 2 +- src/tracetools_analysis | 1 - 7 files changed, 5 insertions(+), 10 deletions(-) delete mode 160000 src/tracetools_analysis diff --git a/.gitmodules b/.gitmodules index 66cfb0c..23b0e9e 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,10 +1,6 @@ [submodule "src/ros_edf"] path = src/ros_edf url = git@git.niklashalle.net:niklas/ROS-Dynamic-Executor.git -[submodule "src/tools/tracetools_analysis"] - path = src/tracetools_analysis - url = git@git.niklashalle.net:niklas/tracetools_analysis.git - branch = foxy [submodule "src/rcl"] path = src/rcl url = git@git.niklashalle.net:niklas/rcl.git diff --git a/src/cyclonedds b/src/cyclonedds index 412d818..1418190 160000 --- a/src/cyclonedds +++ b/src/cyclonedds @@ -1 +1 @@ -Subproject commit 412d818269b6e8e5b7eae32f7e245935bd30f4a7 +Subproject commit 14181900b7bfd89ced1dcc5f33cab50580cf24da diff --git a/src/rcl b/src/rcl index 5da7e19..df1f49f 160000 --- a/src/rcl +++ b/src/rcl @@ -1 +1 @@ -Subproject commit 5da7e193371c25cf1ebd2489978768327c382c04 +Subproject commit df1f49f299242430c1e4949859625dff2e5ede60 diff --git a/src/rclcpp b/src/rclcpp index e57a3a3..c0d7ba5 160000 --- a/src/rclcpp +++ b/src/rclcpp @@ -1 +1 @@ -Subproject commit e57a3a393e46f0c0e0e5cc64090ac34e5cef32bc +Subproject commit c0d7ba53666616776af4190844d05b677d411c9a diff --git a/src/rmw_cyclonedds b/src/rmw_cyclonedds index 79e0792..90d9bc7 160000 --- a/src/rmw_cyclonedds +++ b/src/rmw_cyclonedds @@ -1 +1 @@ -Subproject commit 79e07924d66b205838d28da4c9df6a941df6795c +Subproject commit 90d9bc72965fec0f2e3bbe495a6f946d4c31a7d6 diff --git a/src/ros2_tracing b/src/ros2_tracing index e8637c9..0d51819 160000 --- a/src/ros2_tracing +++ b/src/ros2_tracing @@ -1 +1 @@ -Subproject commit e8637c9043b9bc396135dbfa7068143bfa8a879c +Subproject commit 0d51819f5f54d586fb614ce5e1dd9fda96b87675 diff --git a/src/tracetools_analysis b/src/tracetools_analysis deleted file mode 160000 index e18ddb6..0000000 --- a/src/tracetools_analysis +++ /dev/null @@ -1 +0,0 @@ -Subproject commit e18ddb62d580e6f56dc4e232370a2c93beaedb7c From c552c40571b78975224630962f137a64a1125a9f Mon Sep 17 00:00:00 2001 From: Niklas Halle Date: Wed, 30 Jul 2025 16:28:46 +0200 Subject: [PATCH 2/2] allow scheduler switch in simple example --- src/full_topology/src/full_topology.cpp | 12 +++++----- src/full_topology/src/simple.cpp | 32 ++++++++++++++++++++----- 2 files changed, 32 insertions(+), 12 deletions(-) diff --git a/src/full_topology/src/full_topology.cpp b/src/full_topology/src/full_topology.cpp index e260777..523d0c0 100644 --- a/src/full_topology/src/full_topology.cpp +++ b/src/full_topology/src/full_topology.cpp @@ -109,13 +109,13 @@ int main(int argc, char **argv) executor.prio_memory_strategy_ = strategy; #else // default: ROS #ifdef MULTI_THREADED - // Create a multi-threaded executor to handle all nodes - std::cout << "multi threaded ros default" << std::endl; - rclcpp::executors::MultiThreadedExecutor executor; + // Create a multi-threaded executor to handle all nodes + std::cout << "multi threaded ros default" << std::endl; + rclcpp::executors::MultiThreadedExecutor executor{rclcpp::ExecutorOptions(), 2}; #else - // Create a single-threaded executor to handle all nodes - std::cout << "single threaded ros default" << std::endl; - rclcpp::executors::SingleThreadedExecutor executor; + // Create a single-threaded executor to handle all nodes + std::cout << "single threaded ros default" << std::endl; + rclcpp::executors::SingleThreadedExecutor executor; #endif #endif diff --git a/src/full_topology/src/simple.cpp b/src/full_topology/src/simple.cpp index a42ae60..9a1146a 100644 --- a/src/full_topology/src/simple.cpp +++ b/src/full_topology/src/simple.cpp @@ -79,10 +79,13 @@ int main(int argc, char **argv) { + std::cout << "hardware_concurrency: " << std::thread::hardware_concurrency() << " threads." << std::endl; + DECLARE_CHAIN(0); // Chain 1: Image2Ground Mapping rclcpp::init(argc, argv); +#ifdef EDF_PRIORITY_EXECUTOR // Create a priority executor with a custom memory strategy auto strategy = std::make_shared>(); rclcpp::ExecutorOptions options; @@ -91,16 +94,30 @@ int main(int argc, char **argv) // Must be set to post_execute can set new deadlines executor.prio_memory_strategy_ = strategy; +#else // default: ROS + #ifdef MULTI_THREADED + // Create a multi-threaded executor to handle all nodes + std::cout << "multi threaded ros default" << std::endl; + rclcpp::executors::MultiThreadedExecutor executor; + #else + // Create a single-threaded executor to handle all nodes + std::cout << "single threaded ros default" << std::endl; + rclcpp::executors::SingleThreadedExecutor executor; + #endif +#endif double fps = 60.0; // Frames per second for the camera double period = 1000.0 / fps; // Period in milliseconds - // chain id, node name, node class, input topic, output topic, duration in ms - MAKE_INPUT_NODE( 0, cameraA , CameraNode , /*has no input topic*/ "/input/cameraA/raw" , period); // 60 FPS camera, fast - MAKE_PROCESSING_NODE(0, debayerA , DebayerNode , "/input/cameraA/raw" , "/cameraA/debayered" , 18); // Moderate, image conversion - MAKE_PROCESSING_NODE(0, radiometricA, RadiometricNode, "/cameraA/debayered" , "/cameraA/radiometric" , 22); // Heavier image math - MAKE_PROCESSING_NODE(0, geometric , GeometricNode , "/cameraA/radiometric", "/cameraA/geometric" , 12); // Perspective transform, medium - MAKE_OUTPUT_NODE( 0, mapping , MappingNode , "/cameraA/geometric" , "/output/cameraA/mapped", 16); // Warping/stitching, moderate + // chain id, node name, node class, input topic, output topic, duration or period in ms + MAKE_INPUT_NODE( 0, cameraA , CameraNode , /*has no input topic*/ "/input/cameraA/raw" , period); + MAKE_PROCESSING_NODE(0, debayerA , DebayerNode , "/input/cameraA/raw" , "/cameraA/debayered" , 18); + MAKE_PROCESSING_NODE(0, radiometricA, RadiometricNode, "/cameraA/debayered" , "/cameraA/radiometric" , 22); + MAKE_PROCESSING_NODE(0, geometric , GeometricNode , "/cameraA/radiometric", "/cameraA/geometric" , 12); + MAKE_OUTPUT_NODE( 0, mapping , MappingNode , "/cameraA/geometric" , "/output/cameraA/mapped", 16); + period = 1000; + +#ifdef EDF_PRIORITY_EXECUTOR // the new funcitons in PriorityMemoryStrategy accept the handle of the // timer/subscription as the first argument strategy->set_executable_deadline(cameraA->timer_->get_timer_handle() , period, priority_executor::ExecutableType::TIMER , 0); @@ -118,6 +135,7 @@ int main(int argc, char **argv) // you _must_ set the timer_handle of the last callback in the chain to the timer of the first node strategy->get_priority_settings(mapping->subscription_->get_subscription_handle())->timer_handle = cameraA->timer_; +#endif executor.add_node(cameraA); executor.add_node(debayerA); @@ -125,6 +143,7 @@ int main(int argc, char **argv) executor.add_node(geometric); executor.add_node(mapping); +#ifdef EDF_PRIORITY_EXECUTOR std::cout << *strategy->get_priority_settings(cameraA->timer_->get_timer_handle()) << std::endl; std::cout << *strategy->get_priority_settings(debayerA->subscription_->get_subscription_handle()) @@ -135,6 +154,7 @@ int main(int argc, char **argv) << std::endl; std::cout << *strategy->get_priority_settings(mapping->subscription_->get_subscription_handle()) << std::endl; +#endif executor.spin();