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

* feat: updated the ars548 driver according to new received documentation Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: removed logs and added the max time slot value Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: added the min/maz frequencies as constants Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: fixed compilation errors Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: added units to a print Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: replaced hardcoded strings for some defined in the message Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: removed hardcoded numbers in print Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: refactored the rate checker logic into a class Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: replaced hardcoded prints Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: refactored the ring buffer Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: refactored for clarity Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: added the units to the variable names Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: replaced ifelse clauses byt switch Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * Revert "chore: replaced ifelse clauses byt switch" This reverts commit 0d2efb5. * chore: deleted duplicated logic with method call Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: deleted unused code Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: replaced initial check with the use of an optional Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * fix: fixed burst of messages sent to the radar during startup Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> --------- Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>
tier4 · Feb 18, 2025 · 4a2037c · 4a2037c
1 parent 1953495
commit 4a2037c
Show file tree

Hide file tree

Showing 8 changed files with 256 additions and 18 deletions.
diff --git a/nebula_common/include/nebula_common/continental/continental_ars548.hpp b/nebula_common/include/nebula_common/continental/continental_ars548.hpp
@@ -253,10 +253,19 @@ 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 max_time_slot_ms = 90;
+
 constexpr int hcc_worldwide = 1;
 constexpr int hcc_japan = 2;
 
@@ -280,6 +289,9 @@ constexpr int blockage_test_failed = 0;
 constexpr int blockage_test_passed = 1;
 constexpr int blockage_test_ongoing = 2;
 
+constexpr int min_odometry_hz = 10;
+constexpr int max_odometry_hz = 50;
+
 #pragma pack(push, 1)
 
 struct HeaderPacket

diff --git a/nebula_common/include/nebula_common/util/rate_checker.hpp b/nebula_common/include/nebula_common/util/rate_checker.hpp
@@ -0,0 +1,56 @@
+// 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_hz, double max_rate_hz, std::size_t buffer_size)
+  : min_rate_hz_(min_rate_hz), max_rate_hz_(max_rate_hz), ring_buffer_(buffer_size)
+  {
+  }
+
+  bool is_full() const { return ring_buffer_.is_full(); }
+
+  void update(double stamp)
+  {
+    if (last_stamp_) {
+      ring_buffer_.push_back(stamp - last_stamp_.value());
+    }
+
+    last_stamp_ = stamp;
+  }
+
+  bool is_valid() const
+  {
+    double average = get_average();
+    return average >= min_rate_hz_ && average <= max_rate_hz_;
+  }
+
+  double get_average() const { return 1.0 / ring_buffer_.get_average(); }
+
+private:
+  std::optional<double> last_stamp_;
+  double min_rate_hz_;
+  double max_rate_hz_;
+  RingBuffer<double> ring_buffer_;
+};
+
+}  // namespace nebula::util
diff --git a/nebula_common/include/nebula_common/util/ring_buffer.hpp b/nebula_common/include/nebula_common/util/ring_buffer.hpp
@@ -0,0 +1,55 @@
+// 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) { buffer_.reserve(capacity); }
+
+  void push_back(const T & value)
+  {
+    if (is_full()) {
+      sum_ -= buffer_[index_];
+      buffer_[index_] = value;
+    } else {
+      buffer_.push_back(value);
+    }
+
+    sum_ += value;
+    index_ = (index_ + 1) % buffer_.capacity();
+  }
+
+  std::size_t size() const { return buffer_.size(); }
+
+  bool is_full() const { return buffer_.size() == buffer_.capacity(); }
+
+  T get_average() const { return sum_ / buffer_.size(); }
+
+private:
+  T sum_{};
+  std::vector<T> buffer_;
+  std::size_t index_{0};
+};
+
+}  // namespace nebula::util
diff --git a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp
@@ -486,7 +486,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";

diff --git a/...la_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp b/...la_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp
@@ -191,14 +191,25 @@ 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,
+  uint16_t maximum_distance, uint8_t frequency_slot, uint8_t cycle_time_ms, uint8_t time_slot_ms,
   uint8_t hcc, uint8_t power_save_standstill)
 {
   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 ||
+    time_slot_ms > max_time_slot_ms) {
+    print_error("Invalid time_slot_ms value");
     return Status::SENSOR_CONFIG_ERROR;
   }
 
