add param to customize yaw th

Signed-off-by: Daniel Sanchez <[email protected]>

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

@@ -26,6 +26,7 @@
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/twist.hpp>
 
+#include <cmath>
 #include <vector>
 
 namespace autoware::behavior_path_planner::utils::path_safety_checker
@@ -98,7 +99,8 @@ bool checkSafetyWithRSS(
   const std::vector<PoseWithVelocityStamped> & ego_predicted_path,
   const std::vector<ExtendedPredictedObject> & objects, CollisionCheckDebugMap & debug_map,
   const BehaviorPathPlannerParameters & parameters, const RSSparams & rss_params,
-  const bool check_all_predicted_path, const double hysteresis_factor);
+  const bool check_all_predicted_path, const double hysteresis_factor,
+  const double yaw_difference_th = M_PI);
 
 /**
  * @brief Iterate the points in the ego and target's predicted path and
@@ -120,7 +122,8 @@ bool checkCollision(
   const ExtendedPredictedObject & target_object,
   const PredictedPathWithPolygon & target_object_path,
   const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters,
-  const double hysteresis_factor, CollisionCheckDebug & debug);
+  const double hysteresis_factor, CollisionCheckDebug & debug,
+  const double yaw_difference_th = M_PI);
 
 /**
  * @brief Iterate the points in the ego and target's predicted path and
@@ -142,7 +145,8 @@ std::vector<Polygon2d> getCollidedPolygons(
   const ExtendedPredictedObject & target_object,
   const PredictedPathWithPolygon & target_object_path,
   const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters,
-  const double hysteresis_factor, const double max_velocity_limit, CollisionCheckDebug & debug);
+  const double hysteresis_factor, const double max_velocity_limit, CollisionCheckDebug & debug,
+  const double yaw_difference_th = M_PI);
 
 bool checkPolygonsIntersects(
   const std::vector<Polygon2d> & polys_1, const std::vector<Polygon2d> & polys_2);

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

@@ -26,6 +26,9 @@
 #include <boost/geometry/algorithms/union.hpp>
 #include <boost/geometry/strategies/strategies.hpp>
 
+#include <tf2/utils.h>
+
+#include <cmath>
 #include <limits>
 
 namespace autoware::behavior_path_planner::utils::path_safety_checker
@@ -482,7 +485,8 @@ bool checkSafetyWithRSS(
   const std::vector<PoseWithVelocityStamped> & ego_predicted_path,
   const std::vector<ExtendedPredictedObject> & objects, CollisionCheckDebugMap & debug_map,
   const BehaviorPathPlannerParameters & parameters, const RSSparams & rss_params,
-  const bool check_all_predicted_path, const double hysteresis_factor)
+  const bool check_all_predicted_path, const double hysteresis_factor,
+  const double yaw_difference_th)
 {
   // Check for collisions with each predicted path of the object
   const bool is_safe = !std::any_of(objects.begin(), objects.end(), [&](const auto & object) {
@@ -495,7 +499,7 @@ bool checkSafetyWithRSS(
       obj_predicted_paths.begin(), obj_predicted_paths.end(), [&](const auto & obj_path) {
         const bool has_collision = !utils::path_safety_checker::checkCollision(
           planned_path, ego_predicted_path, object, obj_path, parameters, rss_params,
-          hysteresis_factor, current_debug_data.second);
+          hysteresis_factor, current_debug_data.second, yaw_difference_th);
 
         utils::path_safety_checker::updateCollisionCheckDebugMap(
           debug_map, current_debug_data, !has_collision);
@@ -559,11 +563,12 @@ bool checkCollision(
   const ExtendedPredictedObject & target_object,
   const PredictedPathWithPolygon & target_object_path,
   const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters,
-  const double hysteresis_factor, CollisionCheckDebug & debug)
+  const double hysteresis_factor, CollisionCheckDebug & debug, const double yaw_difference_th)
 {
   const auto collided_polygons = getCollidedPolygons(
     planned_path, predicted_ego_path, target_object, target_object_path, common_parameters,
-    rss_parameters, hysteresis_factor, std::numeric_limits<double>::max(), debug);
+    rss_parameters, hysteresis_factor, std::numeric_limits<double>::max(), debug,
+    yaw_difference_th);
   return collided_polygons.empty();
 }
 
@@ -573,7 +578,8 @@ std::vector<Polygon2d> getCollidedPolygons(
   const ExtendedPredictedObject & target_object,
   const PredictedPathWithPolygon & target_object_path,
   const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters,
-  double hysteresis_factor, const double max_velocity_limit, CollisionCheckDebug & debug)
+  double hysteresis_factor, const double max_velocity_limit, CollisionCheckDebug & debug,
+  const double yaw_difference_th)
 {
   {
     debug.ego_predicted_path = predicted_ego_path;
@@ -604,6 +610,10 @@ std::vector<Polygon2d> getCollidedPolygons(
     const auto & ego_polygon = interpolated_data->poly;
     const auto ego_velocity = std::min(interpolated_data->velocity, max_velocity_limit);
 
+    const double ego_yaw = tf2::getYaw(ego_pose.orientation);
+    const double object_yaw = tf2::getYaw(obj_pose.orientation);
+    if (std::abs(ego_yaw - object_yaw) > yaw_difference_th) continue;
+
     // check overlap
     if (boost::geometry::overlaps(ego_polygon, obj_polygon)) {
       debug.unsafe_reason = "overlap_polygon";

planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml

@@ -5,6 +5,7 @@
       th_arrived_distance: 1.0
       th_stopped_velocity: 0.01
       th_stopped_time: 1.0
+      collision_check_yaw_diff_threshold: 1.57
       collision_check_margins: [2.0, 1.0, 0.5, 0.1]
       collision_check_margin_from_front_object: 5.0
       extra_width_margin_for_rear_obstacle: 0.5

planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp

@@ -100,6 +100,7 @@ struct StartPlannerParameters
   double prepare_time_before_start{0.0};
   double th_distance_to_middle_of_the_road{0.0};
   double extra_width_margin_for_rear_obstacle{0.0};
+  double collision_check_yaw_diff_threshold{0.0};
   std::vector<double> collision_check_margins{};
   double collision_check_margin_from_front_object{0.0};
   double th_moving_object_velocity{0.0};

planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp

@@ -41,6 +41,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
     node->declare_parameter<double>(ns + "th_distance_to_middle_of_the_road");
   p.extra_width_margin_for_rear_obstacle =
     node->declare_parameter<double>(ns + "extra_width_margin_for_rear_obstacle");
+  p.collision_check_yaw_diff_threshold =
+    node->declare_parameter<double>(ns + "collision_check_yaw_diff_threshold");
   p.collision_check_margins =
     node->declare_parameter<std::vector<double>>(ns + "collision_check_margins");
   p.collision_check_margin_from_front_object =
@@ -367,7 +369,8 @@ void StartPlannerModuleManager::updateModuleParams(
     updateParam<double>(
       parameters, ns + "extra_width_margin_for_rear_obstacle",
       p->extra_width_margin_for_rear_obstacle);
-
+    updateParam<double>(
+      parameters, ns + "collision_check_yaw_diff_threshold", p->collision_check_yaw_diff_threshold);
     updateParam<std::vector<double>>(
       parameters, ns + "collision_check_margins", p->collision_check_margins);
     updateParam<double>(

planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -34,6 +34,7 @@
 #include <lanelet2_core/geometry/Lanelet.h>
 
 #include <algorithm>
+#include <cmath>
 #include <memory>
 #include <string>
 #include <utility>
@@ -104,7 +105,7 @@ void StartPlannerModule::onFreespacePlannerTimer()
   std::optional<ModuleStatus> current_status_opt{std::nullopt};
   std::optional<StartPlannerParameters> parameters_opt{std::nullopt};
   std::optional<PullOutStatus> pull_out_status_opt{std::nullopt};
-  bool is_stopped;
+  bool is_stopped{false};
 
   // making a local copy of thread sensitive data
   {
@@ -1422,10 +1423,12 @@ bool StartPlannerModule::isSafePath() const
   merged_target_object.insert(
     merged_target_object.end(), target_objects_on_lane.on_shoulder_lane.begin(),
     target_objects_on_lane.on_shoulder_lane.end());
+
   return autoware::behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS(
     pull_out_path, ego_predicted_path, merged_target_object, debug_data_.collision_check,
     planner_data_->parameters, safety_check_params_->rss_params,
-    objects_filtering_params_->use_all_predicted_path, hysteresis_factor);
+    objects_filtering_params_->use_all_predicted_path, hysteresis_factor,
+    parameters_->collision_check_yaw_diff_threshold);
 }
 
 bool StartPlannerModule::isGoalBehindOfEgoInSameRouteSegment() const