From 702b183b2889f458bd5086231d76ac9f6eff4424 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 23 Apr 2024 13:13:26 +0900 Subject: [PATCH 1/2] feat(bpp-interface): add empty path validation (#6868) Signed-off-by: satoshi-ota --- .../interface/scene_module_interface.hpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp index a2481040f7548..4947a1b544302 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -141,7 +141,13 @@ class SceneModuleInterface virtual BehaviorModuleOutput run() { updateData(); - return isWaitingApproval() ? planWaitingApproval() : plan(); + const auto output = isWaitingApproval() ? planWaitingApproval() : plan(); + try { + motion_utils::validateNonEmpty(output.path.points); + } catch (const std::exception & ex) { + throw std::invalid_argument("[" + name_ + "]" + ex.what()); + } + return output; } /** From 5a6cde954790dc744b2ec10e88749437e8b57f9e Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Tue, 23 Apr 2024 13:21:09 +0900 Subject: [PATCH 2/2] refactor(bpp): simplify extended predicted object initialization (#6858) * refactor(bpp): simplify extended predicted object initialization Signed-off-by: Muhammad Zulfaqar Azmi * Update planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> --------- Signed-off-by: Muhammad Zulfaqar Azmi Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> --- .../src/utils/utils.cpp | 7 +------ .../path_safety_checker_parameters.hpp | 12 ++++++++++++ .../utils/path_safety_checker/objects_filtering.cpp | 8 +------- 3 files changed, 14 insertions(+), 13 deletions(-) diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp index 97a5ae732ec1b..c401d59f6b71d 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -1112,12 +1112,7 @@ ExtendedPredictedObject transform( [[maybe_unused]] const BehaviorPathPlannerParameters & common_parameters, const LaneChangeParameters & lane_change_parameters, const bool check_at_prepare_phase) { - ExtendedPredictedObject extended_object; - extended_object.uuid = object.object_id; - extended_object.initial_pose = object.kinematics.initial_pose_with_covariance; - extended_object.initial_twist = object.kinematics.initial_twist_with_covariance; - extended_object.initial_acceleration = object.kinematics.initial_acceleration_with_covariance; - extended_object.shape = object.shape; + ExtendedPredictedObject extended_object(object); const auto & time_resolution = lane_change_parameters.prediction_time_resolution; const auto & prepare_duration = lane_change_parameters.lane_change_prepare_duration; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp index b3c8034a3545c..d83c748f214b4 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -31,6 +31,7 @@ namespace behavior_path_planner::utils::path_safety_checker { +using autoware_auto_perception_msgs::msg::PredictedObject; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using tier4_autoware_utils::Polygon2d; @@ -79,6 +80,17 @@ struct ExtendedPredictedObject autoware_auto_perception_msgs::msg::Shape shape; std::vector classification; std::vector predicted_paths; + + ExtendedPredictedObject() = default; + explicit ExtendedPredictedObject(const PredictedObject & object) + : uuid(object.object_id), + initial_pose(object.kinematics.initial_pose_with_covariance), + initial_twist(object.kinematics.initial_twist_with_covariance), + initial_acceleration(object.kinematics.initial_acceleration_with_covariance), + shape(object.shape), + classification(object.classification) + { + } }; using ExtendedPredictedObjects = std::vector; diff --git a/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index 91d743df11ca2..b8014deee9500 100644 --- a/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -289,13 +289,7 @@ ExtendedPredictedObject transform( const PredictedObject & object, const double safety_check_time_horizon, const double safety_check_time_resolution) { - ExtendedPredictedObject extended_object; - extended_object.uuid = object.object_id; - extended_object.initial_pose = object.kinematics.initial_pose_with_covariance; - extended_object.initial_twist = object.kinematics.initial_twist_with_covariance; - extended_object.initial_acceleration = object.kinematics.initial_acceleration_with_covariance; - extended_object.shape = object.shape; - extended_object.classification = object.classification; + ExtendedPredictedObject extended_object(object); const auto obj_velocity = extended_object.initial_twist.twist.linear.x;