feat(start_planner): output object_of_interest (#10053)

feat(start_planner_module): integrate safety factor array from collision check Signed-off-by: kyoichi-sugahara <[email protected]>
autowarefoundation · Jan 30, 2025 · d1a5fe3 · d1a5fe3
1 parent d682048
commit d1a5fe3
Showing 1 changed file with 8 additions and 4 deletions.
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
@@ -744,7 +744,8 @@ BehaviorModuleOutput StartPlannerModule::plan()
     updateRTCStatus(start_distance, finish_distance);
     planning_factor_interface_->add(
       start_distance, finish_distance, status_.pull_out_path.start_pose,
-      status_.pull_out_path.end_pose, planning_factor_direction, SafetyFactorArray{});
+      status_.pull_out_path.end_pose, planning_factor_direction,
+      utils::path_safety_checker::to_safety_factor_array(debug_data_.collision_check));
     setDebugData();
     return output;
   }
@@ -754,7 +755,8 @@ BehaviorModuleOutput StartPlannerModule::plan()
   updateRTCStatus(0.0, distance);
   planning_factor_interface_->add(
     0.0, distance, status_.pull_out_path.start_pose, status_.pull_out_path.end_pose,
-    planning_factor_direction, SafetyFactorArray{});
+    planning_factor_direction,
+    utils::path_safety_checker::to_safety_factor_array(debug_data_.collision_check));
 
   setDebugData();
 
@@ -849,7 +851,8 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
     updateRTCStatus(start_distance, finish_distance);
     planning_factor_interface_->add(
       start_distance, finish_distance, status_.pull_out_path.start_pose,
-      status_.pull_out_path.end_pose, planning_factor_direction, SafetyFactorArray{});
+      status_.pull_out_path.end_pose, planning_factor_direction,
+      utils::path_safety_checker::to_safety_factor_array(debug_data_.collision_check));
     setDebugData();
 
     return output;
@@ -860,7 +863,8 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
   updateRTCStatus(0.0, distance);
   planning_factor_interface_->add(
     0.0, distance, status_.pull_out_path.start_pose, status_.pull_out_path.end_pose,
-    planning_factor_direction, SafetyFactorArray{});
+    planning_factor_direction,
+    utils::path_safety_checker::to_safety_factor_array(debug_data_.collision_check));
 
   setDebugData();