feat(static_obstacle_avoidance): use planning factor (#1726)

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

Hide file tree

Showing 3 changed files with 21 additions and 4 deletions.
diff --git a/...idance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp b/...idance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp
@@ -41,7 +41,7 @@ class StaticObstacleAvoidanceModuleManager : public SceneModuleManagerInterface
   {
     return std::make_unique<StaticObstacleAvoidanceModule>(
       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/...voidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp b/...voidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp
@@ -46,7 +46,8 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface
     const std::string & name, rclcpp::Node & node, std::shared_ptr<AvoidanceParameters> 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);
 
   CandidateOutput planCandidate() const override;
   BehaviorModuleOutput plan() override;
@@ -134,6 +135,9 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface
         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{});
       }
     }
 
@@ -154,6 +158,9 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface
         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/...havior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/...havior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp
@@ -79,8 +79,9 @@ StaticObstacleAvoidanceModule::StaticObstacleAvoidanceModule(
   const std::string & name, rclcpp::Node & node, std::shared_ptr<AvoidanceParameters> 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
   helper_{std::make_shared<AvoidanceHelper>(parameters)},
   parameters_{parameters},
   generator_{parameters}
@@ -780,6 +781,8 @@ void StaticObstacleAvoidanceModule::updateEgoBehavior(
 
   insertReturnDeadLine(isBestEffort(parameters_->policy_deceleration), path);
 
+  set_longitudinal_planning_factor(path.path);
+
   setVelocityFactor(path.path);
 }
 
@@ -1221,6 +1224,13 @@ CandidateOutput StaticObstacleAvoidanceModule::planCandidate() const
     {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;
+  });
+  planning_factor_interface_->add(
+    output.start_distance_to_path_change, output.finish_distance_to_path_change, sl_front.start,
+    sl_back.end, planning_factor_direction, SafetyFactorArray{}, true, 0.0, output.lateral_shift);
+
   output.path_candidate = shifted_path.path;
   return output;
 }