From 5f73198adbabb3a34985fb5e5c7f84b5e4149a86 Mon Sep 17 00:00:00 2001 From: Niklas Halle Date: Mon, 9 Jun 2025 15:21:21 +0200 Subject: [PATCH] added tracepoints, removed cout --- src/priority_executor/src/priority_executor.cpp | 2 ++ src/priority_executor/src/priority_memory_strategy.cpp | 4 ++-- src/priority_executor/src/usage_example.cpp | 3 +-- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/priority_executor/src/priority_executor.cpp b/src/priority_executor/src/priority_executor.cpp index f9eee3e..a3662c0 100755 --- a/src/priority_executor/src/priority_executor.cpp +++ b/src/priority_executor/src/priority_executor.cpp @@ -95,6 +95,7 @@ bool TimedExecutor::get_next_executable( void TimedExecutor::wait_for_work(std::chrono::nanoseconds timeout) { + TRACEPOINT(rclcpp_executor_wait_for_work, timeout.count()); { std::unique_lock lock(memory_strategy_mutex_); @@ -166,6 +167,7 @@ TimedExecutor::wait_for_work(std::chrono::nanoseconds timeout) bool TimedExecutor::get_next_ready_executable(rclcpp::AnyExecutable &any_executable) { + TRACEPOINT(rclcpp_executor_get_next_ready); bool success = false; if (use_priorities) { diff --git a/src/priority_executor/src/priority_memory_strategy.cpp b/src/priority_executor/src/priority_memory_strategy.cpp index 68f6295..df99999 100755 --- a/src/priority_executor/src/priority_memory_strategy.cpp +++ b/src/priority_executor/src/priority_memory_strategy.cpp @@ -255,8 +255,8 @@ PriorityExecutable::PriorityExecutable( ExecutableType t, ExecutableScheduleType sched_type) { handle = std::move(h); - std::cout << "priority_executable constructor called" << std::endl; - std::cout << "type: " << t << std::endl; + //std::cout << "priority_executable constructor called" << std::endl; + //std::cout << "type: " << t << std::endl; type = t; if (sched_type == ExecutableScheduleType::CHAIN_INDEPENDENT_PRIORITY diff --git a/src/priority_executor/src/usage_example.cpp b/src/priority_executor/src/usage_example.cpp index b9c4b4c..c645d5c 100755 --- a/src/priority_executor/src/usage_example.cpp +++ b/src/priority_executor/src/usage_example.cpp @@ -76,8 +76,7 @@ int main(int argc, char* argv[]) { strategy->set_executable_deadline(talker->timer_->get_timer_handle(), 1000, priority_executor::ExecutableType::TIMER, 0); // you _must_ set the timer_handle for each chain - strategy->get_priority_settings(talker->timer_->get_timer_handle()) - ->timer_handle = talker->timer_; + strategy->get_priority_settings(talker->timer_->get_timer_handle())->timer_handle = talker->timer_; // you _must_ mark the first executable in the chain strategy->set_first_in_chain(talker->timer_->get_timer_handle()); // set the same period and chain_id for each callback in the chain