feat(static_obstacle_avoidance): output safety factor (autowarefoundation#10000)

* feat(safety_check): convert to SafetyFactor

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* feat(static_obstacle_avoidance): use safety factor

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(bpp): output detail

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

---------

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Author: satoshi-ota

planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp

@@ -549,7 +549,7 @@ class SceneModuleInterface
     if (stop_pose_.has_value()) {
       planning_factor_interface_->add(
         path.points, getEgoPose(), stop_pose_.value().pose, PlanningFactor::STOP,
-        SafetyFactorArray{});
+        SafetyFactorArray{}, true, 0.0, 0.0, stop_pose_.value().detail);
       return;
     }
 
@@ -559,7 +559,7 @@ class SceneModuleInterface
 
     planning_factor_interface_->add(
       path.points, getEgoPose(), slow_pose_.value().pose, PlanningFactor::SLOW_DOWN,
-      SafetyFactorArray{});
+      SafetyFactorArray{}, true, 0.0, 0.0, slow_pose_.value().detail);
   }
 
   void setDrivableLanes(const std::vector<DrivableLanes> & drivable_lanes);

planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp

@@ -25,6 +25,7 @@
 #include <autoware_perception_msgs/msg/predicted_path.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/twist.hpp>
+#include <tier4_planning_msgs/msg/planning_factor_array.hpp>
 
 #include <cmath>
 #include <string>
@@ -283,6 +284,9 @@ double calculateRoughDistanceToObjects(
 CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj);
 void updateCollisionCheckDebugMap(
   CollisionCheckDebugMap & debug_map, CollisionCheckDebugPair & object_debug, bool is_safe);
+
+tier4_planning_msgs::msg::SafetyFactorArray to_safety_factor_array(
+  const CollisionCheckDebugMap & debug_map);
 }  // namespace autoware::behavior_path_planner::utils::path_safety_checker
 
 #endif  // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_

planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp

@@ -883,4 +883,18 @@ double calculateRoughDistanceToObjects(
   return min_distance;
 }
 
+tier4_planning_msgs::msg::SafetyFactorArray to_safety_factor_array(
+  const CollisionCheckDebugMap & debug_map)
+{
+  tier4_planning_msgs::msg::SafetyFactorArray safety_factors;
+  for (const auto & [uuid, data] : debug_map) {
+    tier4_planning_msgs::msg::SafetyFactor safety_factor;
+    safety_factor.type = tier4_planning_msgs::msg::SafetyFactor::OBJECT;
+    safety_factor.is_safe = data.is_safe;
+    safety_factor.object_id = autoware::universe_utils::toUUIDMsg(uuid);
+    safety_factor.points.push_back(data.current_obj_pose.position);
+    safety_factors.factors.push_back(safety_factor);
+  }
+  return safety_factors;
+}
 }  // namespace autoware::behavior_path_planner::utils::path_safety_checker

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp

@@ -17,6 +17,7 @@
 
 #include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp"
 #include "autoware/behavior_path_planner_common/interface/scene_module_visitor.hpp"
+#include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp"
 #include "autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp"
 #include "autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp"
 #include "autoware/behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp"
@@ -134,7 +135,8 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface
       if (finish_distance > -1.0e-03) {
         planning_factor_interface_->add(
           start_distance, finish_distance, left_shift.start_pose, left_shift.finish_pose,
-          PlanningFactor::SHIFT_LEFT, SafetyFactorArray{});
+          PlanningFactor::SHIFT_LEFT,
+          utils::path_safety_checker::to_safety_factor_array(debug_data_.collision_check));
       }
     }
 
@@ -154,7 +156,8 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface
       if (finish_distance > -1.0e-03) {
         planning_factor_interface_->add(
           start_distance, finish_distance, right_shift.start_pose, right_shift.finish_pose,
-          PlanningFactor::SHIFT_RIGHT, SafetyFactorArray{});
+          PlanningFactor::SHIFT_RIGHT,
+          utils::path_safety_checker::to_safety_factor_array(debug_data_.collision_check));
       }
     }
   }

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp

@@ -1211,9 +1211,12 @@ CandidateOutput StaticObstacleAvoidanceModule::planCandidate() const
   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);
+    sl_back.end, planning_factor_direction,
+    utils::path_safety_checker::to_safety_factor_array(debug_data_.collision_check), true, 0.0,
+    output.lateral_shift);
 
   output.path_candidate = shifted_path.path;
   return output;