Enable throttling logs (#879)

* Enable throttling logs

Signed-off-by: Brian Ezequiel Marchi <brian.marchi65@gmail.com>

* Change time source macro parameter generation

Signed-off-by: Brian Ezequiel Marchi <brian.marchi65@gmail.com>

* Modify only throttle parameters for logging

Signed-off-by: Brian Ezequiel Marchi <brian.marchi65@gmail.com>

rclcpp/include/rclcpp/clock.hpp

@@ -22,6 +22,8 @@
 #include "rclcpp/visibility_control.hpp"
 
 #include "rcl/time.h"
+#include "rcutils/time.h"
+#include "rcutils/types/rcutils_ret.h"
 
 namespace rclcpp
 {

rclcpp/resource/logging.hpp.em

@@ -45,13 +45,20 @@
 #endif
 
 @{
+from collections import OrderedDict
 from rcutils.logging import feature_combinations
 from rcutils.logging import get_macro_parameters
 from rcutils.logging import get_suffix_from_features
 from rcutils.logging import severities
+from rcutils.logging import throttle_args
+from rcutils.logging import throttle_params
 
-# TODO(dhood): Implement the throttle macro using time sources available in rclcpp
-excluded_features = ['named', 'throttle']
+throttle_args['condition_before'] = 'RCUTILS_LOG_CONDITION_THROTTLE_BEFORE(clock, duration)'
+del throttle_params['get_time_point_value']
+throttle_params['clock'] = 'rclcpp::Clock that will be used to get the time point.'
+throttle_params.move_to_end('clock', last=False)
+
+excluded_features = ['named']
 def is_supported_feature_combination(feature_combination):
     is_excluded = any([ef in feature_combination for ef in excluded_features])
     return not is_excluded
@@ -92,14 +99,27 @@ def is_supported_feature_combination(feature_combination):
  * \param ... The format string, followed by the variable arguments for the format string.
  * It also accepts a single argument of type std::string.
  */
-#define RCLCPP_@(severity)@(suffix)(logger, @(''.join([p + ', ' for p in get_macro_parameters(feature_combination).keys()]))...) \
+@{params = get_macro_parameters(feature_combination).keys()}@
+#define RCLCPP_@(severity)@(suffix)(logger, @(''.join([p + ', ' for p in params]))...) \
   do { \
     static_assert( \
       ::std::is_same<typename std::remove_cv<typename std::remove_reference<decltype(logger)>::type>::type, \
       typename ::rclcpp::Logger>::value, \
       "First argument to logging macros must be an rclcpp::Logger"); \
+@[ if 'throttle' in feature_combination]@ \
+    auto get_time_point = [&clock](rcutils_time_point_value_t * time_point) -> rcutils_ret_t { \
+      try { \
+        *time_point = clock.now().nanoseconds(); \
+      } catch (...) { \
+        RCUTILS_SAFE_FWRITE_TO_STDERR( \
+        "[rclcpp|logging.hpp] RCLCPP_@(severity)@(suffix) could not get current time stamp\n"); \
+        return RCUTILS_RET_ERROR; \
+      } \
+        return RCUTILS_RET_OK; \
+    }; \
+@[ end if] \
     RCUTILS_LOG_@(severity)@(suffix)_NAMED( \
-@{params = get_macro_parameters(feature_combination).keys()}@
+@{params = ['get_time_point' if p == 'clock' and 'throttle' in feature_combination else p for p in params]}@
 @[ if params]@
 @(''.join(['      ' + p + ', \\\n' for p in params]))@
 @[ end if]@

rclcpp/test/test_logging.cpp

@@ -19,6 +19,7 @@
 #include <thread>
 #include <vector>
 
+#include "rclcpp/clock.hpp"
 #include "rclcpp/logger.hpp"
 #include "rclcpp/logging.hpp"
 #include "rcutils/logging.h"
@@ -85,7 +86,7 @@ TEST_F(TestLoggingMacros, test_logging_named) {
   if (g_last_log_event.location) {
     EXPECT_STREQ("TestBody", g_last_log_event.location->function_name);
     EXPECT_THAT(g_last_log_event.location->file_name, EndsWith("test_logging.cpp"));
-    EXPECT_EQ(81u, g_last_log_event.location->line_number);
+    EXPECT_EQ(82u, g_last_log_event.location->line_number);
   }
   EXPECT_EQ(RCUTILS_LOG_SEVERITY_DEBUG, g_last_log_event.level);
   EXPECT_EQ("name", g_last_log_event.name);
@@ -163,6 +164,40 @@ TEST_F(TestLoggingMacros, test_logging_skipfirst) {
   }
 }
 
+TEST_F(TestLoggingMacros, test_throttle) {
+  using namespace std::chrono_literals;
+  rclcpp::Clock steady_clock(RCL_STEADY_TIME);
+  for (uint64_t i = 0; i < 3; ++i) {
+    RCLCPP_DEBUG_THROTTLE(g_logger, steady_clock, 10000, "Throttling");
+  }
+  EXPECT_EQ(1u, g_log_calls);
+  RCLCPP_DEBUG_SKIPFIRST_THROTTLE(g_logger, steady_clock, 1, "Skip first throttling");
+  EXPECT_EQ(1u, g_log_calls);
+  for (uint64_t i = 0; i < 6; ++i) {
+    RCLCPP_DEBUG_THROTTLE(g_logger, steady_clock, 10, "Throttling");
+    RCLCPP_DEBUG_SKIPFIRST_THROTTLE(g_logger, steady_clock, 40, "Throttling");
+    std::this_thread::sleep_for(5ms);
+  }
+  EXPECT_EQ(4u, g_log_calls);
+  rclcpp::Clock ros_clock(RCL_ROS_TIME);
+  ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(ros_clock.get_clock_handle()));
+  RCLCPP_DEBUG_THROTTLE(g_logger, ros_clock, 10000, "Throttling");
+  rcl_clock_t * clock = ros_clock.get_clock_handle();
+  ASSERT_TRUE(clock);
+  EXPECT_EQ(4u, g_log_calls);
+  EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock, RCUTILS_MS_TO_NS(10)));
+  for (uint64_t i = 0; i < 2; ++i) {
+    RCLCPP_DEBUG_THROTTLE(g_logger, ros_clock, 10, "Throttling");
+    if (i == 0) {
+      EXPECT_EQ(5u, g_log_calls);
+      rcl_time_point_value_t clock_ns = ros_clock.now().nanoseconds() + RCUTILS_MS_TO_NS(10);
+      EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock, clock_ns));
+    } else {
+      EXPECT_EQ(6u, g_log_calls);
+    }
+  }
+}
+
 bool log_function(rclcpp::Logger logger)
 {
   RCLCPP_INFO(logger, "successful log");