throttle warn

nav2_collision_monitor/src/collision_detector_node.cpp

@@ -322,14 +322,19 @@ void CollisionDetector::process()
   std::unique_ptr<nav2_msgs::msg::CollisionDetectorState> state_msg =
     std::make_unique<nav2_msgs::msg::CollisionDetectorState>();
 
+  // Get clock for throttled logging
+  rclcpp::Clock::SharedPtr clk = get_clock();
+
   // Fill collision_points array from different data sources
   for (std::shared_ptr<Source> source : sources_) {
     if (source->getEnabled()) {
       if (!source->getData(curr_time, collision_points) &&
         source->getSourceTimeout().seconds() != 0.0)
       {
-        RCLCPP_WARN(
+        RCLCPP_WARN_THROTTLE(
           get_logger(),
+          *clk,
+          5000,  // 5 seconds
           "Invalid source %s detected."
           " Either due to data not published yet, or to lack of new data received within the"
           " sensor timeout, or if impossible to transform data to base frame",

nav2_collision_monitor/src/source.cpp

@@ -82,12 +82,20 @@ bool Source::sourceValid(
   const rclcpp::Time & source_time,
   const rclcpp::Time & curr_time) const
 {
+  // Get clock for throttled logging
+  auto node = node_.lock();
+  rclcpp::Clock::SharedPtr clk = node->get_clock();
+
   // Source is considered as not valid, if latest received data timestamp is earlier
   // than current time by source_timeout_ interval
   const rclcpp::Duration dt = curr_time - source_time;
   if (source_timeout_.seconds() != 0.0 && dt > source_timeout_) {
-    RCLCPP_WARN(
+    // Log calls are being ignored if the last logged message
+    // is not longer ago than the specified duration
+    RCLCPP_WARN_THROTTLE(
       logger_,
+      *clk,
+      5000,  // 5 seconds
       "[%s]: Latest source and current collision monitor node timestamps differ on %f seconds. "
       "Ignoring the source.",
       source_name_.c_str(), dt.seconds());