From 33e77cfa081b7beb648aa929cdb70451e5f95d66 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos Tsunekawa Date: Mon, 20 Jan 2025 13:10:51 +0900 Subject: [PATCH] feat(ars548): disable object output for corner radars (#252) * feat: hack to produce correct-looking objects for corner ars548 radars. This essentially assumes that continental ignores the configuration's yaw for FRONT corner radars. Kinematics will be wrong no matter what... Signed-off-by: Kenzo Lobos-Tsunekawa * chore: added log messages to warn the user against using corner radars (it should only be for evaluation purposes) Signed-off-by: Kenzo Lobos-Tsunekawa * fix: fixed compilation and spells Signed-off-by: Kenzo Lobos-Tsunekawa * feat: added a diagnostics message Signed-off-by: Kenzo Lobos-Tsunekawa * feat: disabled objects from corner radars since they are not supported Signed-off-by: Kenzo Lobos-Tsunekawa * chore: refactored Signed-off-by: Kenzo Lobos-Tsunekawa --------- Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental/continental_ars548.hpp | 5 +++++ .../decoders/continental_ars548_decoder.cpp | 8 ++++++++ .../continental_ars548_decoder_wrapper.cpp | 17 +++++++++++++++++ .../continental_ars548_hw_interface_wrapper.cpp | 11 +++++++++++ 4 files changed, 41 insertions(+) diff --git a/nebula_common/include/nebula_common/continental/continental_ars548.hpp b/nebula_common/include/nebula_common/continental/continental_ars548.hpp index 6d861384d..bc4aca2d3 100644 --- a/nebula_common/include/nebula_common/continental/continental_ars548.hpp +++ b/nebula_common/include/nebula_common/continental/continental_ars548.hpp @@ -32,6 +32,11 @@ namespace drivers namespace continental_ars548 { +inline bool is_corner_radar(float yaw) +{ + return std::abs(yaw) > deg2rad(5.0) && std::abs(yaw) < deg2rad(90.0); +} + /// @brief struct for ARS548 sensor configuration struct ContinentalARS548SensorConfiguration : EthernetSensorConfigurationBase { 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 a6d3e983f..83fa7265a 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 @@ -245,6 +245,14 @@ bool ContinentalARS548Decoder::parse_detections_list_packet( bool ContinentalARS548Decoder::parse_objects_list_packet( const nebula_msgs::msg::NebulaPacket & packet_msg) { + // cSpell:ignore knzo25 + // NOTE(knzo25): In the radar firmware used when developing this driver, + // corner radars were not supported. When a new firmware addresses this, + // the driver will be updated. + if (nebula::drivers::continental_ars548::is_corner_radar(radar_status_.yaw)) { + return true; + } + auto msg_ptr = std::make_unique(); auto & msg = *msg_ptr; diff --git a/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp index 1bdfdd717..a02d58cce 100644 --- a/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp @@ -204,6 +204,23 @@ void ContinentalARS548DecoderWrapper::sensor_status_callback( status.name = config_ptr_->frame_id; status.message = "Diagnostic messages from ARS548"; + // cSpell:ignore knzo25 + // NOTE(knzo25): In the radar firmware used when developing this driver, + // corner radars were not supported. When a new firmware addresses this, + // the driver will be updated. + if (nebula::drivers::continental_ars548::is_corner_radar(sensor_status.yaw)) { + rclcpp::Clock clock{RCL_ROS_TIME}; + RCLCPP_WARN_THROTTLE( + logger_, clock, 5000, + "This radar has been configured as a corner radar, which is not supported by the sensor. The " + "driver will not output any objects"); + + status.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + status.message += + ". Unsupported mounting configuration (corner radar). Only detections should be used under " + "these conditions."; + } + auto add_diagnostic = [&status](const std::string & key, const std::string & value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; 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 0fc179524..79e293347 100644 --- a/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp @@ -215,6 +215,17 @@ void ContinentalARS548HwInterfaceWrapper::set_sensor_mounting_request_callback( pitch = rpy.y; } + // cSpell:ignore knzo25 + // NOTE(knzo25): In the radar firmware used when developing this driver, + // corner radars were not supported. When a new firmware addresses this, + // the driver will be updated. + if (nebula::drivers::continental_ars548::is_corner_radar(yaw)) { + RCLCPP_WARN( + logger_, + "You are attempting to configure the device as a corner radar, which is not supported so " + "far."); + } + auto result = hw_interface_->set_sensor_mounting( longitudinal, lateral, vertical, yaw, pitch, request->plug_orientation);