diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index 41524b1b8193c..62b8eaa7d2702 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -240,7 +240,8 @@ class GoalPlannerModule : public SceneModuleInterface const 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); ~GoalPlannerModule() { diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 0a6185127fcf0..7cb51e55b861b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -66,8 +66,9 @@ GoalPlannerModule::GoalPlannerModule( const 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 parameters_{parameters}, vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()}, is_lane_parking_cb_running_{false}, @@ -1357,6 +1358,19 @@ void GoalPlannerModule::updateSteeringFactor( }); steering_factor_interface_.set(pose, distance, steering_factor_direction, type, ""); + + const uint16_t planning_factor_direction = std::invoke([&]() { + const auto turn_signal = calcTurnSignalInfo(context_data); + if (turn_signal.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { + return PlanningFactor::SHIFT_LEFT; + } else if (turn_signal.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) { + return PlanningFactor::SHIFT_RIGHT; + } + return PlanningFactor::NONE; + }); + + planning_factor_interface_->add( + distance[0], distance[1], pose[0], pose[1], planning_factor_direction, SafetyFactorArray{}); } void GoalPlannerModule::decideVelocity(PullOverPath & pull_over_path) @@ -1571,6 +1585,8 @@ void GoalPlannerModule::postProcess() {distance_to_path_change.first, distance_to_path_change.second}, has_decided_path ? SteeringFactor::TURNING : SteeringFactor::APPROACHING); + set_longitudinal_planning_factor(pull_over_path.full_path()); + setVelocityFactor(pull_over_path.full_path()); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp index 6bbc34a495080..a99252c923328 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp @@ -31,7 +31,7 @@ std::unique_ptr GoalPlannerModuleManager::createNewSceneMo { 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_); } GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters(