Merge pull request #182 from ros2/rate_tests

Add unit tests for rclcpp Rate objects

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(

rclcpp/include/rclcpp/rate.hpp

@@ -34,7 +34,7 @@ class RateBase
   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 @@ class GenericRate : public RateBase
   }
 
   virtual bool
-  is_steady()
+  is_steady() const
   {
     return Clock::is_steady;
   }
@@ -109,7 +109,8 @@ class GenericRate : public RateBase
   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>;

rclcpp/test/test_rate.cpp

@@ -0,0 +1,100 @@
+// 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"
+
+/*
+   Basic tests for the Rate and WallRate clases.
+ */
+TEST(TestRate, rate_basics) {
+  auto period = std::chrono::milliseconds(100);
+  auto offset = std::chrono::milliseconds(50);
+  auto epsilon = std::chrono::milliseconds(1);
+  double overrun_ratio = 1.5;
+
+  auto start = std::chrono::system_clock::now();
+  rclcpp::rate::Rate r(period);
+  ASSERT_FALSE(r.is_steady());
+  ASSERT_TRUE(r.sleep());
+  auto one = std::chrono::system_clock::now();
+  auto delta = one - start;
+  ASSERT_TRUE(period < delta);
+  ASSERT_TRUE(period * overrun_ratio > delta);
+
+  rclcpp::utilities::sleep_for(offset);
+  ASSERT_TRUE(r.sleep());
+  auto two = std::chrono::system_clock::now();
+  delta = two - one;
+  ASSERT_TRUE(period < delta + epsilon);
+  ASSERT_TRUE(period * overrun_ratio > delta);
+
+  rclcpp::utilities::sleep_for(offset);
+  auto two_offset = std::chrono::system_clock::now();
+  r.reset();
+  ASSERT_TRUE(r.sleep());
+  auto three = std::chrono::system_clock::now();
+  delta = three - two_offset;
+  ASSERT_TRUE(period < delta);
+  ASSERT_TRUE(period * overrun_ratio > delta);
+
+  rclcpp::utilities::sleep_for(offset + period);
+  auto four = std::chrono::system_clock::now();
+  ASSERT_FALSE(r.sleep());
+  auto five = std::chrono::system_clock::now();
+  delta = five - four;
+  ASSERT_TRUE(epsilon > delta);
+}
+
+TEST(TestRate, wall_rate_basics) {
+  auto period = std::chrono::milliseconds(100);
+  auto offset = std::chrono::milliseconds(50);
+  auto epsilon = std::chrono::milliseconds(1);
+  double overrun_ratio = 1.5;
+
+  auto start = std::chrono::system_clock::now();
+  rclcpp::rate::WallRate r(period);
+  ASSERT_TRUE(r.is_steady());
+  ASSERT_TRUE(r.sleep());
+  auto one = std::chrono::system_clock::now();
+  auto delta = one - start;
+  ASSERT_TRUE(period < delta);
+  ASSERT_TRUE(period * overrun_ratio > delta);
+
+  rclcpp::utilities::sleep_for(offset);
+  ASSERT_TRUE(r.sleep());
+  auto two = std::chrono::system_clock::now();
+  delta = two - one;
+  ASSERT_TRUE(period < delta + epsilon);
+  ASSERT_TRUE(period * overrun_ratio > delta);
+
+  rclcpp::utilities::sleep_for(offset);
+  auto two_offset = std::chrono::system_clock::now();
+  r.reset();
+  ASSERT_TRUE(r.sleep());
+  auto three = std::chrono::system_clock::now();
+  delta = three - two_offset;
+  ASSERT_TRUE(period < delta);
+  ASSERT_TRUE(period * overrun_ratio > delta);
+
+  rclcpp::utilities::sleep_for(offset + period);
+  auto four = std::chrono::system_clock::now();
+  ASSERT_FALSE(r.sleep());
+  auto five = std::chrono::system_clock::now();
+  delta = five - four;
+  ASSERT_TRUE(epsilon > delta);
+}