From a22a695e2801e68f4ffb9b1a14fff8150eda41de Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Mon, 20 Jan 2025 16:19:56 +0900 Subject: [PATCH] feat: updated the ars548 driver according to new received documentation Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental/continental_ars548.hpp | 8 ++ .../nebula_common/util/ring_buffer.hpp | 72 ++++++++++++++++ .../decoders/continental_ars548_decoder.cpp | 2 +- .../continental_ars548_hw_interface.hpp | 4 +- .../continental_ars548_hw_interface.cpp | 52 ++++++++--- .../ContinentalArs548SetRadarParameters.srv | 8 +- ...ontinental_ars548_hw_interface_wrapper.hpp | 8 ++ ...ontinental_ars548_hw_interface_wrapper.cpp | 86 ++++++++++++++++++- 8 files changed, 220 insertions(+), 20 deletions(-) create mode 100644 nebula_common/include/nebula_common/util/ring_buffer.hpp diff --git a/nebula_common/include/nebula_common/continental/continental_ars548.hpp b/nebula_common/include/nebula_common/continental/continental_ars548.hpp index bc4aca2d3..1102bfcf6 100644 --- a/nebula_common/include/nebula_common/continental/continental_ars548.hpp +++ b/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; diff --git a/nebula_common/include/nebula_common/util/ring_buffer.hpp b/nebula_common/include/nebula_common/util/ring_buffer.hpp new file mode 100644 index 000000000..b624c0d09 --- /dev/null +++ b/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 +#include +#include + +namespace nebula::util +{ + +template +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 buffer_; + std::size_t capacity_; + std::size_t size_{0}; + 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 index 83fa7265a..8db0f82ab 100644 --- a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp +++ b/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"; diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp index f22c3c447..cec8274fc 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp +++ b/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 diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp index 0237f355a..375cbfd39 100644 --- a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp +++ b/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)); diff --git a/nebula_messages/continental_srvs/srv/ContinentalArs548SetRadarParameters.srv b/nebula_messages/continental_srvs/srv/ContinentalArs548SetRadarParameters.srv index 50ddddc7e..512ee796d 100644 --- a/nebula_messages/continental_srvs/srv/ContinentalArs548SetRadarParameters.srv +++ b/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 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 index d3f92d556..dea98a22b 100644 --- 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 +#include #include #include @@ -114,6 +115,13 @@ class ContinentalARS548HwInterfaceWrapper acceleration_sub_{}; rclcpp::Subscription::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 odometry_ring_buffer_; + nebula::util::RingBuffer acceleration_ring_buffer_; + nebula::util::RingBuffer steering_angle_ring_buffer_; + bool standstill_{true}; rclcpp::Service::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 index 79e293347..6b6f05d2c 100644 --- a/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp @@ -29,7 +29,10 @@ ContinentalARS548HwInterfaceWrapper::ContinentalARS548HwInterfaceWrapper( std::make_shared()), 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(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,6 +174,31 @@ 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); } @@ -154,6 +206,34 @@ void ContinentalARS548HwInterfaceWrapper::acceleration_callback( 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(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();