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();