From 6d79b5e0e3ab31e10b4d1bd9580fb1e503b3c7f2 Mon Sep 17 00:00:00 2001
From: Tully Foote <tfoote@osrfoundation.org>
Date: Tue, 15 Dec 2015 00:27:46 -0800
Subject: [PATCH] adding basic unit tests for rate

---
 rclcpp/CMakeLists.txt          | 11 +++++
 rclcpp/include/rclcpp/rate.hpp |  7 ++--
 rclcpp/test/test_rate.cpp      | 76 ++++++++++++++++++++++++++++++++++
 3 files changed, 91 insertions(+), 3 deletions(-)
 create mode 100644 rclcpp/test/test_rate.cpp

diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt
index 11207cb..d1ca78e 100644
--- a/rclcpp/CMakeLists.txt
+++ b/rclcpp/CMakeLists.txt
@@ -102,6 +102,17 @@ if(AMENT_ENABLE_TESTING)
       ${rosidl_generator_cpp_INCLUDE_DIRS}
     )
   endif()
+  ament_add_gtest(test_rate test/test_rate.cpp)
+  if(TARGET test_rate)
+    target_include_directories(test_rate PUBLIC
+      ${rcl_interfaces_INCLUDE_DIRS}
+      ${rmw_INCLUDE_DIRS}
+      ${rosidl_generator_cpp_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_rate
+      ${PROJECT_NAME}${target_suffix}
+    )
+  endif()
 endif()
 
 ament_package(
diff --git a/rclcpp/include/rclcpp/rate.hpp b/rclcpp/include/rclcpp/rate.hpp
index 4f25634..713b4a3 100644
--- a/rclcpp/include/rclcpp/rate.hpp
+++ b/rclcpp/include/rclcpp/rate.hpp
@@ -34,7 +34,7 @@ public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(RateBase);
 
   virtual bool sleep() = 0;
-  virtual bool is_steady() = 0;
+  virtual bool is_steady() const = 0;
   virtual void reset() = 0;
 };
 
@@ -89,7 +89,7 @@ public:
   }
 
   virtual bool
-  is_steady()
+  is_steady() const
   {
     return Clock::is_steady;
   }
@@ -109,7 +109,8 @@ private:
   RCLCPP_DISABLE_COPY(GenericRate);
 
   std::chrono::nanoseconds period_;
-  std::chrono::time_point<Clock> last_interval_;
+  using ClockDurationNano = std::chrono::duration<typename Clock::rep, std::nano>;
+  std::chrono::time_point<Clock, ClockDurationNano> last_interval_;
 };
 
 using Rate = GenericRate<std::chrono::system_clock>;
diff --git a/rclcpp/test/test_rate.cpp b/rclcpp/test/test_rate.cpp
new file mode 100644
index 0000000..256b34c
--- /dev/null
+++ b/rclcpp/test/test_rate.cpp
@@ -0,0 +1,76 @@
+// Copyright 2015 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 <gtest/gtest.h>
+
+#include <string>
+
+#include "rclcpp/rate.hpp"
+
+/*
+   Tests that funcion_traits calculates arity of several functors.
+ */
+TEST(TestRate, rate_basics) {
+  auto period = std::chrono::milliseconds(10);
+  auto delta = std::chrono::milliseconds(1);
+
+  auto start = std::chrono::system_clock::now();
+  rclcpp::rate::Rate r(period);
+  ASSERT_FALSE(r.is_steady());
+  r.sleep();
+  auto one = std::chrono::system_clock::now();
+  ASSERT_TRUE(period - delta < one - start);
+  ASSERT_TRUE(period + delta > one - start);
+
+  rclcpp::utilities::sleep_for(delta * 4);
+  r.sleep();
+  auto two = std::chrono::system_clock::now();
+
+  ASSERT_TRUE(period - delta < two - one);
+  ASSERT_TRUE(period + delta > two - one);
+
+  rclcpp::utilities::sleep_for(delta * 4);
+  r.reset();
+  r.sleep();
+  auto three = std::chrono::system_clock::now();
+  ASSERT_TRUE(period + 3 * delta < three - two);
+  ASSERT_TRUE(period + 5 * delta > three - two);
+}
+
+TEST(TestRate, wallrate_basics) {
+  auto period = std::chrono::milliseconds(10);
+  auto delta = std::chrono::milliseconds(1);
+
+  auto start = std::chrono::system_clock::now();
+  rclcpp::rate::WallRate r(period);
+  ASSERT_TRUE(r.is_steady());
+  r.sleep();
+  auto one = std::chrono::system_clock::now();
+  ASSERT_TRUE(period - delta < one - start);
+  ASSERT_TRUE(period + delta > one - start);
+
+  rclcpp::utilities::sleep_for(delta * 4);
+  r.sleep();
+  auto two = std::chrono::system_clock::now();
+
+  ASSERT_TRUE(period - delta < two - one);
+  ASSERT_TRUE(period + delta > two - one);
+
+  rclcpp::utilities::sleep_for(delta * 4);
+  r.reset();
+  r.sleep();
+  auto three = std::chrono::system_clock::now();
+  ASSERT_TRUE(period + 3 * delta < three - two);
+  ASSERT_TRUE(period + 5 * delta > three - two);
+}