feat: updated the ars548 driver according to new received documentation

Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]>

nebula_common/include/nebula_common/continental/continental_ars548.hpp

@@ -253,10 +253,18 @@ constexpr int sync_lost = 3;
 constexpr int plug_right = 0;
 constexpr int plug_left = 1;
 
+constexpr int maximum_distance_min_value = 93;
+constexpr int maximum_distance_max_value = 1514;
+
 constexpr int frequency_slot_low = 0;
 constexpr int frequency_slot_mid = 1;
 constexpr int frequency_slot_high = 2;
 
+constexpr int min_cycle_time_ms = 50;
+constexpr int max_cycle_time_ms = 100;
+
+constexpr int min_time_slot_ms = 10;
+
 constexpr int hcc_worldwide = 1;
 constexpr int hcc_japan = 2;
 

nebula_common/include/nebula_common/util/ring_buffer.hpp

@@ -0,0 +1,72 @@
+// 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 <algorithm>
+#include <stdexcept>
+#include <vector>
+
+namespace nebula::util
+{
+
+template <typename T>
+class RingBuffer
+{
+public:
+  explicit RingBuffer(std::size_t capacity) : capacity_(capacity) { buffer_.resize(capacity_); }
+
+  void push_back(const T & value)
+  {
+    if (is_full()) {
+      sum_ -= buffer_[index_];
+    }
+
+    sum_ += value;
+    buffer_[index_] = value;
+    index_ = (index_ + 1) % capacity_;
+    size_ = std::min(size_ + 1, capacity_);
+  }
+
+  T & operator[](std::size_t index)
+  {
+    if (index >= size_) {
+      throw std::out_of_range("Index out of range");
+    }
+    return buffer_[(index_ + index) % size_];
+  }
+
+  const T & operator[](std::size_t index) const
+  {
+    if (index >= size_) {
+      throw std::out_of_range("Index out of range");
+    }
+    return buffer_[(index_ + index) % size_];
+  }
+
+  std::size_t size() const { return size_; }
+
+  bool is_full() const { return size_ == capacity_; }
+
+  T get_average() const { return sum_ / size_; }
+
+private:
+  T sum_{};
+  std::vector<T> buffer_;
+  std::size_t capacity_;
+  std::size_t size_{0};
+  std::size_t index_{0};
+};
+
+}  // namespace nebula::util

nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp

@@ -483,7 +483,7 @@ bool ContinentalARS548Decoder::parse_sensor_status_packet(
       radar_status_.hcc = "1:Worldwide";
       break;
     case hcc_japan:
-      radar_status_.hcc = "1:Japan";
+      radar_status_.hcc = "2:Japan";
       break;
     default:
       radar_status_.hcc = std::to_string(sensor_status_packet.status.hcc) + ":Invalid hcc";

nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp

@@ -87,8 +87,8 @@ class ContinentalARS548HwInterface
   /// @param power_save_standstill Desired power_save_standstill value (0 = Off, 1 = On)
   /// @return Resulting status
   Status set_radar_parameters(
-    uint16_t maximum_distance, uint8_t frequency_slot, uint8_t cycle_time, uint8_t time_slot,
-    uint8_t hcc, uint8_t power_save_standstill);
+    uint16_t maximum_distance, std::string frequency_band, uint8_t cycle_time, uint8_t time_slot,
+    std::string country_code, uint8_t power_save_standstill);
 
   /// @brief Set the sensor ip address
   /// @param sensor_ip_address Desired sensor ip address

nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp

