feat(static_obstacle_avoidance): output safety factor (autowarefounda…

…tion#10000) * feat(safety_check): convert to SafetyFactor Signed-off-by: satoshi-ota <[email protected]> * feat(static_obstacle_avoidance): use safety factor Signed-off-by: satoshi-ota <[email protected]> * fix(bpp): output detail Signed-off-by: satoshi-ota <[email protected]> --------- Signed-off-by: satoshi-ota <[email protected]>
tier4 · Jan 23, 2025 · ba4b755 · ba4b755
1 parent d4e488a
commit ba4b755
Show file tree

Hide file tree

Showing 5 changed files with 29 additions and 5 deletions.
diff --git a/...common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp b/...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);

diff --git a/.../include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/.../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_
diff --git a/...nner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/...nner/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
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
@@ -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));
       }
     }
   }

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
@@ -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;