Skip to content

Commit

Permalink
Add method to get the current callback time and period
Browse files Browse the repository at this point in the history
  • Loading branch information
saikishor committed Nov 8, 2024
1 parent 2a67d35 commit ef478a6
Showing 1 changed file with 15 additions and 1 deletion.
16 changes: 15 additions & 1 deletion include/realtime_tools/async_function_handler.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -156,6 +156,18 @@ class AsyncFunctionHandler
*/
T get_last_return_value() const { return async_callback_return_; }

/// Get the current callback time
/**
* @return The current callback time
*/
const rclcpp::Time & get_current_callback_time() const { return current_callback_time_; }

/// Get the current callback period
/**
* @return The current callback period
*/
const rclcpp::Duration & get_current_callback_period() const { return current_callback_period_; }

/// Resets the internal variables of the AsyncFunctionHandler
/**
* A method to reset the internal variables of the AsyncFunctionHandler.
Expand All @@ -169,6 +181,8 @@ class AsyncFunctionHandler
std::unique_lock<std::mutex> lock(async_mtx_);
stop_async_callback_ = false;
trigger_in_progress_ = false;
current_callback_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED);
current_callback_period_ = rclcpp::Duration(0, 0);
last_execution_time_ = std::chrono::nanoseconds(0);
async_callback_return_ = T();
async_exception_ptr_ = nullptr;
Expand Down Expand Up @@ -304,7 +318,7 @@ class AsyncFunctionHandler
}

private:
rclcpp::Time current_callback_time_;
rclcpp::Time current_callback_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED);
rclcpp::Duration current_callback_period_{0, 0};

std::function<T(const rclcpp::Time &, const rclcpp::Duration &)> async_function_;
Expand Down

0 comments on commit ef478a6

Please sign in to comment.