From a06905b1d9d2297a5e35e7744f84c9cc48bea783 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 23 Aug 2024 09:38:44 +0900 Subject: [PATCH] feat(autonomous_emergency_braking): enable aeb with only one req path (#8569) * make it so AEB works with only one req path type (imu or MPC) Signed-off-by: Daniel Sanchez * fix missing mpc path return Signed-off-by: Daniel Sanchez * add check Signed-off-by: Daniel Sanchez * modify no path msg Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .../src/node.cpp | 29 ++++++++++++------- 1 file changed, 19 insertions(+), 10 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 2498ca4aec115..3b2fe01fb5783 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/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::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";