From c2532f41db4d6531d0ba8edceb8d571647f1eca0 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Thu, 26 Dec 2024 10:13:12 +0900 Subject: [PATCH] fix(start_planner): remove velocity factor Signed-off-by: satoshi-ota --- .../start_planner_module.hpp | 15 -------------- .../src/start_planner_module.cpp | 20 ------------------- 2 files changed, 35 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp index 755cd3aa14598..35db6737a5a1a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp @@ -201,21 +201,6 @@ class StartPlannerModule : public SceneModuleInterface bool requiresDynamicObjectsCollisionDetection() const; - uint16_t getSteeringFactorDirection( - const autoware::behavior_path_planner::BehaviorModuleOutput & output) const - { - switch (output.turn_signal_info.turn_signal.command) { - case TurnIndicatorsCommand::ENABLE_LEFT: - return SteeringFactor::LEFT; - - case TurnIndicatorsCommand::ENABLE_RIGHT: - return SteeringFactor::RIGHT; - - default: - return SteeringFactor::STRAIGHT; - } - }; - uint16_t getPlanningFactorDirection( const autoware::behavior_path_planner::BehaviorModuleOutput & output) const { diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index 9b59fe2c36524..41876392c2662 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -101,9 +101,6 @@ StartPlannerModule::StartPlannerModule( std::bind(&StartPlannerModule::onFreespacePlannerTimer, this), freespace_planner_timer_cb_group_); } - - steering_factor_interface_.init(PlanningBehavior::START_PLANNER); - velocity_factor_interface_.init(PlanningBehavior::START_PLANNER); } void StartPlannerModule::onFreespacePlannerTimer() @@ -744,11 +741,8 @@ BehaviorModuleOutput StartPlannerModule::plan() setDrivableAreaInfo(output); - setVelocityFactor(output.path); - set_longitudinal_planning_factor(output.path); - const auto steering_factor_direction = getSteeringFactorDirection(output); const auto planning_factor_direction = getPlanningFactorDirection(output); if (status_.driving_forward) { @@ -759,9 +753,6 @@ BehaviorModuleOutput StartPlannerModule::plan() path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.end_pose.position); updateRTCStatus(start_distance, finish_distance); - steering_factor_interface_.set( - {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, - {start_distance, finish_distance}, steering_factor_direction, SteeringFactor::TURNING, ""); planning_factor_interface_->add( start_distance, finish_distance, status_.pull_out_path.start_pose, status_.pull_out_path.end_pose, planning_factor_direction, SafetyFactorArray{}); @@ -772,9 +763,6 @@ BehaviorModuleOutput StartPlannerModule::plan() path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); updateRTCStatus(0.0, distance); - steering_factor_interface_.set( - {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance}, - steering_factor_direction, SteeringFactor::TURNING, ""); planning_factor_interface_->add( 0.0, distance, status_.pull_out_path.start_pose, status_.pull_out_path.end_pose, planning_factor_direction, SafetyFactorArray{}); @@ -860,7 +848,6 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() setDrivableAreaInfo(output); - const auto steering_factor_direction = getSteeringFactorDirection(output); const auto planning_factor_direction = getPlanningFactorDirection(output); if (status_.driving_forward) { @@ -871,10 +858,6 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() stop_path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.end_pose.position); updateRTCStatus(start_distance, finish_distance); - steering_factor_interface_.set( - {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, - {start_distance, finish_distance}, steering_factor_direction, SteeringFactor::APPROACHING, - ""); planning_factor_interface_->add( start_distance, finish_distance, status_.pull_out_path.start_pose, status_.pull_out_path.end_pose, planning_factor_direction, SafetyFactorArray{}); @@ -886,9 +869,6 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() stop_path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); updateRTCStatus(0.0, distance); - steering_factor_interface_.set( - {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance}, - steering_factor_direction, SteeringFactor::APPROACHING, ""); planning_factor_interface_->add( 0.0, distance, status_.pull_out_path.start_pose, status_.pull_out_path.end_pose, planning_factor_direction, SafetyFactorArray{});