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 dd071d096748d..247e22e1aedad 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 @@ -132,9 +132,6 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface } if (finish_distance > -1.0e-03) { - 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{}); @@ -155,9 +152,6 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface } if (finish_distance > -1.0e-03) { - 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 80ee8e9030469..82c14b5dff516 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 @@ -86,8 +86,6 @@ StaticObstacleAvoidanceModule::StaticObstacleAvoidanceModule( parameters_{parameters}, generator_{parameters} { - steering_factor_interface_.init(PlanningBehavior::AVOIDANCE); - velocity_factor_interface_.init(PlanningBehavior::AVOIDANCE); } bool StaticObstacleAvoidanceModule::isExecutionRequested() const @@ -782,8 +780,6 @@ void StaticObstacleAvoidanceModule::updateEgoBehavior( insertReturnDeadLine(isBestEffort(parameters_->policy_deceleration), path); set_longitudinal_planning_factor(path.path); - - setVelocityFactor(path.path); } bool StaticObstacleAvoidanceModule::isSafePath( @@ -1216,14 +1212,6 @@ CandidateOutput StaticObstacleAvoidanceModule::planCandidate() const output.start_distance_to_path_change = sl_front.start_longitudinal; output.finish_distance_to_path_change = sl_back.end_longitudinal; - const uint16_t steering_factor_direction = std::invoke([&output]() { - return output.lateral_shift > 0.0 ? SteeringFactor::LEFT : SteeringFactor::RIGHT; - }); - steering_factor_interface_.set( - {sl_front.start, sl_back.end}, - {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; });