Make Rate to select the clock to work with

rclcpp/CMakeLists.txt

@@ -107,6 +107,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/qos.cpp
   src/rclcpp/event_handler.cpp
   src/rclcpp/qos_overriding_options.cpp
+  src/rclcpp/rate.cpp
   src/rclcpp/serialization.cpp
   src/rclcpp/serialized_message.cpp
   src/rclcpp/service.cpp

rclcpp/include/rclcpp/rate.hpp

@@ -19,6 +19,8 @@
 #include <memory>
 #include <thread>
 
+#include "rclcpp/clock.hpp"
+#include "rclcpp/duration.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -31,9 +33,20 @@ class RateBase
 public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(RateBase)
 
+  RCLCPP_PUBLIC
   virtual ~RateBase() {}
+
+  RCLCPP_PUBLIC
   virtual bool sleep() = 0;
+
+  [[deprecated("use get_type() instead")]]
+  RCLCPP_PUBLIC
   virtual bool is_steady() const = 0;
+
+  RCLCPP_PUBLIC
+  virtual rcl_clock_type_t get_type() const = 0;
+
+  RCLCPP_PUBLIC
   virtual void reset() = 0;
 };
 
@@ -42,14 +55,13 @@ using std::chrono::duration_cast;
 using std::chrono::nanoseconds;
 
 template<class Clock = std::chrono::high_resolution_clock>
-class GenericRate : public RateBase
+class [[deprecated("use rclcpp::Rate class instead of GenericRate")]] GenericRate : public RateBase
 {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(GenericRate)
 
   explicit GenericRate(double rate)
-  : GenericRate<Clock>(
-      duration_cast<nanoseconds>(duration<double>(1.0 / rate)))
+  : period_(duration_cast<nanoseconds>(duration<double>(1.0 / rate))), last_interval_(Clock::now())
   {}
   explicit GenericRate(std::chrono::nanoseconds period)
   : period_(period), last_interval_(Clock::now())
@@ -87,12 +99,18 @@ class GenericRate : public RateBase
     return true;
   }
 
+  [[deprecated("use get_type() instead")]]
   virtual bool
   is_steady() const
   {
     return Clock::is_steady;
   }
 
+  virtual rcl_clock_type_t get_type() const
+  {
+    return Clock::is_steady ? RCL_STEADY_TIME : RCL_SYSTEM_TIME;
+  }
+
   virtual void
   reset()
   {
@@ -112,8 +130,69 @@ class GenericRate : public RateBase
   std::chrono::time_point<Clock, ClockDurationNano> last_interval_;
 };
 
-using Rate = GenericRate<std::chrono::system_clock>;
-using WallRate = GenericRate<std::chrono::steady_clock>;
+class Rate : public RateBase
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(Rate)
+
+  RCLCPP_PUBLIC
+  explicit Rate(
+    const double rate,
+    Clock::SharedPtr clock = std::make_shared<Clock>(RCL_SYSTEM_TIME));
+
+  RCLCPP_PUBLIC
+  explicit Rate(
+    const Duration & period,
+    Clock::SharedPtr clock = std::make_shared<Clock>(RCL_SYSTEM_TIME));
+
+  RCLCPP_PUBLIC
+  virtual bool
+  sleep();
+
+  [[deprecated("use get_type() instead")]]
+  RCLCPP_PUBLIC
+  virtual bool
+  is_steady() const;
+
+  RCLCPP_PUBLIC
+  virtual rcl_clock_type_t
+  get_type() const;
+
+  RCLCPP_PUBLIC
+  virtual void
+  reset();
+
+  RCLCPP_PUBLIC
+  Duration
+  period() const;
+
+private:
+  RCLCPP_DISABLE_COPY(Rate)
+
+  Clock::SharedPtr clock_;
+  Duration period_;
+  Time last_interval_;
+};
+
+class WallRate : public Rate
+{
+public:
+  RCLCPP_PUBLIC
+  explicit WallRate(const double rate);
+
+  RCLCPP_PUBLIC
+  explicit WallRate(const Duration & period);
+};
+
+class ROSRate : public Rate
+{
+public:
+  RCLCPP_PUBLIC
+  explicit ROSRate(const double rate);
+
+  RCLCPP_PUBLIC
+  explicit ROSRate(const Duration & period);
+};
 
 }  // namespace rclcpp
 

rclcpp/src/rclcpp/rate.cpp

@@ -0,0 +1,112 @@
+// Copyright 2023 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/rate.hpp"
+
+#include <stdexcept>
+
+namespace rclcpp
+{
+
+Rate::Rate(
+  const double rate, Clock::SharedPtr clock)
+: clock_(clock), period_(0, 0), last_interval_(clock_->now())
+{
+  if (rate <= 0.0) {
+    throw std::invalid_argument{"rate must be greater than 0"};
+  }
+  period_ = Duration::from_seconds(1.0 / rate);
+}
+
+Rate::Rate(
+  const Duration & period, Clock::SharedPtr clock)
+: clock_(clock), period_(period), last_interval_(clock_->now())
+{
+  if (period <= Duration(0, 0)) {
+    throw std::invalid_argument{"period must be greater than 0"};
+  }
+}
+
+bool
+Rate::sleep()
+{
+  // Time coming into sleep
+  auto now = clock_->now();
+  // Time of next interval
+  auto next_interval = last_interval_ + period_;
+  // Detect backwards time flow
+  if (now < last_interval_) {
+    // Best thing to do is to set the next_interval to now + period
+    next_interval = now + period_;
+  }
+  // Update the interval
+  last_interval_ += period_;
+  // If the time_to_sleep is negative or zero, don't sleep
+  if (next_interval <= now) {
+    // If an entire cycle was missed then reset next interval.
+    // This might happen if the loop took more than a cycle.
+    // Or if time jumps forward.
+    if (now > next_interval + period_) {
+      last_interval_ = now + period_;
+    }
+    // Either way do not sleep and return false
+    return false;
+  }
+  // Calculate the time to sleep
+  auto time_to_sleep = next_interval - now;
+  // Sleep (will get interrupted by ctrl-c, may not sleep full time)
+  return clock_->sleep_for(time_to_sleep);
+}
+
+bool
+Rate::is_steady() const
+{
+  return clock_->get_clock_type() == RCL_STEADY_TIME;
+}
+
+rcl_clock_type_t
+Rate::get_type() const
+{
+  return clock_->get_clock_type();
+}
+
+void
+Rate::reset()
+{
+  last_interval_ = clock_->now();
+}
+
+Duration
+Rate::period() const
+{
+  return period_;
+}
+
+WallRate::WallRate(const double rate)
+: Rate(rate, std::make_shared<Clock>(RCL_STEADY_TIME))
+{}
+
+WallRate::WallRate(const Duration & period)
+: Rate(period, std::make_shared<Clock>(RCL_STEADY_TIME))
+{}
+
+ROSRate::ROSRate(const double rate)
+: Rate(rate, std::make_shared<Clock>(RCL_ROS_TIME))
+{}
+
+ROSRate::ROSRate(const Duration & period)
+: Rate(period, std::make_shared<Clock>(RCL_ROS_TIME))
+{}
+
+}  // namespace rclcpp