Merge pull request #1242 from tier4/hotfix/v0.26.1/cherry-pick-avoida…

…nce-pr

fix(avoidance): cherry pick avoidance hotfix PRs

planning/behavior_path_avoidance_module/config/avoidance.param.yaml

@@ -287,7 +287,8 @@
           nominal_jerk: 0.5                             # [m/sss]
           max_deceleration: -1.5                        # [m/ss]
           max_jerk: 1.0                                 # [m/sss]
-          max_acceleration: 1.0                         # [m/ss]
+          max_acceleration: 0.5                         # [m/ss]
+          min_velocity_to_limit_max_acceleration: 2.78  # [m/ss]
 
       shift_line_pipeline:
         trim:

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

@@ -143,6 +143,9 @@ struct AvoidanceParameters
   // To prevent large acceleration while avoidance.
   double max_acceleration{0.0};
 
+  // To prevent large acceleration while avoidance.
+  double min_velocity_to_limit_max_acceleration{0.0};
+
   // upper distance for envelope polygon expansion.
   double upper_distance_for_polygon_expansion{0.0};
 

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp

@@ -24,6 +24,7 @@
 #include <algorithm>
 #include <limits>
 #include <memory>
+#include <utility>
 #include <vector>
 
 namespace behavior_path_planner::helper::avoidance
@@ -32,7 +33,10 @@ namespace behavior_path_planner::helper::avoidance
 using behavior_path_planner::PathShifter;
 using behavior_path_planner::PlannerData;
 using motion_utils::calcDecelDistWithJerkAndAccConstraints;
+using motion_utils::calcLongitudinalOffsetPose;
+using motion_utils::calcSignedArcLength;
 using motion_utils::findNearestIndex;
+using tier4_autoware_utils::getPose;
 
 class AvoidanceHelper
 {
@@ -61,6 +65,11 @@ class AvoidanceHelper
 
   geometry_msgs::msg::Pose getEgoPose() const { return data_->self_odometry->pose.pose; }
 
+  geometry_msgs::msg::Point getEgoPosition() const
+  {
+    return data_->self_odometry->pose.pose.position;
+  }
+
   static size_t getConstraintsMapIndex(const double velocity, const std::vector<double> & values)
   {
     const auto itr = std::find_if(
@@ -262,6 +271,20 @@ class AvoidanceHelper
     return *itr;
   }
 
+  std::pair<double, double> getDistanceToAccelEndPoint(const PathWithLaneId & path)
+  {
+    updateAccelEndPoint(path);
+
+    if (!max_v_point_.has_value()) {
+      return std::make_pair(0.0, std::numeric_limits<double>::max());
+    }
+
+    const auto start_idx = data_->findEgoIndex(path.points);
+    const auto distance =
+      calcSignedArcLength(path.points, start_idx, max_v_point_.value().first.position);
+    return std::make_pair(distance, max_v_point_.value().second);
+  }
+
   double getFeasibleDecelDistance(
     const double target_velocity, const bool use_hard_constraints = true) const
   {
@@ -367,6 +390,56 @@ class AvoidanceHelper
     return true;
   }
 
+  void updateAccelEndPoint(const PathWithLaneId & path)
+  {
+    const auto & p = parameters_;
+    const auto & a_now = data_->self_acceleration->accel.accel.linear.x;
+    if (a_now < 0.0) {
+      max_v_point_ = std::nullopt;
+      return;
+    }
+
+    if (getEgoSpeed() < p->min_velocity_to_limit_max_acceleration) {
+      max_v_point_ = std::nullopt;
+      return;
+    }
+
+    if (max_v_point_.has_value()) {
+      return;
+    }
+
+    const auto v0 = getEgoSpeed() + p->buf_slow_down_speed;
+
+    const auto t_neg_jerk = std::max(0.0, a_now - p->max_acceleration) / p->max_jerk;
+    const auto v_neg_jerk = v0 + a_now * t_neg_jerk + std::pow(t_neg_jerk, 2.0) / 2.0;
+    const auto x_neg_jerk = v0 * t_neg_jerk + a_now * std::pow(t_neg_jerk, 2.0) / 2.0 +
+                            p->max_jerk * std::pow(t_neg_jerk, 3.0) / 6.0;
+
+    const auto & v_max = data_->parameters.max_vel;
+    if (v_max < v_neg_jerk) {
+      max_v_point_ = std::nullopt;
+      return;
+    }
+
+    const auto t_max_accel = (v_max - v_neg_jerk) / p->max_acceleration;
+    const auto x_max_accel =
+      v_neg_jerk * t_max_accel + p->max_acceleration * std::pow(t_max_accel, 2.0) / 2.0;
+
+    const auto point =
+      calcLongitudinalOffsetPose(path.points, getEgoPosition(), x_neg_jerk + x_max_accel);
+    if (point.has_value()) {
+      max_v_point_ = std::make_pair(point.value(), v_max);
+      return;
+    }
+
+    const auto x_end = calcSignedArcLength(path.points, getEgoPosition(), path.points.size() - 1);
+    const auto t_end =
+      (std::sqrt(v0 * v0 + 2.0 * p->max_acceleration * x_end) - v0) / p->max_acceleration;
+    const auto v_end = v0 + p->max_acceleration * t_end;
+
+    max_v_point_ = std::make_pair(getPose(path.points.back()), v_end);
+  }
+
   void reset()
   {
     prev_reference_path_ = PathWithLaneId();
@@ -417,6 +490,8 @@ class AvoidanceHelper
   ShiftedPath prev_linear_shift_path_;
 
   lanelet::ConstLanelets prev_driving_lanes_;
+
+  std::optional<std::pair<Pose, double>> max_v_point_;
 };
 }  // namespace behavior_path_planner::helper::avoidance
 

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp

@@ -313,6 +313,10 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
     p.use_shorten_margin_immediately =
       getOrDeclareParameter<bool>(*node, ns + "use_shorten_margin_immediately");
 
+    if (p.policy_approval != "per_shift_line" && p.policy_approval != "per_avoidance_maneuver") {
+      throw std::domain_error("invalid policy. please select 'best_effort' or 'reliable'.");
+    }
+
     if (p.policy_deceleration != "best_effort" && p.policy_deceleration != "reliable") {
       throw std::domain_error("invalid policy. please select 'best_effort' or 'reliable'.");
     }
@@ -330,6 +334,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
     p.max_deceleration = getOrDeclareParameter<double>(*node, ns + "max_deceleration");
     p.max_jerk = getOrDeclareParameter<double>(*node, ns + "max_jerk");
     p.max_acceleration = getOrDeclareParameter<double>(*node, ns + "max_acceleration");
+    p.min_velocity_to_limit_max_acceleration =
+      getOrDeclareParameter<double>(*node, ns + "min_velocity_to_limit_max_acceleration");
   }
 
   // constraints (lateral)

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp

