fix(autonomous_emergency_braking): fix debug marker visual bug (autow…

…arefoundation#8611) fix bug by using the collision data keeper Signed-off-by: Daniel Sanchez <[email protected]>
tier4 · Sep 10, 2024 · 93c4240 · 93c4240
1 parent a06905b
commit 93c4240
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 14 deletions.
diff --git a/...oware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp b/...oware_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<ObjectData> 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
@@ -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<ObjectData>(*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