From 48a19cee0f88f237364afea4e075511e755f821b Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 23 Dec 2024 11:07:49 +0900 Subject: [PATCH] feat(static_obstacle_avoidance): use planning factor (#1726) Signed-off-by: satoshi-ota --- .../manager.hpp | 2 +- .../scene.hpp | 9 ++++++++- .../src/scene.cpp | 14 ++++++++++++-- 3 files changed, 21 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp index 895390f91cc16..6296acd8e71f7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp @@ -41,7 +41,7 @@ class StaticObstacleAvoidanceModuleManager : public SceneModuleManagerInterface { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_); } void updateModuleParams(const std::vector & parameters) override; 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 5848cc75f148e..dd071d096748d 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 @@ -46,7 +46,8 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr & planning_factor_interface); CandidateOutput planCandidate() const override; BehaviorModuleOutput plan() override; @@ -134,6 +135,9 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface steering_factor_interface_.set( {left_shift.start_pose, left_shift.finish_pose}, {start_distance, finish_distance}, SteeringFactor::LEFT, SteeringFactor::TURNING, ""); + planning_factor_interface_->add( + start_distance, finish_distance, left_shift.start_pose, left_shift.finish_pose, + PlanningFactor::SHIFT_LEFT, SafetyFactorArray{}); } } @@ -154,6 +158,9 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface steering_factor_interface_.set( {right_shift.start_pose, right_shift.finish_pose}, {start_distance, finish_distance}, SteeringFactor::RIGHT, SteeringFactor::TURNING, ""); + planning_factor_interface_->add( + start_distance, finish_distance, right_shift.start_pose, right_shift.finish_pose, + PlanningFactor::SHIFT_RIGHT, SafetyFactorArray{}); } } } 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 be4ce4a125c68..80ee8e9030469 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 @@ -79,8 +79,9 @@ StaticObstacleAvoidanceModule::StaticObstacleAvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr & planning_factor_interface) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, planning_factor_interface}, // NOLINT helper_{std::make_shared(parameters)}, parameters_{parameters}, generator_{parameters} @@ -780,6 +781,8 @@ void StaticObstacleAvoidanceModule::updateEgoBehavior( insertReturnDeadLine(isBestEffort(parameters_->policy_deceleration), path); + set_longitudinal_planning_factor(path.path); + setVelocityFactor(path.path); } @@ -1221,6 +1224,13 @@ CandidateOutput StaticObstacleAvoidanceModule::planCandidate() const {output.start_distance_to_path_change, output.finish_distance_to_path_change}, steering_factor_direction, SteeringFactor::APPROACHING, ""); + 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); + output.path_candidate = shifted_path.path; return output; }