diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index 684d7738273a2..408396eea5382 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -289,7 +289,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: diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp index 572e2f3ee8231..48b16f1ff2b69 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp +++ b/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}; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp index 6d2eb81b328f4..4ff02df97bd89 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp @@ -24,6 +24,7 @@ #include #include #include +#include #include 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 & values) { const auto itr = std::find_if( @@ -262,6 +271,20 @@ class AvoidanceHelper return *itr; } + std::pair getDistanceToAccelEndPoint(const PathWithLaneId & path) + { + updateAccelEndPoint(path); + + if (!max_v_point_.has_value()) { + return std::make_pair(0.0, std::numeric_limits::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> max_v_point_; }; } // namespace behavior_path_planner::helper::avoidance diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp index 8ed9d2d0683c4..86576e2a70e70 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp @@ -332,6 +332,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node) p.max_deceleration = getOrDeclareParameter(*node, ns + "max_deceleration"); p.max_jerk = getOrDeclareParameter(*node, ns + "max_jerk"); p.max_acceleration = getOrDeclareParameter(*node, ns + "max_acceleration"); + p.min_velocity_to_limit_max_acceleration = + getOrDeclareParameter(*node, ns + "min_velocity_to_limit_max_acceleration"); } // constraints (lateral) diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp index bb727a11d6934..ead834c68d01f 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp @@ -235,6 +235,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. diff --git a/planning/behavior_path_avoidance_module/src/manager.cpp b/planning/behavior_path_avoidance_module/src/manager.cpp index 1f420baf4ae63..5ce63987621ed 100644 --- a/planning/behavior_path_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_module/src/manager.cpp @@ -244,6 +244,19 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "nominal_deceleration", p->nominal_deceleration); + updateParam(parameters, ns + "nominal_jerk", p->nominal_jerk); + updateParam(parameters, ns + "max_deceleration", p->max_deceleration); + updateParam(parameters, ns + "max_jerk", p->max_jerk); + updateParam(parameters, ns + "max_acceleration", p->max_acceleration); + updateParam( + parameters, ns + "min_velocity_to_limit_max_acceleration", + p->min_velocity_to_limit_max_acceleration); + } + { const std::string ns = "avoidance.shift_line_pipeline."; updateParam( diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 2d77adde08fe7..814c39083151d 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -686,6 +686,7 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif } insertPrepareVelocity(path); + insertAvoidanceVelocity(path); switch (data.state) { case AvoidanceState::NOT_AVOID: { @@ -1706,6 +1707,52 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const shifted_path.path.points, start_idx, distance_to_object); } +void AvoidanceModule::insertAvoidanceVelocity(ShiftedPath & shifted_path) const +{ + const auto & data = avoid_data_; + + // do nothing if no shift line is approved. + if (path_shifter_.getShiftLines().empty()) { + return; + } + + // do nothing if there is no avoidance target. + if (data.target_objects.empty()) { + return; + } + + const auto [distance_to_accel_end_point, v_max] = + helper_->getDistanceToAccelEndPoint(shifted_path.path); + if (distance_to_accel_end_point < 1e-3) { + return; + } + + const auto start_idx = planner_data_->findEgoIndex(shifted_path.path.points); + for (size_t i = start_idx; i < shifted_path.path.points.size(); ++i) { + const auto distance_from_ego = calcSignedArcLength(shifted_path.path.points, start_idx, i); + + // slow down speed is inserted only in front of the object. + const auto accel_distance = distance_to_accel_end_point - distance_from_ego; + if (accel_distance < 0.0) { + break; + } + + const double v_target_square = + v_max * v_max - 2.0 * parameters_->max_acceleration * accel_distance; + if (v_target_square < 1e-3) { + break; + } + + // target speed with nominal jerk limits. + const double v_target = std::max(getEgoSpeed(), std::sqrt(v_target_square)); + const double v_original = shifted_path.path.points.at(i).point.longitudinal_velocity_mps; + shifted_path.path.points.at(i).point.longitudinal_velocity_mps = std::min(v_original, v_target); + } + + slow_pose_ = motion_utils::calcLongitudinalOffsetPose( + shifted_path.path.points, start_idx, distance_to_accel_end_point); +} + std::shared_ptr AvoidanceModule::get_debug_msg_array() const { debug_data_.avoidance_debug_msg_array.header.stamp = clock_->now();