feat(autonomous_emergency_braking): enable aeb with only one req path (…

…autowarefoundation#8569)

* make it so AEB works with only one req path type (imu or MPC)

Signed-off-by: Daniel Sanchez <[email protected]>

* fix missing mpc path return

Signed-off-by: Daniel Sanchez <[email protected]>

* add check

Signed-off-by: Daniel Sanchez <[email protected]>

* modify no path msg

Signed-off-by: Daniel Sanchez <[email protected]>

---------

Signed-off-by: Daniel Sanchez <[email protected]>

control/autoware_autonomous_emergency_braking/src/node.cpp

@@ -335,21 +335,30 @@ bool AEB::fetchLatestData()
     return missing("object detection method (pointcloud or predicted objects)");
   }
 
-  const auto imu_ptr = sub_imu_.takeData();
-  if (use_imu_path_) {
+  const bool has_imu_path = std::invoke([&]() {
+    if (!use_imu_path_) return false;
+    const auto imu_ptr = sub_imu_.takeData();
     if (!imu_ptr) {
       return missing("imu message");
     }
     // imu_ptr is valid
     onImu(imu_ptr);
-  }
-  if (use_imu_path_ && !angular_velocity_ptr_) {
-    return missing("imu");
-  }
+    return (!angular_velocity_ptr_) ? missing("imu") : true;
+  });
 
-  predicted_traj_ptr_ = sub_predicted_traj_.takeData();
-  if (use_predicted_trajectory_ && !predicted_traj_ptr_) {
-    return missing("control predicted trajectory");
+  const bool has_predicted_path = std::invoke([&]() {
+    if (!use_predicted_trajectory_) {
+      return false;
+    }
+    predicted_traj_ptr_ = sub_predicted_traj_.takeData();
+    return (!predicted_traj_ptr_) ? missing("control predicted trajectory") : true;
+  });
+
+  if (!has_imu_path && !has_predicted_path) {
+    RCLCPP_INFO_SKIPFIRST_THROTTLE(
+      get_logger(), *get_clock(), 5000,
+      "[AEB] At least one path (IMU or predicted trajectory) is required for operation");
+    return false;
   }
 
   autoware_state_ = sub_autoware_state_.takeData();
@@ -469,7 +478,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers)
 
   // step3. make function to check collision with ego path created with sensor data
   const auto has_collision_ego = [&](pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_objects) -> bool {
-    if (!use_imu_path_) return false;
+    if (!use_imu_path_ || !angular_velocity_ptr_) return false;
     const double current_w = angular_velocity_ptr_->z;
     constexpr colorTuple debug_color = {0.0 / 256.0, 148.0 / 256.0, 205.0 / 256.0, 0.999};
     const std::string ns = "ego";