diff --git a/.gitmodules b/.gitmodules index 23b0e9e..66cfb0c 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,10 @@ [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 1418190..412d818 160000 --- a/src/cyclonedds +++ b/src/cyclonedds @@ -1 +1 @@ -Subproject commit 14181900b7bfd89ced1dcc5f33cab50580cf24da +Subproject commit 412d818269b6e8e5b7eae32f7e245935bd30f4a7 diff --git a/src/full_topology/src/full_topology.cpp b/src/full_topology/src/full_topology.cpp index 523d0c0..e260777 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{rclcpp::ExecutorOptions(), 2}; + // 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; + // 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 9a1146a..a42ae60 100644 --- a/src/full_topology/src/simple.cpp +++ b/src/full_topology/src/simple.cpp @@ -79,13 +79,10 @@ 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; @@ -94,30 +91,16 @@ 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 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); + // 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 - 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); @@ -135,7 +118,6 @@ 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); @@ -143,7 +125,6 @@ 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()) @@ -154,7 +135,6 @@ int main(int argc, char **argv) << std::endl; std::cout << *strategy->get_priority_settings(mapping->subscription_->get_subscription_handle()) << std::endl; -#endif executor.spin(); diff --git a/src/rcl b/src/rcl index df1f49f..5da7e19 160000 --- a/src/rcl +++ b/src/rcl @@ -1 +1 @@ -Subproject commit df1f49f299242430c1e4949859625dff2e5ede60 +Subproject commit 5da7e193371c25cf1ebd2489978768327c382c04 diff --git a/src/rclcpp b/src/rclcpp index c0d7ba5..e57a3a3 160000 --- a/src/rclcpp +++ b/src/rclcpp @@ -1 +1 @@ -Subproject commit c0d7ba53666616776af4190844d05b677d411c9a +Subproject commit e57a3a393e46f0c0e0e5cc64090ac34e5cef32bc diff --git a/src/rmw_cyclonedds b/src/rmw_cyclonedds index 90d9bc7..79e0792 160000 --- a/src/rmw_cyclonedds +++ b/src/rmw_cyclonedds @@ -1 +1 @@ -Subproject commit 90d9bc72965fec0f2e3bbe495a6f946d4c31a7d6 +Subproject commit 79e07924d66b205838d28da4c9df6a941df6795c diff --git a/src/ros2_tracing b/src/ros2_tracing index 0d51819..e8637c9 160000 --- a/src/ros2_tracing +++ b/src/ros2_tracing @@ -1 +1 @@ -Subproject commit 0d51819f5f54d586fb614ce5e1dd9fda96b87675 +Subproject commit e8637c9043b9bc396135dbfa7068143bfa8a879c diff --git a/src/tracetools_analysis b/src/tracetools_analysis new file mode 160000 index 0000000..e18ddb6 --- /dev/null +++ b/src/tracetools_analysis @@ -0,0 +1 @@ +Subproject commit e18ddb62d580e6f56dc4e232370a2c93beaedb7c