diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp index 741d4552d8558..c5c0972972773 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -549,7 +549,7 @@ class SceneModuleInterface if (stop_pose_.has_value()) { planning_factor_interface_->add( path.points, getEgoPose(), stop_pose_.value().pose, PlanningFactor::STOP, - SafetyFactorArray{}); + SafetyFactorArray{}, true, 0.0, 0.0, stop_pose_.value().detail); return; } @@ -559,7 +559,7 @@ class SceneModuleInterface planning_factor_interface_->add( path.points, getEgoPose(), slow_pose_.value().pose, PlanningFactor::SLOW_DOWN, - SafetyFactorArray{}); + SafetyFactorArray{}, true, 0.0, 0.0, slow_pose_.value().detail); } void setDrivableLanes(const std::vector & drivable_lanes); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp index ab62c369c41e4..23ff10a2a0abf 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -283,6 +284,9 @@ double calculateRoughDistanceToObjects( CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj); void updateCollisionCheckDebugMap( CollisionCheckDebugMap & debug_map, CollisionCheckDebugPair & object_debug, bool is_safe); + +tier4_planning_msgs::msg::SafetyFactorArray to_safety_factor_array( + const CollisionCheckDebugMap & debug_map); } // namespace autoware::behavior_path_planner::utils::path_safety_checker #endif // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index 5ae62f51bdb52..b448ae50492ac 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -883,4 +883,18 @@ double calculateRoughDistanceToObjects( return min_distance; } +tier4_planning_msgs::msg::SafetyFactorArray to_safety_factor_array( + const CollisionCheckDebugMap & debug_map) +{ + tier4_planning_msgs::msg::SafetyFactorArray safety_factors; + for (const auto & [uuid, data] : debug_map) { + tier4_planning_msgs::msg::SafetyFactor safety_factor; + safety_factor.type = tier4_planning_msgs::msg::SafetyFactor::OBJECT; + safety_factor.is_safe = data.is_safe; + safety_factor.object_id = autoware::universe_utils::toUUIDMsg(uuid); + safety_factor.points.push_back(data.current_obj_pose.position); + safety_factors.factors.push_back(safety_factor); + } + return safety_factors; +} } // namespace autoware::behavior_path_planner::utils::path_safety_checker diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp index 247e22e1aedad..46b6388fd5119 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp @@ -17,6 +17,7 @@ #include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" #include "autoware/behavior_path_planner_common/interface/scene_module_visitor.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include "autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp" #include "autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp" #include "autoware/behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp" @@ -134,7 +135,8 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface if (finish_distance > -1.0e-03) { planning_factor_interface_->add( start_distance, finish_distance, left_shift.start_pose, left_shift.finish_pose, - PlanningFactor::SHIFT_LEFT, SafetyFactorArray{}); + PlanningFactor::SHIFT_LEFT, + utils::path_safety_checker::to_safety_factor_array(debug_data_.collision_check)); } } @@ -154,7 +156,8 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface if (finish_distance > -1.0e-03) { planning_factor_interface_->add( start_distance, finish_distance, right_shift.start_pose, right_shift.finish_pose, - PlanningFactor::SHIFT_RIGHT, SafetyFactorArray{}); + PlanningFactor::SHIFT_RIGHT, + utils::path_safety_checker::to_safety_factor_array(debug_data_.collision_check)); } } } diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index 739b5b21f59dc..e0f96c3ab2ed1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -1211,9 +1211,12 @@ CandidateOutput StaticObstacleAvoidanceModule::planCandidate() const const uint16_t planning_factor_direction = std::invoke([&output]() { return output.lateral_shift > 0.0 ? PlanningFactor::SHIFT_LEFT : PlanningFactor::SHIFT_RIGHT; }); + planning_factor_interface_->add( output.start_distance_to_path_change, output.finish_distance_to_path_change, sl_front.start, - sl_back.end, planning_factor_direction, SafetyFactorArray{}, true, 0.0, output.lateral_shift); + sl_back.end, planning_factor_direction, + utils::path_safety_checker::to_safety_factor_array(debug_data_.collision_check), true, 0.0, + output.lateral_shift); output.path_candidate = shifted_path.path; return output;