From 337124cd879995d5f1034b138a0d6c732da75226 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Mon, 26 Aug 2024 14:26:00 +0900 Subject: [PATCH] fix(autonomous_emergency_braking): fix debug marker visual bug (#8611) fix bug by using the collision data keeper Signed-off-by: Daniel Sanchez --- .../autonomous_emergency_braking/node.hpp | 5 +--- .../src/node.cpp | 25 +++++++++++-------- 2 files changed, 16 insertions(+), 14 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp index 9ab0e997f61d2..30ac91a3c890b 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp @@ -153,10 +153,7 @@ class CollisionDataKeeper * @brief Get the closest object data * @return Object data of the closest object */ - [[nodiscard]] ObjectData get() const - { - return (closest_object_.has_value()) ? closest_object_.value() : ObjectData(); - } + [[nodiscard]] std::optional get() const { return closest_object_; } /** * @brief Get the previous closest object data diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 3b2fe01fb5783..52b4d22f2e40d 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -380,12 +380,15 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) const auto diag_level = DiagnosticStatus::ERROR; stat.summary(diag_level, error_msg); const auto & data = collision_data_keeper_.get(); - stat.addf("RSS", "%.2f", data.rss); - stat.addf("Distance", "%.2f", data.distance_to_object); - stat.addf("Object Speed", "%.2f", data.velocity); - if (publish_debug_markers_) { - addCollisionMarker(data, debug_markers); + if (data.has_value()) { + stat.addf("RSS", "%.2f", data.value().rss); + stat.addf("Distance", "%.2f", data.value().distance_to_object); + stat.addf("Object Speed", "%.2f", data.value().velocity); + if (publish_debug_markers_) { + addCollisionMarker(data.value(), debug_markers); + } } + addVirtualStopWallMarker(info_markers); } else { const std::string error_msg = "[AEB]: No Collision"; @@ -463,17 +466,19 @@ bool AEB::checkCollision(MarkerArray & debug_markers) return std::make_optional(*closest_object_point_itr); }); + const bool has_collision = (closest_object_point.has_value()) + ? hasCollision(current_v, closest_object_point.value()) + : false; + // Add debug markers if (publish_debug_markers_) { const auto [color_r, color_g, color_b, color_a] = debug_colors; addMarker( - this->get_clock()->now(), path, ego_polys, objects, closest_object_point, color_r, color_g, - color_b, color_a, debug_ns, debug_markers); + this->get_clock()->now(), path, ego_polys, objects, collision_data_keeper_.get(), color_r, + color_g, color_b, color_a, debug_ns, debug_markers); } // check collision using rss distance - return (closest_object_point.has_value()) - ? hasCollision(current_v, closest_object_point.value()) - : false; + return has_collision; }; // step3. make function to check collision with ego path created with sensor data