feat(ars548): hack to produce correct-looking objects for corner ars548 radars

nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp

@@ -345,6 +345,27 @@ bool ContinentalARS548Decoder::parse_objects_list_packet(
     object_msg.orientation = object.position_orientation.value();
     object_msg.orientation_std = object.position_orientation_std.value();
 
+    // cSpell:ignore knzo25
+    // NOTE(knzo25): In the radar firmware used when developing this driver,
+    // corner radars are not supported. We can partially address this,
+    // but the coordinates look only spatially correct (not the dynamics).
+    // so its use is the responsibility of the user.
+    // Corner radars are expected to be supported in a new firmware version,
+    // but this is not yet confirmed.
+    if (
+      std::abs(radar_status_.yaw) > 5.0 * M_PI / 180.0 &&
+      std::abs(radar_status_.yaw) < 90.0 * M_PI / 180.0) {
+      const double dx = radar_status_.longitudinal + radar_status_.wheel_base;
+      const double dy = radar_status_.lateral;
+      double x = object_msg.position.x - dx;
+      double y = object_msg.position.y - dy;
+      const auto & yaw = radar_status_.yaw;
+
+      object_msg.position.x = x * std::cos(yaw) - y * std::sin(yaw) + dx;
+      object_msg.position.y = x * std::sin(yaw) + y * std::cos(yaw) + dy;
+      object_msg.orientation += yaw;
+    }
+
     object_msg.existence_probability = object.existence_probability.value();
     object_msg.classification_car = object.classification_car;
     object_msg.classification_truck = object.classification_truck;

nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp

@@ -204,6 +204,30 @@ 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 are not supported. We can partially address this,
+  // but the coordinates look only spatially correct (not the dynamics).
+  // so its use is the responsibility of the user.
+  // Corner radars are expected to be supported in a new firmware version,
+  // but this is not yet confirmed.
+  if (
+    std::abs(sensor_status.yaw) > 5.0 * M_PI / 180.0 &&
+    std::abs(sensor_status.yaw) < 90.0 * M_PI / 180.0) {
+    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. We "
+      "can partially address this, but the coordinates look only spatially correct (not the "
+      "dynamics). so its use is the responsibility of the user. Corner radars are expected to be "
+      "supported in a new firmware version, but this is not yet confirmed.");
+
+    status.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    status.message +=
+      ". Unsupported mounting configuration (corner radar). This should only be used for "
+      "evaluation purposes.";
+  }
+
   auto add_diagnostic = [&status](const std::string & key, const std::string & value) {
     diagnostic_msgs::msg::KeyValue key_value;
     key_value.key = key;

nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp

@@ -215,6 +215,22 @@ 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 are not supported. We can partially address this,
+  // but the coordinates look only spatially correct (not the dynamics).
+  // so its use is the responsibility of the user.
+  // Corner radars are expected to be supported in a new firmware version,
+  // but this is not yet confirmed.
+  if (std::abs(yaw) > 5.0 * M_PI / 180.0 && std::abs(yaw) < 90.0 * M_PI / 180.0) {
+    RCLCPP_WARN(
+      logger_,
+      "This radar has been configured as a corner radar, which is not supported by the sensor. We "
+      "can partially address this, but the coordinates look only spatially correct (not the "
+      "dynamics). so its use is the responsibility of the user. Corner radars are expected to be "
+      "supported in a new firmware version, but this is not yet confirmed.");
+  }
+
   auto result = hw_interface_->set_sensor_mounting(
     longitudinal, lateral, vertical, yaw, pitch, request->plug_orientation);