From bf407841b37a2708b0c7278160dac765ec711b15 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Thu, 22 Aug 2024 10:30:22 +0900 Subject: [PATCH 1/4] make it so AEB works with only one req path type (imu or MPC) Signed-off-by: Daniel Sanchez --- .../src/node.cpp | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index cd97a8301ad9d..74449b2eae8d9 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -349,20 +349,23 @@ bool AEB::fetchLatestData() } const auto imu_ptr = sub_imu_.takeData(); - if (use_imu_path_) { + const bool has_imu_path = std::invoke([&]() { + if (!use_imu_path_) return false; 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 = (use_predicted_trajectory_ && !predicted_traj_ptr_) + ? missing("control predicted trajectory") + : true; + + if (!has_imu_path && !has_predicted_path) { + return missing("any type of path"); } autoware_state_ = sub_autoware_state_.takeData(); From a1e10336b436a61d7ac71d146fd3fee0e08f61f9 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Thu, 22 Aug 2024 11:02:48 +0900 Subject: [PATCH 2/4] fix missing mpc path return Signed-off-by: Daniel Sanchez --- .../src/node.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 74449b2eae8d9..c1ff7caad338b 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -348,9 +348,9 @@ bool AEB::fetchLatestData() return missing("object detection method (pointcloud or predicted objects)"); } - const auto imu_ptr = sub_imu_.takeData(); 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"); } @@ -359,10 +359,13 @@ bool AEB::fetchLatestData() return (!angular_velocity_ptr_) ? missing("imu") : true; }); - predicted_traj_ptr_ = sub_predicted_traj_.takeData(); - const bool has_predicted_path = (use_predicted_trajectory_ && !predicted_traj_ptr_) - ? missing("control predicted trajectory") - : true; + 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) { return missing("any type of path"); From 19caa18e34472af83cf2d77800a089b4bf89fe16 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Thu, 22 Aug 2024 13:50:42 +0900 Subject: [PATCH 3/4] add check Signed-off-by: Daniel Sanchez --- control/autoware_autonomous_emergency_braking/src/node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index c1ff7caad338b..18a0a83055f1d 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -488,7 +488,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"; From 5da597210643ce15a1f6d6374c1d7c54f69d659b Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Thu, 22 Aug 2024 18:09:09 +0900 Subject: [PATCH 4/4] modify no path msg Signed-off-by: Daniel Sanchez --- control/autoware_autonomous_emergency_braking/src/node.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 18a0a83055f1d..dc8c876bd56f6 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -368,7 +368,10 @@ bool AEB::fetchLatestData() }); if (!has_imu_path && !has_predicted_path) { - return missing("any type of 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();