@@ -185,14 +185,46 @@ Status ContinentalARS548HwInterface::set_vehicle_parameters(
 }
 
 Status ContinentalARS548HwInterface::set_radar_parameters(
-  uint16_t maximum_distance, uint8_t frequency_slot, uint8_t cycle_time, uint8_t time_slot,
-  uint8_t hcc, uint8_t power_save_standstill)
+  uint16_t maximum_distance, std::string frequency_band, uint8_t cycle_time_ms,
+  uint8_t time_slot_ms, std::string country_code, uint8_t power_save_standstill)
 {
+  uint8_t frequency_slot = 0;
+  uint8_t hcc = 0;
+
+  if (frequency_band == "low") {
+    frequency_slot = frequency_slot_low;
+  } else if (frequency_band == "mid") {
+    frequency_slot = frequency_slot_mid;
+  } else if (frequency_band == "high") {
+    frequency_slot = frequency_slot_high;
+  } else {
+    print_error("Invalid frequency_band value");
+    return Status::SENSOR_CONFIG_ERROR;
+  }
+
+  if (country_code == "worldwide") {
+    hcc = hcc_worldwide;
+  } else if (country_code == "japan") {
+    hcc = hcc_japan;
+  } else {
+    print_error("Invalid country_code value");
+    return Status::SENSOR_CONFIG_ERROR;
+  }
+
   if (
-    maximum_distance < 93 || maximum_distance > 1514 || frequency_slot > 2 || cycle_time < 50 ||
-    cycle_time > 100 || time_slot < 10 || time_slot > 90 || hcc < 1 || hcc > 2 ||
-    power_save_standstill > 1) {
-    print_error("Invalid SetRadarParameters values");
+    maximum_distance < maximum_distance_min_value ||
+    maximum_distance > maximum_distance_max_value) {
+    print_error("Invalid maximum_distance value");
+    return Status::SENSOR_CONFIG_ERROR;
+  }
+
+  if (cycle_time_ms < min_cycle_time_ms || cycle_time_ms > max_cycle_time_ms) {
+    print_error("Invalid cycle_time_ms value");
+    return Status::SENSOR_CONFIG_ERROR;
+  }
+
+  if (time_slot_ms < min_time_slot_ms || time_slot_ms > cycle_time_ms - 1) {
+    print_error("Invalid time_slot_ms value");
     return Status::SENSOR_CONFIG_ERROR;
   }
 
@@ -203,8 +235,8 @@ Status ContinentalARS548HwInterface::set_radar_parameters(
   configuration_packet.header.length = configuration_payload_length;
   configuration_packet.configuration.maximum_distance = maximum_distance;
   configuration_packet.configuration.frequency_slot = frequency_slot;
-  configuration_packet.configuration.cycle_time = cycle_time;
-  configuration_packet.configuration.time_slot = time_slot;
+  configuration_packet.configuration.cycle_time = cycle_time_ms;
+  configuration_packet.configuration.time_slot = time_slot_ms;
   configuration_packet.configuration.hcc = hcc;
   configuration_packet.configuration.powersave_standstill = power_save_standstill;
   configuration_packet.new_radar_parameters = 1;
@@ -214,8 +246,8 @@ Status ContinentalARS548HwInterface::set_radar_parameters(
 
   print_info("maximum_distance = " + std::to_string(maximum_distance));
   print_info("frequency_slot = " + std::to_string(frequency_slot));
-  print_info("cycle_time = " + std::to_string(cycle_time));
-  print_info("time_slot = " + std::to_string(time_slot));
+  print_info("cycle_time = " + std::to_string(cycle_time_ms));
+  print_info("time_slot = " + std::to_string(time_slot_ms));
   print_info("hcc = " + std::to_string(hcc));
   print_info("power_save_standstill = " + std::to_string(power_save_standstill));
 

nebula_messages/continental_srvs/srv/ContinentalArs548SetRadarParameters.srv

@@ -1,8 +1,8 @@
 uint16 maximum_distance
-uint16 frequency_slot
-uint16 cycle_time
-uint16 time_slot
-uint16 country_code
+string frequency_band
+uint16 cycle_time_ms
+uint16 time_slot_ms
+string country_code
 uint16 powersave_standstill
 ---
 bool success

nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp

@@ -17,6 +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_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp>
 #include <rclcpp/rclcpp.hpp>
 
@@ -114,6 +115,13 @@ 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_;
+
   bool standstill_{true};
 
   rclcpp::Service<continental_srvs::srv::ContinentalArs548SetNetworkConfiguration>::SharedPtr

nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp

@@ -29,7 +29,10 @@ ContinentalARS548HwInterfaceWrapper::ContinentalARS548HwInterfaceWrapper(
     std::make_shared<nebula::drivers::continental_ars548::ContinentalARS548HwInterface>()),
   logger_(parent_node->get_logger().get_child("HwInterfaceWrapper")),
   status_(Status::NOT_INITIALIZED),
-  config_ptr_(config_ptr)
+  config_ptr_(config_ptr),
+  odometry_ring_buffer_(100),
+  acceleration_ring_buffer_(100),
+  steering_angle_ring_buffer_(100)
 {
   hw_interface_->set_logger(
     std::make_shared<rclcpp::Logger>(parent_node->get_logger().get_child("HwInterface")));
@@ -122,6 +125,30 @@ ContinentalARS548HwInterfaceWrapper::hw_interface() const
 void ContinentalARS548HwInterfaceWrapper::odometry_callback(
   const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg)
 {
+  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();
+  RCLCPP_ERROR(
+    logger_,
+    "The current odometry rate is %.2f Hz. The sensor requires a rate in the range of 10Hz to 50Hz",
+    estimated_hz);
+
+  if (odometry_ring_buffer_.is_full() && (estimated_hz < 10.0 || estimated_hz > 50.0)) {
+    rclcpp::Clock clock{RCL_ROS_TIME};
+    RCLCPP_WARN_THROTTLE(
+      logger_, clock, 5000,
+      "The current odometry rate is %.2f Hz. The sensor requires a rate in the range of 10Hz to "
+      "50Hz",
+      estimated_hz);
+  }
+
   constexpr float speed_to_standstill = 0.5f;
   constexpr float speed_to_moving = 2.f;
 
@@ -147,13 +174,66 @@ void ContinentalARS548HwInterfaceWrapper::odometry_callback(
 void ContinentalARS548HwInterfaceWrapper::acceleration_callback(
   const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg)
 {
+  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();
+  RCLCPP_ERROR(
+    logger_,
+    "The current acceleration rate is %.2f Hz. The sensor requires a rate in the range of 10Hz to "
+    "50Hz",
+    estimated_hz);
+
+  if (acceleration_ring_buffer_.is_full() && (estimated_hz < 10.0 || estimated_hz > 50.0)) {
+    rclcpp::Clock clock{RCL_ROS_TIME};
+    RCLCPP_WARN_THROTTLE(
+      logger_, clock, 5000,
+      "Current acceleration rate is %.2f Hz. The sensor requires a rate in the range of 10Hz to "
+      "50Hz",
+      estimated_hz);
+  }
+
   hw_interface_->set_acceleration_lateral_cog(msg->accel.accel.linear.y);
   hw_interface_->set_acceleration_longitudinal_cog(msg->accel.accel.linear.x);
 }
 
 void ContinentalARS548HwInterfaceWrapper::steering_angle_callback(
   const std_msgs::msg::Float32::SharedPtr msg)
 {
+  const auto now = std::chrono::high_resolution_clock::now();
+  const auto 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();
+  RCLCPP_ERROR(
+    logger_,
+    "The current steering angle rate is %.2f Hz. The sensor requires a rate in the range of 10Hz "
+    "to 50Hz",
+    estimated_hz);
+
+  if (steering_angle_ring_buffer_.is_full() && (estimated_hz < 10.0 || estimated_hz > 50.0)) {
+    rclcpp::Clock clock{RCL_ROS_TIME};
+    RCLCPP_WARN_THROTTLE(
+      logger_, clock, 5000,
+      "Current steering angle rate is %.2f Hz. The sensor requires a rate in the range of 10Hz to "
+      "50Hz",
+      estimated_hz);
+  }
+
   constexpr float rad_to_deg = 180.f / M_PI;
   hw_interface_->set_steering_angle_front_axle(rad_to_deg * msg->data);
 }
@@ -287,8 +367,8 @@ void ContinentalARS548HwInterfaceWrapper::set_radar_parameters_request_callback(
     response)
 {
   auto result = hw_interface_->set_radar_parameters(
-    request->maximum_distance, request->frequency_slot, request->cycle_time, request->time_slot,
-    request->country_code, request->powersave_standstill);
+    request->maximum_distance, request->frequency_band, request->cycle_time_ms,
+    request->time_slot_ms, request->country_code, request->powersave_standstill);
 
   response->success = result == Status::OK;
   response->message = (std::stringstream() << result).str();