feat(start_planner): use planning factor (#1730)

Signed-off-by: satoshi-ota <[email protected]>
tier4 · Dec 23, 2024 · 844f888 · 844f888
1 parent a91367d
commit 844f888
Show file tree

Hide file tree

Showing 3 changed files with 37 additions and 4 deletions.
diff --git a/...path_start_planner_module/include/autoware/behavior_path_start_planner_module/manager.hpp b/...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<StartPlannerModule>(
       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<rclcpp::Parameter> & parameters) override;

diff --git a/...anner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp b/...anner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp
@@ -88,7 +88,8 @@ class StartPlannerModule : public SceneModuleInterface
     const std::shared_ptr<StartPlannerParameters> & 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);
 
   ~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/...ior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/...ior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp
@@ -62,8 +62,9 @@ StartPlannerModule::StartPlannerModule(
   const std::shared_ptr<StartPlannerParameters> & 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_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();