fix(AEB): check empty path at low speed (autowarefoundation#6346)

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

Author: soblin

control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp

@@ -41,6 +41,7 @@
 
 #include <memory>
 #include <mutex>
+#include <optional>
 #include <string>
 #include <vector>
 
@@ -140,10 +141,9 @@ class AEB : public rclcpp::Node
   bool hasCollision(
     const double current_v, const Path & ego_path, const std::vector<ObjectData> & objects);
 
-  void generateEgoPath(
-    const double curr_v, const double curr_w, Path & path, std::vector<Polygon2d> & polygons);
-  void generateEgoPath(
-    const Trajectory & predicted_traj, Path & path, std::vector<Polygon2d> & polygons);
+  Path generateEgoPath(const double curr_v, const double curr_w, std::vector<Polygon2d> & polygons);
+  std::optional<Path> generateEgoPath(
+    const Trajectory & predicted_traj, std::vector<Polygon2d> & polygons);
   void createObjectData(
     const Path & ego_path, const std::vector<Polygon2d> & ego_polys, const rclcpp::Time & stamp,
     std::vector<ObjectData> & objects);

control/autonomous_emergency_braking/package.xml

@@ -6,6 +6,7 @@
   <description>Autonomous Emergency Braking package as a ROS 2 node</description>
   <maintainer email="takamasa.horibe@tier4.jp">Takamasa Horibe</maintainer>
   <maintainer email="tomoya.kimura@tier4.jp">Tomoya Kimura</maintainer>
+  <maintainer email="mamoru.sobue@tier4.jp">Mamoru Sobue</maintainer>
 
   <license>Apache License 2.0</license>
 

control/autonomous_emergency_braking/src/node.cpp

@@ -309,16 +309,14 @@ bool AEB::checkCollision(MarkerArray & debug_markers)
   // step3. create ego path based on sensor data
   bool has_collision_ego = false;
   if (use_imu_path_) {
-    Path ego_path;
     std::vector<Polygon2d> ego_polys;
     const double current_w = angular_velocity_ptr_->z;
     constexpr double color_r = 0.0 / 256.0;
     constexpr double color_g = 148.0 / 256.0;
     constexpr double color_b = 205.0 / 256.0;
     constexpr double color_a = 0.999;
     const auto current_time = get_clock()->now();
-    generateEgoPath(current_v, current_w, ego_path, ego_polys);
-
+    const auto ego_path = generateEgoPath(current_v, current_w, ego_polys);
     std::vector<ObjectData> objects;
     createObjectData(ego_path, ego_polys, current_time, objects);
     has_collision_ego = hasCollision(current_v, ego_path, objects);
@@ -332,23 +330,25 @@ bool AEB::checkCollision(MarkerArray & debug_markers)
   // step4. transform predicted trajectory from control module
   bool has_collision_predicted = false;
   if (use_predicted_trajectory_) {
-    Path predicted_path;
     std::vector<Polygon2d> predicted_polys;
     const auto predicted_traj_ptr = predicted_traj_ptr_;
     constexpr double color_r = 0.0;
     constexpr double color_g = 100.0 / 256.0;
     constexpr double color_b = 0.0;
     constexpr double color_a = 0.999;
     const auto current_time = predicted_traj_ptr->header.stamp;
-    generateEgoPath(*predicted_traj_ptr, predicted_path, predicted_polys);
-    std::vector<ObjectData> objects;
-    createObjectData(predicted_path, predicted_polys, current_time, objects);
-    has_collision_predicted = hasCollision(current_v, predicted_path, objects);
-
-    std::string ns = "predicted";
-    addMarker(
-      current_time, predicted_path, predicted_polys, objects, color_r, color_g, color_b, color_a,
-      ns, debug_markers);
+    const auto predicted_path_opt = generateEgoPath(*predicted_traj_ptr, predicted_polys);
+    if (predicted_path_opt) {
+      const auto & predicted_path = predicted_path_opt.value();
+      std::vector<ObjectData> objects;
+      createObjectData(predicted_path, predicted_polys, current_time, objects);
+      has_collision_predicted = hasCollision(current_v, predicted_path, objects);
+
+      std::string ns = "predicted";
+      addMarker(
+        current_time, predicted_path, predicted_polys, objects, color_r, color_g, color_b, color_a,
+        ns, debug_markers);
+    }
   }
 
   return has_collision_ego || has_collision_predicted;
@@ -384,9 +384,10 @@ bool AEB::hasCollision(
   return false;
 }
 
-void AEB::generateEgoPath(
-  const double curr_v, const double curr_w, Path & path, std::vector<Polygon2d> & polygons)
+Path AEB::generateEgoPath(
+  const double curr_v, const double curr_w, std::vector<Polygon2d> & polygons)
 {
+  Path path;
   double curr_x = 0.0;
   double curr_y = 0.0;
   double curr_yaw = 0.0;
@@ -397,7 +398,7 @@ void AEB::generateEgoPath(
 
   if (curr_v < 0.1) {
     // if current velocity is too small, assume it stops at the same point
-    return;
+    return path;
   }
 
   constexpr double epsilon = 1e-6;
@@ -435,14 +436,14 @@ void AEB::generateEgoPath(
   for (size_t i = 0; i < path.size() - 1; ++i) {
     polygons.at(i) = createPolygon(path.at(i), path.at(i + 1), vehicle_info_, expand_width_);
   }
+  return path;
 }
 
-void AEB::generateEgoPath(
-  const Trajectory & predicted_traj, Path & path,
-  std::vector<tier4_autoware_utils::Polygon2d> & polygons)
+std::optional<Path> AEB::generateEgoPath(
+  const Trajectory & predicted_traj, std::vector<tier4_autoware_utils::Polygon2d> & polygons)
 {
   if (predicted_traj.points.empty()) {
-    return;
+    return std::nullopt;
   }
 
   geometry_msgs::msg::TransformStamped transform_stamped{};
@@ -452,10 +453,11 @@ void AEB::generateEgoPath(
       rclcpp::Duration::from_seconds(0.5));
   } catch (tf2::TransformException & ex) {
     RCLCPP_ERROR_STREAM(get_logger(), "[AEB] Failed to look up transform from base_link to map");
-    return;
+    return std::nullopt;
   }
 
   // create path
+  Path path;
   path.resize(predicted_traj.points.size());
   for (size_t i = 0; i < predicted_traj.points.size(); ++i) {
     geometry_msgs::msg::Pose map_pose;
@@ -471,6 +473,7 @@ void AEB::generateEgoPath(
   for (size_t i = 0; i < path.size() - 1; ++i) {
     polygons.at(i) = createPolygon(path.at(i), path.at(i + 1), vehicle_info_, expand_width_);
   }
+  return path;
 }
 
 void AEB::createObjectData(