From e48c43c56c9adf55b4b9988cde5a53d651bdba27 Mon Sep 17 00:00:00 2001
From: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>
Date: Fri, 17 Jan 2025 15:45:31 +0900
Subject: [PATCH] feat: disabled objects from corner radars since they are not
supported
Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>
---
.../decoders/continental_ars548_decoder.cpp | 31 ++++++-------------
.../continental_ars548_decoder_wrapper.cpp | 17 ++++------
...ontinental_ars548_hw_interface_wrapper.cpp | 13 +++-----
3 files changed, 20 insertions(+), 41 deletions(-)
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 c2ee20c54..f33a94829 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,16 @@ 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 (
+ std::abs(radar_status_.yaw) > 5.0 * M_PI / 180.0 &&
+ std::abs(radar_status_.yaw) < 90.0 * M_PI / 180.0) {
+ return true;
+ }
+
auto msg_ptr = std::make_unique<continental_msgs::msg::ContinentalArs548ObjectList>();
auto & msg = *msg_ptr;
@@ -345,27 +355,6 @@ 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;
diff --git a/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp
index 7515fb010..da389d3d7 100644
--- a/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp
+++ b/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp
@@ -206,26 +206,21 @@ void ContinentalARS548DecoderWrapper::sensor_status_callback(
// 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.
+ // corner radars were not supported. When a new firmware addresses this,
+ // the driver will be updated.
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.");
+ "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). This should only be used for "
- "evaluation purposes.";
+ ". 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) {
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 905e8a40b..dae6cf062 100644
--- a/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp
+++ b/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp
@@ -217,18 +217,13 @@ void ContinentalARS548HwInterfaceWrapper::set_sensor_mounting_request_callback(
// 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.
+ // corner radars were not supported. When a new firmware addresses this,
+ // the driver will be updated.
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.");
+ "You are attempting to configure the device as a corner radar, which is not supported so "
+ "far.");
}
auto result = hw_interface_->set_sensor_mounting(