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 <[email protected]> * 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 <[email protected]> * fix: fixed compilation and spells Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * feat: added a diagnostics message Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * feat: disabled objects from corner radars since they are not supported Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: refactored Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> --------- Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]>
tier4 · Jan 20, 2025 · 33e77cf · 33e77cf
1 parent 4669309
commit 33e77cf
Show file tree

Hide file tree

Showing 4 changed files with 41 additions and 0 deletions.
diff --git 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
@@ -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<continental_msgs::msg::ContinentalArs548ObjectList>();
   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
@@ -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
@@ -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);