From 4ddb76f466d331e4ca8ff4df7ba25a5dce8496af Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Fri, 27 Jul 2018 18:27:25 -0700 Subject: [PATCH] construct TimerBase/GenericTimer with Clock (#523) * construct TimerBase/GenericTimer with Clock * pass rcl_time_point_value_t to rcl_clock_get_now * update docblocks --- rclcpp/include/rclcpp/clock.hpp | 4 +++ rclcpp/include/rclcpp/timer.hpp | 43 +++++++++++++++++++++++-------- rclcpp/src/rclcpp/clock.cpp | 8 +++++- rclcpp/src/rclcpp/time_source.cpp | 2 +- rclcpp/src/rclcpp/timer.cpp | 7 +++-- 5 files changed, 49 insertions(+), 15 deletions(-) diff --git a/rclcpp/include/rclcpp/clock.hpp b/rclcpp/include/rclcpp/clock.hpp index 84ddcd1c1c..fa35046a41 100644 --- a/rclcpp/include/rclcpp/clock.hpp +++ b/rclcpp/include/rclcpp/clock.hpp @@ -99,6 +99,10 @@ class Clock bool ros_time_is_active(); + RCLCPP_PUBLIC + rcl_clock_t * + get_clock_handle(); + RCLCPP_PUBLIC rcl_clock_type_t get_clock_type(); diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index d79e79cea1..0f9852c2f5 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -23,6 +23,7 @@ #include #include +#include "rclcpp/clock.hpp" #include "rclcpp/function_traits.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/rate.hpp" @@ -44,7 +45,7 @@ class TimerBase RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(TimerBase) RCLCPP_PUBLIC - explicit TimerBase(std::chrono::nanoseconds period); + explicit TimerBase(Clock::SharedPtr clock, std::chrono::nanoseconds period); RCLCPP_PUBLIC ~TimerBase(); @@ -85,6 +86,7 @@ class TimerBase bool is_ready(); protected: + Clock::SharedPtr clock_; std::shared_ptr timer_handle_; }; @@ -92,14 +94,12 @@ class TimerBase using VoidCallbackType = std::function; using TimerCallbackType = std::function; -/// Generic timer templated on the clock type. Periodically executes a user-specified callback. +/// Generic timer. Periodically executes a user-specified callback. template< typename FunctorT, - class Clock, typename std::enable_if< - (rclcpp::function_traits::same_arguments::value || - rclcpp::function_traits::same_arguments::value) && - Clock::is_steady + rclcpp::function_traits::same_arguments::value || + rclcpp::function_traits::same_arguments::value >::type * = nullptr > class GenericTimer : public TimerBase @@ -109,11 +109,14 @@ class GenericTimer : public TimerBase /// Default constructor. /** + * \param[in] clock The clock providing the current time. * \param[in] period The interval at which the timer fires. * \param[in] callback User-specified callback function. */ - GenericTimer(std::chrono::nanoseconds period, FunctorT && callback) - : TimerBase(period), callback_(std::forward(callback)) + explicit GenericTimer( + Clock::SharedPtr clock, std::chrono::nanoseconds period, FunctorT && callback + ) + : TimerBase(clock, period), callback_(std::forward(callback)) { } @@ -165,7 +168,7 @@ class GenericTimer : public TimerBase virtual bool is_steady() { - return Clock::is_steady; + return clock_->get_clock_type() == RCL_STEADY_TIME; } protected: @@ -174,8 +177,26 @@ class GenericTimer : public TimerBase FunctorT callback_; }; -template -using WallTimer = GenericTimer; +template< + typename FunctorT, + typename std::enable_if< + rclcpp::function_traits::same_arguments::value || + rclcpp::function_traits::same_arguments::value + >::type * = nullptr +> +class WallTimer : public GenericTimer +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(WallTimer) + + explicit WallTimer(std::chrono::nanoseconds period, FunctorT && callback) + : GenericTimer( + std::make_shared(RCL_STEADY_TIME), period, std::move(callback)) + {} + +protected: + RCLCPP_DISABLE_COPY(WallTimer) +}; } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/clock.cpp b/rclcpp/src/rclcpp/clock.cpp index 64ae080c66..df38ec0219 100644 --- a/rclcpp/src/rclcpp/clock.cpp +++ b/rclcpp/src/rclcpp/clock.cpp @@ -77,7 +77,7 @@ Clock::now() { Time now(0, 0, rcl_clock_.type); - auto ret = rcl_clock_get_now(&rcl_clock_, &now.rcl_time_); + auto ret = rcl_clock_get_now(&rcl_clock_, &now.rcl_time_.nanoseconds); if (ret != RCL_RET_OK) { rclcpp::exceptions::throw_from_rcl_error( ret, "could not get current time stamp"); @@ -103,6 +103,12 @@ Clock::ros_time_is_active() return is_enabled; } +rcl_clock_t * +Clock::get_clock_handle() +{ + return &rcl_clock_; +} + rcl_clock_type_t Clock::get_clock_type() { diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 50714f3b0f..bfd0ea2cda 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -65,7 +65,7 @@ void TimeSource::attachNode( node_services_ = node_services_interface; // TODO(tfoote): Update QOS - const std::string & topic_name = "/clock"; + const std::string topic_name = "/clock"; rclcpp::callback_group::CallbackGroup::SharedPtr group; using rclcpp::message_memory_strategy::MessageMemoryStrategy; diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index 138e19d42f..b38e2fe5e8 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -22,8 +22,10 @@ using rclcpp::TimerBase; -TimerBase::TimerBase(std::chrono::nanoseconds period) +TimerBase::TimerBase(rclcpp::Clock::SharedPtr clock, std::chrono::nanoseconds period) { + clock_ = clock; + timer_handle_ = std::shared_ptr( new rcl_timer_t, [ = ](rcl_timer_t * timer) { @@ -38,8 +40,9 @@ TimerBase::TimerBase(std::chrono::nanoseconds period) *timer_handle_.get() = rcl_get_zero_initialized_timer(); + rcl_clock_t * clock_handle = clock_->get_clock_handle(); if (rcl_timer_init( - timer_handle_.get(), period.count(), nullptr, + timer_handle_.get(), clock_handle, period.count(), nullptr, rcl_get_default_allocator()) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED(