260class AsyncFunctionHandler
263 AsyncFunctionHandler() =
default;
275 std::function<T(
const rclcpp::Time &,
const rclcpp::Duration &)> callback,
276 int thread_priority = 50)
278 if (callback ==
nullptr) {
279 throw std::runtime_error(
280 "AsyncFunctionHandler: parsed function to call asynchronously is not valid!");
282 if (thread_.joinable()) {
283 throw std::runtime_error(
284 "AsyncFunctionHandler: Cannot reinitialize while the thread is "
285 "running. Please stop the async callback first!");
287 async_function_ = callback;
288 thread_priority_ = thread_priority;
306 std::function<T(
const rclcpp::Time &,
const rclcpp::Duration &)> callback,
307 std::function<
bool()> trigger_predicate,
int thread_priority = 50)
309 if (trigger_predicate ==
nullptr) {
310 throw std::runtime_error(
"AsyncFunctionHandler: parsed trigger predicate is not valid!");
312 init(callback, thread_priority);
313 trigger_predicate_ = trigger_predicate;
317 std::function<T(
const rclcpp::Time &,
const rclcpp::Duration &)> callback,
321 init(callback, params.trigger_predicate, params.thread_priority);
323 pause_thread_ = params.wait_until_initial_trigger;
347 const rclcpp::Time & time,
const rclcpp::Duration & period)
350 throw std::runtime_error(
"AsyncFunctionHandler: need to be initialized first!");
352 if (async_exception_ptr_) {
354 params_.logger,
"AsyncFunctionHandler: Exception caught in the async callback thread!");
355 std::rethrow_exception(async_exception_ptr_);
362 "AsyncFunctionHandler is configured with DETACHED or SELF_PACED scheduling policy. "
363 "This means that the async callback may not be synchronized with the main thread.");
364 if (pause_thread_.load(std::memory_order_relaxed)) {
366 std::unique_lock<std::mutex> lock(async_mtx_);
367 pause_thread_ =
false;
368 RCLCPP_INFO(params_.logger,
"AsyncFunctionHandler: Resuming the async callback thread.");
369 async_callback_return_ = T();
370 auto const sync_period = std::chrono::nanoseconds(1'000'000'000 / params_.exec_rate);
371 previous_time_ = params_.clock->now() - rclcpp::Duration(sync_period);
373 async_callback_condition_.notify_one();
375 return std::make_pair(
true, async_callback_return_.load(std::memory_order_relaxed));
378 throw std::runtime_error(
379 "AsyncFunctionHandler: need to start the async callback thread first before triggering!");
381 std::unique_lock<std::mutex> lock(async_mtx_, std::try_to_lock);
382 bool trigger_status =
false;
383 if (lock.owns_lock() && !trigger_in_progress_ && trigger_predicate_()) {
385 std::unique_lock<std::mutex> scoped_lock(std::move(lock));
386 trigger_in_progress_ =
true;
387 current_callback_time_ = time;
388 current_callback_period_ = period;
390 async_callback_condition_.notify_one();
391 trigger_status =
true;
393 const T return_value = async_callback_return_;
394 return std::make_pair(trigger_status, return_value);
425 std::unique_lock<std::mutex> lock(async_mtx_);
426 stop_async_callback_ =
false;
427 trigger_in_progress_ =
false;
428 current_callback_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED);
429 current_callback_period_ = rclcpp::Duration(0, 0);
430 last_execution_time_ = std::chrono::nanoseconds(0);
431 async_callback_return_ = T();
432 async_exception_ptr_ =
nullptr;
442 std::unique_lock<std::mutex> lock(async_mtx_);
443 cycle_end_condition_.wait(lock, [
this] {
return !trigger_in_progress_; });
459 RCLCPP_INFO_EXPRESSION(
460 params_.logger, !pause_thread_,
"AsyncFunctionHandler: Pausing the async callback thread.");
462 pause_thread_ =
true;
466 pause_thread_.store(
true, std::memory_order_relaxed);
467 std::unique_lock<std::mutex> lock(async_mtx_);
471 return pause_thread_.load(std::memory_order_relaxed);
502 bool is_stopped()
const {
return stop_async_callback_.load(std::memory_order_relaxed); }
508 bool is_paused()
const {
return pause_thread_.load(std::memory_order_relaxed); }
543 stop_async_callback_.store(
true, std::memory_order_relaxed);
544 std::unique_lock<std::mutex> lock(async_mtx_);
546 async_callback_condition_.notify_one();
557 return last_execution_time_.load(std::memory_order_relaxed);
569 throw std::runtime_error(
"AsyncFunctionHandler: need to be initialized first!");
571 if (!thread_.joinable()) {
573 thread_ = std::thread([
this]() ->
void {
577 "Could not enable FIFO RT scheduling policy. Consider setting up your user to do FIFO "
578 "RT scheduling. See "
579 "[https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] "
582 if (!params_.cpu_affinity_cores.empty()) {
583 const auto affinity_result =
585 RCLCPP_WARN_EXPRESSION(
586 params_.logger, !affinity_result.first,
587 "Could not set CPU affinity for the async worker thread. Error: %s",
588 affinity_result.second.c_str());
590 std::string cores_list_print =
591 fmt::format(
"[{}]", fmt::join(params_.cpu_affinity_cores,
", "));
593 RCLCPP_WARN_EXPRESSION(
594 params_.logger, affinity_result.first,
595 "Async worker thread successfully pinned to Cores: %s", cores_list_print.c_str());
597 if (!params_.thread_name.empty()) {
600 if (!rename_result.first) {
602 params_.logger,
"Could not set thread name for the async worker thread. Error: %s",
603 rename_result.second.c_str());
605 RCLCPP_INFO(params_.logger,
"%s", rename_result.second.c_str());
611 execute_synchronized_callback();
615 execute_detached_callback();
618 throw std::runtime_error(
619 "AsyncFunctionHandler: start_thread(): Not a valid scheduling policy, this should "
628 void execute_synchronized_callback()
630 while (!stop_async_callback_.load(std::memory_order_relaxed)) {
632 std::unique_lock<std::mutex> lock(async_mtx_);
633 async_callback_condition_.wait(
634 lock, [
this] {
return trigger_in_progress_ || stop_async_callback_; });
635 if (!stop_async_callback_) {
636 const auto start_time = std::chrono::steady_clock::now();
638 async_callback_return_ =
639 async_function_(current_callback_time_, current_callback_period_);
641 async_exception_ptr_ = std::current_exception();
643 const auto end_time = std::chrono::steady_clock::now();
644 last_execution_time_ =
645 std::chrono::duration_cast<std::chrono::nanoseconds>(end_time - start_time);
647 trigger_in_progress_ =
false;
649 cycle_end_condition_.notify_all();
653 void execute_detached_callback()
655 if (!params_.clock) {
656 throw std::runtime_error(
657 "AsyncFunctionHandler: Clock must be set when using DETACHED or SELF_PACED scheduling "
660 if (params_.exec_rate == 0u) {
661 throw std::runtime_error(
662 "AsyncFunctionHandler: Execution rate must be set when using DETACHED or SELF_PACED "
667 auto const period = std::chrono::nanoseconds(1'000'000'000 / params_.exec_rate);
670 std::unique_lock<std::mutex> lock(async_mtx_);
671 async_callback_condition_.wait(
672 lock, [
this] {
return !pause_thread_ || stop_async_callback_; });
675 previous_time_ = params_.clock->now();
677 std::this_thread::sleep_for(period);
679 previous_time_ = params_.clock->now() - rclcpp::Duration(period);
681 std::chrono::steady_clock::time_point next_iteration_time{std::chrono::steady_clock::now()};
682 while (!stop_async_callback_.load(std::memory_order_relaxed)) {
684 std::unique_lock<std::mutex> lock(async_mtx_);
685 async_callback_condition_.wait(
686 lock, [
this] {
return !pause_thread_ || stop_async_callback_; });
687 if (!stop_async_callback_) {
689 auto const current_time = params_.clock->now();
690 auto const measured_period = current_time - previous_time_;
691 previous_time_ = current_time;
692 current_callback_time_ = current_time;
693 current_callback_period_ = measured_period;
695 const auto start_time = std::chrono::steady_clock::now();
697 async_callback_return_ = async_function_(current_time, measured_period);
699 async_exception_ptr_ = std::current_exception();
701 last_execution_time_ = std::chrono::duration_cast<std::chrono::nanoseconds>(
702 std::chrono::steady_clock::now() - start_time);
705 next_iteration_time += period;
706 const auto time_now = std::chrono::steady_clock::now();
707 if (next_iteration_time < time_now) {
708 const double time_diff =
709 std::chrono::duration<double, std::milli>(time_now - next_iteration_time).count();
710 const double cm_period = 1.e3 /
static_cast<double>(params_.exec_rate);
711 const int overrun_count =
static_cast<int>(std::ceil(time_diff / cm_period));
712 if (params_.print_warnings) {
713 RCLCPP_WARN_THROTTLE(
714 params_.logger, *params_.clock, 1000,
715 "Overrun detected! The async callback missed its desired rate of %d Hz. The loop "
716 "took %f ms (missed cycles : %d).",
717 params_.exec_rate, time_diff + cm_period, overrun_count + 1);
719 next_iteration_time += (overrun_count * period);
721 std::this_thread::sleep_until(next_iteration_time);
723 const auto minimum_period = period / 10;
724 const auto last_period = last_execution_time_.load(std::memory_order_relaxed);
725 if (last_period < minimum_period) {
726 if (params_.print_warnings) {
727 RCLCPP_WARN_THROTTLE(
728 params_.logger, *params_.clock, 1000,
729 "The SELF_PACED callback took less than 10%% of the cycle period. Check whether "
730 "your read() blocks. The handler sleeps for 100 us to protect the CPU.");
732 std::this_thread::sleep_for(std::chrono::microseconds(100));
736 trigger_in_progress_ =
false;
738 cycle_end_condition_.notify_all();
742 rclcpp::Time current_callback_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED);
743 rclcpp::Duration current_callback_period_{0, 0};
745 std::function<T(
const rclcpp::Time &,
const rclcpp::Duration &)> async_function_;
746 std::function<bool()> trigger_predicate_ = []() {
return true; };
750 AsyncFunctionHandlerParams params_;
751 rclcpp::Time previous_time_{0, 0, RCL_CLOCK_UNINITIALIZED};
752 int thread_priority_ = std::numeric_limits<int>::quiet_NaN();
753 std::atomic_bool stop_async_callback_{
false};
754 std::atomic_bool trigger_in_progress_{
false};
755 std::atomic_bool pause_thread_{
false};
756 std::atomic<T> async_callback_return_;
757 std::condition_variable async_callback_condition_;
758 std::condition_variable cycle_end_condition_;
759 std::mutex async_mtx_;
760 std::atomic<std::chrono::nanoseconds> last_execution_time_;
761 std::atomic<double> periodicity_;
762 std::exception_ptr async_exception_ptr_;