feat(goal_planner): use planning factor (#1731)

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
tier4 · Jan 15, 2025 · bbac8f3 · bbac8f3
1 parent daf60c1
commit bbac8f3
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 4 deletions.
diff --git a/...planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/...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<GoalPlannerParameters> & parameters,
     const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
     std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
-      objects_of_interest_marker_interface_ptr_map);
+      objects_of_interest_marker_interface_ptr_map,
+    const std::shared_ptr<PlanningFactorInterface> planning_factor_interface);
 
   ~GoalPlannerModule()
   {

diff --git a/...avior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/...avior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp
@@ -66,8 +66,9 @@ GoalPlannerModule::GoalPlannerModule(
   const std::shared_ptr<GoalPlannerParameters> & parameters,
   const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
   std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
-    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<PlanningFactorInterface> 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
@@ -31,7 +31,7 @@ std::unique_ptr<SceneModuleInterface> GoalPlannerModuleManager::createNewSceneMo
 {
   return std::make_unique<GoalPlannerModule>(
     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(