From 6b5b72c553f3be8f0a1ae9e8081adc53b05ed2c7 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Mon, 26 Aug 2024 14:10:39 +0900 Subject: [PATCH] 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 24eaa8516a732..c29247a978c73 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 @@ -156,10 +156,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 dc8c876bd56f6..e8744d85dc11a 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -393,12 +393,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"; @@ -476,17 +479,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