add param to other modules

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

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml

@@ -180,6 +180,7 @@
             time_horizon: 10.0
           # hysteresis factor to expand/shrink polygon with the value
           hysteresis_factor_expand_rate: 1.0
+          collision_check_yaw_diff_threshold: 3.1416
           # temporary
           backward_path_length: 100.0
           forward_path_length: 100.0

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -2319,8 +2319,10 @@ std::pair<bool, bool> GoalPlannerModule::isSafePath(
       return autoware::behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS(
         pull_over_path, ego_predicted_path, filtered_objects, collision_check,
         planner_data->parameters, safety_check_params->rss_params,
-        objects_filtering_params->use_all_predicted_path, hysteresis_factor);
-    } else if (parameters.safety_check_params.method == "integral_predicted_polygon") {
+        objects_filtering_params->use_all_predicted_path, hysteresis_factor,
+        safety_check_params->collision_check_yaw_diff_threshold);
+    }
+    if (parameters.safety_check_params.method == "integral_predicted_polygon") {
       return utils::path_safety_checker::checkSafetyWithIntegralPredictedPolygon(
         ego_predicted_path, vehicle_info_, filtered_objects,
         objects_filtering_params->check_all_predicted_path,

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp

@@ -357,6 +357,8 @@ void GoalPlannerModuleManager::init(rclcpp::Node * node)
     p.safety_check_params.method = node->declare_parameter<std::string>(safety_check_ns + "method");
     p.safety_check_params.hysteresis_factor_expand_rate =
       node->declare_parameter<double>(safety_check_ns + "hysteresis_factor_expand_rate");
+    p.safety_check_params.collision_check_yaw_diff_threshold =
+      node->declare_parameter<double>(safety_check_ns + "collision_check_yaw_diff_threshold");
     p.safety_check_params.backward_path_length =
       node->declare_parameter<double>(safety_check_ns + "backward_path_length");
     p.safety_check_params.forward_path_length =
@@ -778,6 +780,9 @@ void GoalPlannerModuleManager::updateModuleParams(
     updateParam<double>(
       parameters, safety_check_ns + "hysteresis_factor_expand_rate",
       p->safety_check_params.hysteresis_factor_expand_rate);
+    updateParam<double>(
+      parameters, safety_check_ns + "collision_check_yaw_diff_threshold",
+      p->safety_check_params.collision_check_yaw_diff_threshold);
     updateParam<double>(
       parameters, safety_check_ns + "backward_path_length",
       p->safety_check_params.backward_path_length);

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml

@@ -30,6 +30,7 @@
       # safety check
       safety_check:
         allow_loose_check_for_cancel: true
+        collision_check_yaw_diff_threshold: 3.1416
         execution:
           expected_front_deceleration: -1.0
           expected_rear_deceleration: -1.0

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp

@@ -137,6 +137,7 @@ struct LaneChangeParameters
 
   // safety check
   bool allow_loose_check_for_cancel{true};
+  double collision_check_yaw_diff_threshold{3.1416};
   utils::path_safety_checker::RSSparams rss_params{};
   utils::path_safety_checker::RSSparams rss_params_for_abort{};
   utils::path_safety_checker::RSSparams rss_params_for_stuck{};

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp

@@ -102,6 +102,8 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node)
   // safety check
   p.allow_loose_check_for_cancel =
     getOrDeclareParameter<bool>(*node, parameter("safety_check.allow_loose_check_for_cancel"));
+  p.collision_check_yaw_diff_threshold = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.collision_check_yaw_diff_threshold"));
 
   p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter<double>(
     *node, parameter("safety_check.execution.longitudinal_distance_min_threshold"));

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

@@ -30,6 +30,7 @@
 #include <lanelet2_core/geometry/Polygon.h>
 
 #include <algorithm>
+#include <cmath>
 #include <limits>
 #include <memory>
 #include <utility>
@@ -2048,6 +2049,8 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
     lane_change_parameters_->lane_expansion_left_offset,
     lane_change_parameters_->lane_expansion_right_offset);
 
+  constexpr double collision_check_yaw_diff_threshold{M_PI};
+
   for (const auto & obj : collision_check_objects) {
     auto current_debug_data = utils::path_safety_checker::createObjectDebug(obj);
     const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj(
@@ -2056,7 +2059,8 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
     for (const auto & obj_path : obj_predicted_paths) {
       const auto collided_polygons = utils::path_safety_checker::getCollidedPolygons(
         path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0,
-        get_max_velocity_for_safety_check(), current_debug_data.second);
+        get_max_velocity_for_safety_check(), collision_check_yaw_diff_threshold,
+        current_debug_data.second);
 
       if (collided_polygons.empty()) {
         utils::path_safety_checker::updateCollisionCheckDebugMap(

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

@@ -23,6 +23,7 @@
 
 #include <boost/uuid/uuid_hash.hpp>
 
+#include <cmath>
 #include <string>
 #include <unordered_map>
 #include <utility>
@@ -203,7 +204,9 @@ struct SafetyCheckParams
   /// possible values: ["RSS", "integral_predicted_polygon"]
   double keep_unsafe_time{0.0};  ///< Time to keep unsafe before changing to safe.
   double hysteresis_factor_expand_rate{
-    0.0};                            ///< Hysteresis factor to expand/shrink polygon with the value.
+    0.0};  ///< Hysteresis factor to expand/shrink polygon with the value.
+  double collision_check_yaw_diff_threshold{
+    3.1416};                         ///< threshold yaw difference between ego and object.
   double backward_path_length{0.0};  ///< Length of the backward lane for path generation.
   double forward_path_length{0.0};   ///< Length of the forward path lane for path generation.
   RSSparams rss_params{};            ///< Parameters related to the RSS model.

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

@@ -100,7 +100,7 @@ bool checkSafetyWithRSS(
   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 double yaw_difference_th = M_PI);
+  const double yaw_difference_th);
 
 /**
  * @brief Iterate the points in the ego and target's predicted path and
@@ -113,6 +113,8 @@ bool checkSafetyWithRSS(
  * @param common_parameters The common parameters used in behavior path planner.
  * @param front_object_deceleration The deceleration of the object in the front.(used in RSS)
  * @param rear_object_deceleration The deceleration of the object in the rear.(used in RSS)
+ * @param yaw_difference_th maximum yaw difference between any given ego path pose and object
+ * predicted path pose.
  * @param debug The debug information for collision checking.
  * @return true if distance is safe.
  */
@@ -122,8 +124,7 @@ 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 yaw_difference_th = M_PI);
+  const double hysteresis_factor, const double yaw_difference_th, CollisionCheckDebug & debug);
 
 /**
  * @brief Iterate the points in the ego and target's predicted path and
@@ -145,8 +146,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 yaw_difference_th = M_PI);
+  const double hysteresis_factor, const double max_velocity_limit, const double yaw_difference_th,
+  CollisionCheckDebug & debug);
 
 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

@@ -499,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, yaw_difference_th);
+          hysteresis_factor, yaw_difference_th, current_debug_data.second);
 
         utils::path_safety_checker::updateCollisionCheckDebugMap(
           debug_map, current_debug_data, !has_collision);
@@ -563,12 +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 yaw_difference_th)
+  const double hysteresis_factor, const double yaw_difference_th, CollisionCheckDebug & debug)
 {
   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,
-    yaw_difference_th);
+    rss_parameters, hysteresis_factor, std::numeric_limits<double>::max(), yaw_difference_th,
+    debug);
   return collided_polygons.empty();
 }
 
@@ -578,8 +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,
-  const double yaw_difference_th)
+  double hysteresis_factor, const double max_velocity_limit, const double yaw_difference_th,
+  CollisionCheckDebug & debug)
 {
   {
     debug.ego_predicted_path = predicted_ego_path;

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

@@ -5,7 +5,6 @@
       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
@@ -144,8 +143,10 @@
             lateral_distance_max_threshold: 2.0
             longitudinal_distance_min_threshold: 3.0
             longitudinal_velocity_delta_time: 0.8
+            extended_polygon_policy: "along_path" # [-] select "rectangle" or "along_path"
           # hysteresis factor to expand/shrink polygon
           hysteresis_factor_expand_rate: 1.0
+          collision_check_yaw_diff_threshold: 1.578
           # temporary
           backward_path_length: 30.0
           forward_path_length: 100.0

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

@@ -100,7 +100,6 @@ 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/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp

@@ -77,7 +77,7 @@ struct PullOutStatus
   // record if the ego has departed from the start point
   bool has_departed{false};
 
-  PullOutStatus() {}
+  PullOutStatus() = default;
 };
 
 class StartPlannerModule : public SceneModuleInterface
@@ -90,7 +90,7 @@ class StartPlannerModule : public SceneModuleInterface
     std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
       objects_of_interest_marker_interface_ptr_map);
 
-  ~StartPlannerModule()
+  ~StartPlannerModule() override
   {
     if (freespace_planner_timer_) {
       freespace_planner_timer_->cancel();
@@ -296,16 +296,16 @@ class StartPlannerModule : public SceneModuleInterface
   PathWithLaneId getCurrentPath() const;
   void planWithPriority(
     const std::vector<Pose> & start_pose_candidates, const Pose & refined_start_pose,
-    const Pose & goal_pose, const std::string search_priority);
+    const Pose & goal_pose, const std::string & search_priority);
   PathWithLaneId generateStopPath() const;
   lanelet::ConstLanelets getPathRoadLanes(const PathWithLaneId & path) const;
   std::vector<DrivableLanes> generateDrivableLanes(const PathWithLaneId & path) const;
   void updatePullOutStatus();
   void updateStatusAfterBackwardDriving();
   PredictedObjects filterStopObjectsInPullOutLanes(
-    const lanelet::ConstLanelets & pull_out_lanes, const geometry_msgs::msg::Point & current_pose,
-    const double velocity_threshold, const double object_check_backward_distance,
-    const double object_check_forward_distance) const;
+    const lanelet::ConstLanelets & pull_out_lanes, const geometry_msgs::msg::Point & current_point,
+    const double velocity_threshold, const double object_check_forward_distance,
+    const double object_check_backward_distance) const;
   bool needToPrepareBlinkerBeforeStartDrivingForward() const;
   bool hasReachedFreespaceEnd() const;
   bool hasReachedPullOutEnd() const;

planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp

@@ -41,8 +41,6 @@ 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 =
@@ -285,6 +283,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
       node->declare_parameter<double>(safety_check_ns + "forward_path_length");
     p.safety_check_params.publish_debug_marker =
       node->declare_parameter<bool>(safety_check_ns + "publish_debug_marker");
+    p.safety_check_params.collision_check_yaw_diff_threshold =
+      node->declare_parameter<double>(safety_check_ns + "collision_check_yaw_diff_threshold");
   }
 
   // RSSparams
@@ -300,6 +300,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
       node->declare_parameter<double>(rss_ns + "longitudinal_distance_min_threshold");
     p.safety_check_params.rss_params.longitudinal_velocity_delta_time =
       node->declare_parameter<double>(rss_ns + "longitudinal_velocity_delta_time");
+    p.safety_check_params.rss_params.extended_polygon_policy =
+      node->declare_parameter<std::string>(rss_ns + "extended_polygon_policy");
   }
 
   // surround moving obstacle check
@@ -369,8 +371,6 @@ 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>(
@@ -661,6 +661,9 @@ void StartPlannerModuleManager::updateModuleParams(
     updateParam<bool>(
       parameters, safety_check_ns + "publish_debug_marker",
       p->safety_check_params.publish_debug_marker);
+    updateParam<double>(
+      parameters, safety_check_ns + "collision_check_yaw_diff_threshold",
+      p->safety_check_params.collision_check_yaw_diff_threshold);
   }
   const std::string rss_ns = safety_check_ns + "rss_params.";
   {
@@ -679,6 +682,9 @@ void StartPlannerModuleManager::updateModuleParams(
     updateParam<double>(
       parameters, rss_ns + "longitudinal_velocity_delta_time",
       p->safety_check_params.rss_params.longitudinal_velocity_delta_time);
+    updateParam<std::string>(
+      parameters, rss_ns + "extended_polygon_policy",
+      p->safety_check_params.rss_params.extended_polygon_policy);
   }
   std::string surround_moving_obstacle_check_ns = ns + "surround_moving_obstacle_check.";
   {

planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -36,6 +36,7 @@
 #include <algorithm>
 #include <cmath>
 #include <memory>
+#include <optional>
 #include <string>
 #include <utility>
 #include <vector>
@@ -66,7 +67,7 @@ StartPlannerModule::StartPlannerModule(
 {
   lane_departure_checker_ = std::make_shared<LaneDepartureChecker>();
   lane_departure_checker_->setVehicleInfo(vehicle_info_);
-  autoware::lane_departure_checker::Param lane_departure_checker_params;
+  autoware::lane_departure_checker::Param lane_departure_checker_params{};
   lane_departure_checker_params.footprint_extra_margin =
     parameters->lane_departure_check_expansion_margin;
 
@@ -502,7 +503,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const
   // Get the closest target obj width in the relevant lanes
   const auto closest_object_width = std::invoke([&]() -> std::optional<double> {
     double arc_length_to_closet_object = std::numeric_limits<double>::max();
-    double closest_object_width = -1.0;
+    std::optional<double> closest_object_width = std::nullopt;
     std::for_each(
       target_objects_on_lane.on_current_lane.begin(), target_objects_on_lane.on_current_lane.end(),
       [&](const auto & o) {
@@ -513,7 +514,6 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const
         arc_length_to_closet_object = arc_length;
         closest_object_width = o.shape.dimensions.y;
       });
-    if (closest_object_width < 0.0) return std::nullopt;
     return closest_object_width;
   });
   if (!closest_object_width) return false;
@@ -838,7 +838,7 @@ PathWithLaneId StartPlannerModule::getCurrentPath() const
 
 void StartPlannerModule::planWithPriority(
   const std::vector<Pose> & start_pose_candidates, const Pose & refined_start_pose,
-  const Pose & goal_pose, const std::string search_priority)
+  const Pose & goal_pose, const std::string & search_priority)
 {
   if (start_pose_candidates.empty()) return;
 
@@ -1113,8 +1113,8 @@ void StartPlannerModule::updateStatusAfterBackwardDriving()
   waitApproval();
   // To enable approval of the forward path, the RTC status is removed.
   removeRTCStatus();
-  for (auto itr = uuid_map_.begin(); itr != uuid_map_.end(); ++itr) {
-    itr->second = generateUUID();
+  for (auto & itr : uuid_map_) {
+    itr.second = generateUUID();
   }
 }
 
@@ -1304,7 +1304,7 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo()
     return ignore_signal_.value() == id;
   };
 
-  const auto update_ignore_signal = [this](const lanelet::Id & id, const bool is_ignore) {
+  const auto update_ignore_signal = [](const lanelet::Id & id, const bool is_ignore) {
     return is_ignore ? std::make_optional(id) : std::nullopt;
   };
 
@@ -1428,7 +1428,7 @@ bool StartPlannerModule::isSafePath() const
     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,
-    parameters_->collision_check_yaw_diff_threshold);
+    safety_check_params_->collision_check_yaw_diff_threshold);
 }
 
 bool StartPlannerModule::isGoalBehindOfEgoInSameRouteSegment() const
@@ -1696,7 +1696,7 @@ void StartPlannerModule::setDebugData()
 
   // safety check
   if (parameters_->safety_check_params.enable_safety_check) {
-    if (debug_data_.ego_predicted_path.size() > 0) {
+    if (!debug_data_.ego_predicted_path.empty()) {
       const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath(
         debug_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution);
       add(
@@ -1705,7 +1705,7 @@ void StartPlannerModule::setDebugData()
         debug_marker_);
     }
 
-    if (debug_data_.filtered_objects.objects.size() > 0) {
+    if (!debug_data_.filtered_objects.objects.empty()) {
       add(
         createObjectsMarkerArray(
           debug_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9),