From baf85591261d5480051a6f17b5ad45168cbb40fc Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 17 Dec 2024 12:49:07 +0900 Subject: [PATCH] feat(start_planner): use planning factor Signed-off-by: satoshi-ota --- .../manager.hpp | 2 +- .../start_planner_module.hpp | 18 +++++++++++++++- .../src/start_planner_module.cpp | 21 +++++++++++++++++-- 3 files changed, 37 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/manager.hpp index 5d3d224673124..68b1e67e484c3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/manager.hpp @@ -38,7 +38,7 @@ class StartPlannerModuleManager : 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_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 47eecb54e2b2a..755cd3aa14598 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 @@ -88,7 +88,8 @@ class StartPlannerModule : 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); ~StartPlannerModule() override { @@ -215,6 +216,21 @@ class StartPlannerModule : public SceneModuleInterface } }; + uint16_t getPlanningFactorDirection( + const autoware::behavior_path_planner::BehaviorModuleOutput & output) const + { + switch (output.turn_signal_info.turn_signal.command) { + case TurnIndicatorsCommand::ENABLE_LEFT: + return PlanningFactor::SHIFT_LEFT; + + case TurnIndicatorsCommand::ENABLE_RIGHT: + return PlanningFactor::SHIFT_RIGHT; + + default: + return PlanningFactor::NONE; + } + }; + /** * @brief Check if there are no moving objects around within a certain radius. * 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 c223390e454d1..9b59fe2c36524 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 @@ -62,8 +62,9 @@ StartPlannerModule::StartPlannerModule( 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_freespace_planner_cb_running_{false} @@ -745,7 +746,10 @@ BehaviorModuleOutput StartPlannerModule::plan() 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) { const double start_distance = autoware::motion_utils::calcSignedArcLength( @@ -758,6 +762,9 @@ BehaviorModuleOutput StartPlannerModule::plan() 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{}); setDebugData(); return output; } @@ -768,6 +775,9 @@ BehaviorModuleOutput StartPlannerModule::plan() 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{}); setDebugData(); @@ -851,6 +861,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() setDrivableAreaInfo(output); const auto steering_factor_direction = getSteeringFactorDirection(output); + const auto planning_factor_direction = getPlanningFactorDirection(output); if (status_.driving_forward) { const double start_distance = autoware::motion_utils::calcSignedArcLength( @@ -864,6 +875,9 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() {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{}); setDebugData(); return output; @@ -875,6 +889,9 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() 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{}); setDebugData();