feat(ars548): updated the driver according to new documentation

nebula_common/include/nebula_common/util/rate_checker.hpp

@@ -0,0 +1,57 @@
+// Copyright 2025 TIER IV, 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.
+
+#pragma once
+
+#include "nebula_common/util/ring_buffer.hpp"
+
+namespace nebula::util
+{
+
+class RateChecker
+{
+public:
+  RateChecker(double min_rate, double max_rate, std::size_t buffer_size)
+  : min_rate_(min_rate), max_rate_(max_rate), ring_buffer_(buffer_size)
+  {
+  }
+
+  bool is_valid(double stamp)
+  {
+    if (last_stamp_ > 0.0) {
+      double current_time = stamp;
+      double dt = stamp - last_stamp_;
+      ring_buffer_.push_back(dt);
+    }
+
+    last_stamp_ = stamp;
+
+    if (!ring_buffer_.is_full()) {
+      return true;
+    }
+
+    double average = 1.0 / ring_buffer_.get_average();
+    return average >= min_rate_ && average <= max_rate_;
+  }
+
+  double get_average() const { return 1.0 / ring_buffer_.get_average(); }
+
+private:
+  double last_stamp_{0.0};
+  double min_rate_;
+  double max_rate_;
+  RingBuffer<double> ring_buffer_;
+};
+
+}  // namespace nebula::util

nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp

@@ -17,7 +17,7 @@
 #include "nebula_ros/common/parameter_descriptors.hpp"
 
 #include <nebula_common/continental/continental_ars548.hpp>
-#include <nebula_common/util/ring_buffer.hpp>
+#include <nebula_common/util/rate_checker.hpp>
 #include <nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp>
 #include <rclcpp/rclcpp.hpp>
 
@@ -115,12 +115,9 @@ class ContinentalARS548HwInterfaceWrapper
     acceleration_sub_{};
   rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr steering_angle_sub_{};
 
-  double last_odometry_stamp_{0.0};
-  double last_acceleration_stamp_{0.0};
-  double last_steering_angle_stamp_{0.0};
-  nebula::util::RingBuffer<double> odometry_ring_buffer_;
-  nebula::util::RingBuffer<double> acceleration_ring_buffer_;
-  nebula::util::RingBuffer<double> steering_angle_ring_buffer_;
+  nebula::util::RateChecker odometry_rate_checker_;
+  nebula::util::RateChecker acceleration_rate_checker_;
+  nebula::util::RateChecker steering_angle_rate_checker_;
 
   bool standstill_{true};
 

nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp

@@ -30,9 +30,15 @@ ContinentalARS548HwInterfaceWrapper::ContinentalARS548HwInterfaceWrapper(
   logger_(parent_node->get_logger().get_child("HwInterfaceWrapper")),
   status_(Status::NOT_INITIALIZED),
   config_ptr_(config_ptr),
-  odometry_ring_buffer_(100),
-  acceleration_ring_buffer_(100),
-  steering_angle_ring_buffer_(100)
+  odometry_rate_checker_(
+    nebula::drivers::continental_ars548::min_odometry_hz,
+    nebula::drivers::continental_ars548::max_odometry_hz, 100),
+  acceleration_rate_checker_(
+    nebula::drivers::continental_ars548::min_odometry_hz,
+    nebula::drivers::continental_ars548::max_odometry_hz, 100),
+  steering_angle_rate_checker_(
+    nebula::drivers::continental_ars548::min_odometry_hz,
+    nebula::drivers::continental_ars548::max_odometry_hz, 100)
 {
   hw_interface_->set_logger(
     std::make_shared<rclcpp::Logger>(parent_node->get_logger().get_child("HwInterface")));
@@ -128,20 +134,10 @@ void ContinentalARS548HwInterfaceWrapper::odometry_callback(
   using nebula::drivers::continental_ars548::max_odometry_hz;
   using nebula::drivers::continental_ars548::min_odometry_hz;
 
-  if (last_odometry_stamp_ == 0.0) {
-    last_odometry_stamp_ = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9;
-  } else {
-    double current_time = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9;
-    double dt = current_time - last_odometry_stamp_;
-    last_odometry_stamp_ = current_time;
-    odometry_ring_buffer_.push_back(dt);
-  }
-
-  double estimated_hz = 1.0 / odometry_ring_buffer_.get_average();
+  double stamp = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9;
 
-  if (
-    odometry_ring_buffer_.is_full() && (estimated_hz < static_cast<double>(min_odometry_hz) ||
-                                        estimated_hz > static_cast<double>(max_odometry_hz))) {
+  if (!odometry_rate_checker_.is_valid(stamp)) {
+    double estimated_hz = odometry_rate_checker_.get_average();
     rclcpp::Clock clock{RCL_ROS_TIME};
     RCLCPP_WARN_THROTTLE(
       logger_, clock, 5000,
@@ -178,20 +174,10 @@ void ContinentalARS548HwInterfaceWrapper::acceleration_callback(
   using nebula::drivers::continental_ars548::max_odometry_hz;
   using nebula::drivers::continental_ars548::min_odometry_hz;
 
-  if (last_acceleration_stamp_ == 0.0) {
-    last_acceleration_stamp_ = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9;
-  } else {
-    double current_time = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9;
-    double dt = current_time - last_acceleration_stamp_;
-    last_acceleration_stamp_ = current_time;
-    acceleration_ring_buffer_.push_back(dt);
-  }
-
-  double estimated_hz = 1.0 / acceleration_ring_buffer_.get_average();
+  double stamp = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9;
 
-  if (
-    acceleration_ring_buffer_.is_full() && (estimated_hz < static_cast<double>(min_odometry_hz) ||
-                                            estimated_hz > static_cast<double>(max_odometry_hz))) {
+  if (!acceleration_rate_checker_.is_valid(stamp)) {
+    double estimated_hz = acceleration_rate_checker_.get_average();
     rclcpp::Clock clock{RCL_ROS_TIME};
     RCLCPP_WARN_THROTTLE(
       logger_, clock, 5000,
@@ -211,23 +197,11 @@ void ContinentalARS548HwInterfaceWrapper::steering_angle_callback(
   using nebula::drivers::continental_ars548::min_odometry_hz;
 
   const auto now = std::chrono::high_resolution_clock::now();
-  const auto stamp =
+  const double stamp =
     1e-9 * std::chrono::duration_cast<std::chrono::nanoseconds>(now.time_since_epoch()).count();
 
-  if (last_steering_angle_stamp_ == 0.0) {
-    last_steering_angle_stamp_ = stamp;
-  } else {
-    double dt = stamp - last_steering_angle_stamp_;
-    last_steering_angle_stamp_ = stamp;
-    steering_angle_ring_buffer_.push_back(dt);
-  }
-
-  double estimated_hz = 1.0 / steering_angle_ring_buffer_.get_average();
-
-  if (
-    steering_angle_ring_buffer_.is_full() &&
-    (estimated_hz < static_cast<double>(min_odometry_hz) ||
-     estimated_hz > static_cast<double>(max_odometry_hz))) {
+  if (!steering_angle_rate_checker_.is_valid(stamp)) {
+    double estimated_hz = steering_angle_rate_checker_.get_average();
     rclcpp::Clock clock{RCL_ROS_TIME};
     RCLCPP_WARN_THROTTLE(
       logger_, clock, 5000,