@@ -233,6 +233,12 @@ class AvoidanceModule : public SceneModuleInterface
    */
   void insertPrepareVelocity(ShiftedPath & shifted_path) const;
 
+  /**
+   * @brief insert max velocity in output path to limit acceleration.
+   * @param target path.
+   */
+  void insertAvoidanceVelocity(ShiftedPath & shifted_path) const;
+
   /**
    * @brief calculate stop distance based on object's overhang.
    * @param stop distance.

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp

@@ -85,10 +85,6 @@ lanelet::ConstLanelets getAdjacentLane(
   const std::shared_ptr<const PlannerData> & planner_data,
   const std::shared_ptr<AvoidanceParameters> & parameters, const bool is_right_shift);
 
-lanelet::ConstLanelets getTargetLanelets(
-  const std::shared_ptr<const PlannerData> & planner_data, lanelet::ConstLanelets & route_lanelets,
-  const double left_offset, const double right_offset);
-
 lanelet::ConstLanelets getCurrentLanesFromPath(
   const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data);
 

planning/behavior_path_avoidance_module/src/manager.cpp

@@ -91,6 +91,76 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector<rclcpp::Parame
       p->upper_distance_for_polygon_expansion);
   }
 
+  {
+    const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) {
+      if (p->object_parameters.count(object_type) == 0) {
+        return;
+      }
+      updateParam<bool>(parameters, ns, p->object_parameters.at(object_type).is_avoidance_target);
+    };
+
+    const std::string ns = "avoidance.target_filtering.";
+    set_target_flag(ObjectClassification::CAR, ns + "target_type.car");
+    set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck");
+    set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer");
+    set_target_flag(ObjectClassification::BUS, ns + "target_type.bus");
+    set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian");
+    set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle");
+    set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle");
+    set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown");
+
+    updateParam<double>(
+      parameters, ns + "object_check_goal_distance", p->object_check_goal_distance);
+    updateParam<double>(
+      parameters, ns + "object_check_return_pose_distance", p->object_check_return_pose_distance);
+    updateParam<double>(
+      parameters, ns + "threshold_distance_object_is_on_center",
+      p->threshold_distance_object_is_on_center);
+    updateParam<double>(
+      parameters, ns + "object_check_shiftable_ratio", p->object_check_shiftable_ratio);
+    updateParam<double>(
+      parameters, ns + "object_check_min_road_shoulder_width",
+      p->object_check_min_road_shoulder_width);
+    updateParam<double>(
+      parameters, ns + "object_last_seen_threshold", p->object_last_seen_threshold);
+  }
+
+  {
+    const std::string ns = "avoidance.target_filtering.detection_area.";
+    updateParam<bool>(parameters, ns + "static", p->use_static_detection_area);
+    updateParam<double>(
+      parameters, ns + "min_forward_distance", p->object_check_min_forward_distance);
+    updateParam<double>(
+      parameters, ns + "max_forward_distance", p->object_check_max_forward_distance);
+    updateParam<double>(parameters, ns + "backward_distance", p->object_check_backward_distance);
+  }
+
+  {
+    const std::string ns = "avoidance.avoidance.lateral.avoidance_for_ambiguous_vehicle.";
+    updateParam<bool>(parameters, ns + "enable", p->enable_avoidance_for_ambiguous_vehicle);
+    updateParam<double>(
+      parameters, ns + "closest_distance_to_wait_and_see",
+      p->closest_distance_to_wait_and_see_for_ambiguous_vehicle);
+    updateParam<double>(
+      parameters, ns + "condition.time_threshold", p->time_threshold_for_ambiguous_vehicle);
+    updateParam<double>(
+      parameters, ns + "condition.distance_threshold", p->distance_threshold_for_ambiguous_vehicle);
+    updateParam<double>(
+      parameters, ns + "ignore_area.traffic_light.front_distance",
+      p->object_ignore_section_traffic_light_in_front_distance);
+    updateParam<double>(
+      parameters, ns + "ignore_area.crosswalk.front_distance",
+      p->object_ignore_section_crosswalk_in_front_distance);
+    updateParam<double>(
+      parameters, ns + "ignore_area.crosswalk.behind_distance",
+      p->object_ignore_section_crosswalk_behind_distance);
+  }
+
+  {
+    const std::string ns = "avoidance.target_filtering.intersection.";
+    updateParam<double>(parameters, ns + "yaw_deviation", p->object_check_yaw_deviation);
+  }
+
   {
     const std::string ns = "avoidance.avoidance.lateral.";
     updateParam<double>(
@@ -107,6 +177,7 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector<rclcpp::Parame
     const std::string ns = "avoidance.avoidance.longitudinal.";
     updateParam<double>(parameters, ns + "min_prepare_time", p->min_prepare_time);
     updateParam<double>(parameters, ns + "max_prepare_time", p->max_prepare_time);
+    updateParam<double>(parameters, ns + "min_prepare_distance", p->min_prepare_distance);
     updateParam<double>(parameters, ns + "min_slow_down_speed", p->min_slow_down_speed);
     updateParam<double>(parameters, ns + "buf_slow_down_speed", p->buf_slow_down_speed);
   }
@@ -117,6 +188,33 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector<rclcpp::Parame
     updateParam<double>(parameters, ns + "stop_buffer", p->stop_buffer);
   }
 
+  {
+    const std::string ns = "avoidance.policy.";
+    updateParam<std::string>(parameters, ns + "make_approval_request", p->policy_approval);
+    updateParam<std::string>(parameters, ns + "deceleration", p->policy_deceleration);
+    updateParam<std::string>(parameters, ns + "lateral_margin", p->policy_lateral_margin);
+    updateParam<bool>(
+      parameters, ns + "use_shorten_margin_immediately", p->use_shorten_margin_immediately);
+
+    if (p->policy_approval != "per_shift_line" && p->policy_approval != "per_avoidance_maneuver") {
+      RCLCPP_ERROR(
+        rclcpp::get_logger(__func__),
+        "invalid policy. please select 'per_shift_line' or 'per_avoidance_maneuver'.");
+    }
+
+    if (p->policy_deceleration != "best_effort" && p->policy_deceleration != "reliable") {
+      RCLCPP_ERROR(
+        rclcpp::get_logger(__func__),
+        "invalid deceleration policy. Please select 'best_effort' or 'reliable'.");
+    }
+
+    if (p->policy_lateral_margin != "best_effort" && p->policy_lateral_margin != "reliable") {
+      RCLCPP_ERROR(
+        rclcpp::get_logger(__func__),
+        "invalid lateral margin policy. Please select 'best_effort' or 'reliable'.");
+    }
+  }
+
   {
     const std::string ns = "avoidance.constrains.lateral.";
 
@@ -146,6 +244,19 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector<rclcpp::Parame
     }
   }
 
+  {
+    const std::string ns = "avoidance.constraints.longitudinal.";
+
+    updateParam<double>(parameters, ns + "nominal_deceleration", p->nominal_deceleration);
+    updateParam<double>(parameters, ns + "nominal_jerk", p->nominal_jerk);
+    updateParam<double>(parameters, ns + "max_deceleration", p->max_deceleration);
+    updateParam<double>(parameters, ns + "max_jerk", p->max_jerk);
+    updateParam<double>(parameters, ns + "max_acceleration", p->max_acceleration);
+    updateParam<double>(
+      parameters, ns + "min_velocity_to_limit_max_acceleration",
+      p->min_velocity_to_limit_max_acceleration);
+  }
+
   {
     const std::string ns = "avoidance.shift_line_pipeline.";
     updateParam<double>(