@@ -209,8 +220,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;
@@ -219,9 +230,9 @@ Status ContinentalARS548HwInterface::set_radar_parameters(
   std::memcpy(send_vector.data(), &configuration_packet, sizeof(ConfigurationPacket));
 
   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("frequency_slot = " + std::to_string(frequency_slot) + " ms");
+  print_info("cycle_time = " + std::to_string(cycle_time_ms) + " ms");
+  print_info("time_slot = " + std::to_string(time_slot_ms) + " ms");
   print_info("hcc = " + std::to_string(hcc));
   print_info("power_save_standstill = " + std::to_string(power_save_standstill));
 

diff --git a/nebula_messages/continental_srvs/srv/ContinentalArs548SetRadarParameters.srv b/nebula_messages/continental_srvs/srv/ContinentalArs548SetRadarParameters.srv
@@ -1,8 +1,13 @@
+string FREQUENCY_BAND_LOW="low"
+string FREQUENCY_BAND_MID="mid"
+string FREQUENCY_BAND_HIGH="high"
+string COUNTRY_CODE_JAPAN="japan"
+string COUNTRY_CODE_WORLDWIDE="worldwide"
 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

diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp b/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/rate_checker.hpp>
 #include <nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp>
 #include <rclcpp/rclcpp.hpp>
 
@@ -114,6 +115,10 @@ class ContinentalARS548HwInterfaceWrapper
     acceleration_sub_{};
   rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr steering_angle_sub_{};
 
+  nebula::util::RateChecker odometry_rate_checker_;
+  nebula::util::RateChecker acceleration_rate_checker_;
+  nebula::util::RateChecker steering_angle_rate_checker_;
+
   bool standstill_{true};
 
   rclcpp::Service<continental_srvs::srv::ContinentalArs548SetNetworkConfiguration>::SharedPtr

diff --git a/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp
@@ -33,7 +33,16 @@ 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_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")));
@@ -125,6 +134,24 @@ ContinentalARS548HwInterfaceWrapper::hw_interface() const
 void ContinentalARS548HwInterfaceWrapper::odometry_callback(
   const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg)
 {
+  using nebula::drivers::continental_ars548::max_odometry_hz;
+  using nebula::drivers::continental_ars548::min_odometry_hz;
+
+  double stamp = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9;
+  odometry_rate_checker_.update(stamp);
+
+  if (!odometry_rate_checker_.is_full()) {
+    return;
+  } else if (!odometry_rate_checker_.is_valid()) {
+    double estimated_hz = odometry_rate_checker_.get_average();
+    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 %dHz to "
+      "%dHz",
+      estimated_hz, min_odometry_hz, max_odometry_hz);
+  }
+
   constexpr float speed_to_standstill = 0.5f;
   constexpr float speed_to_moving = 2.f;
 
@@ -150,13 +177,51 @@ void ContinentalARS548HwInterfaceWrapper::odometry_callback(
 void ContinentalARS548HwInterfaceWrapper::acceleration_callback(
   const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg)
 {
+  using nebula::drivers::continental_ars548::max_odometry_hz;
+  using nebula::drivers::continental_ars548::min_odometry_hz;
+
+  double stamp = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9;
+  acceleration_rate_checker_.update(stamp);
+
+  if (!acceleration_rate_checker_.is_full()) {
+    return;
+  } else if (!acceleration_rate_checker_.is_valid()) {
+    double estimated_hz = acceleration_rate_checker_.get_average();
+    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 %dHz to "
+      "%dHz",
+      estimated_hz, min_odometry_hz, max_odometry_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)
 {
+  using nebula::drivers::continental_ars548::max_odometry_hz;
+  using nebula::drivers::continental_ars548::min_odometry_hz;
+
+  const auto now = std::chrono::high_resolution_clock::now();
+  const double stamp =
+    1e-9 * std::chrono::duration_cast<std::chrono::nanoseconds>(now.time_since_epoch()).count();
+  steering_angle_rate_checker_.update(stamp);
+
+  if (!steering_angle_rate_checker_.is_full()) {
+    return;
+  } else if (!steering_angle_rate_checker_.is_valid()) {
+    double estimated_hz = steering_angle_rate_checker_.get_average();
+    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 %dHz to "
+      "%dHz",
+      estimated_hz, min_odometry_hz, max_odometry_hz);
+  }
+
   constexpr float rad_to_deg = 180.f / M_PI;
   hw_interface_->set_steering_angle_front_axle(rad_to_deg * msg->data);
 }
@@ -289,9 +354,38 @@ void ContinentalARS548HwInterfaceWrapper::set_radar_parameters_request_callback(
   const std::shared_ptr<continental_srvs::srv::ContinentalArs548SetRadarParameters::Response>
     response)
 {
+  using Request = continental_srvs::srv::ContinentalArs548SetRadarParameters::Request;
+
+  uint8_t frequency_slot = 0;
+  uint8_t hcc = 0;
+
+  if (request->frequency_band == Request::FREQUENCY_BAND_LOW) {
+    frequency_slot = nebula::drivers::continental_ars548::frequency_slot_low;
+  } else if (request->frequency_band == Request::FREQUENCY_BAND_MID) {
+    frequency_slot = nebula::drivers::continental_ars548::frequency_slot_mid;
+  } else if (request->frequency_band == Request::FREQUENCY_BAND_HIGH) {
+    frequency_slot = nebula::drivers::continental_ars548::frequency_slot_high;
+  } else {
+    RCLCPP_ERROR(logger_, "Invalid frequency_band value");
+    response->success = false;
+    response->message = "Invalid frequency_band value";
+    return;
+  }
+
+  if (request->country_code == Request::COUNTRY_CODE_WORLDWIDE) {
+    hcc = nebula::drivers::continental_ars548::hcc_worldwide;
+  } else if (request->country_code == Request::COUNTRY_CODE_JAPAN) {
+    hcc = nebula::drivers::continental_ars548::hcc_japan;
+  } else {
+    RCLCPP_ERROR(logger_, "Invalid country_code value");
+    response->success = false;
+    response->message = "Invalid country_code value";
+    return;
+  }
+
   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, frequency_slot, request->cycle_time_ms, request->time_slot_ms, hcc,
+    request->powersave_standstill);
 
   response->success = result == Status::OK;
   response->message = util::to_string(result);