From 97c8ee3c651d457ec3dbb58724e707ed300ae4ef Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Fri, 30 Jun 2023 12:06:09 +0900 Subject: [PATCH 1/6] feat(path_shifter): add utils Signed-off-by: satoshi-ota --- .../utils/path_shifter/path_shifter.hpp | 3 +++ .../src/utils/path_shifter/path_shifter.cpp | 12 ++++++++++++ 2 files changed, 15 insertions(+) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp index 76119a18646ab..93c3a4a768c0b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp @@ -136,6 +136,9 @@ class PathShifter // Utility Functions //////////////////////////////////////// + static double calcFeasibleVelocityFromJerk( + const double lateral, const double jerk, const double distance); + static double calcLongitudinalDistFromJerk( const double lateral, const double jerk, const double velocity); diff --git a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp index c1058969434d9..eb752220d3690 100644 --- a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp @@ -554,6 +554,18 @@ double PathShifter::calcShiftTimeFromJerk(const double lateral, const double jer return t_total; } +double PathShifter::calcFeasibleVelocityFromJerk( + const double lateral, const double jerk, const double distance) +{ + const double j = std::abs(jerk); + const double l = std::abs(lateral); + const double d = std::abs(distance); + if (j < 1.0e-8) { + return 1.0e10; // TODO(Horibe) maybe invalid arg? + } + return d / (4.0 * std::pow(0.5 * l / j, 1.0 / 3.0)); +} + double PathShifter::calcLongitudinalDistFromJerk( const double lateral, const double jerk, const double velocity) { From c384b6f80d8c0145ea2dcd653e4a9c84260399f8 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Fri, 30 Jun 2023 12:09:50 +0900 Subject: [PATCH 2/6] feat(avoidance): shorten avoidance stop distance Signed-off-by: satoshi-ota --- .../config/avoidance/avoidance.param.yaml | 3 +++ .../utils/avoidance/avoidance_module_data.hpp | 9 +++++++++ .../behavior_path_planner/utils/avoidance/helper.hpp | 9 +-------- .../src/scene_module/avoidance/avoidance_module.cpp | 4 ++-- .../src/scene_module/avoidance/manager.cpp | 2 ++ 5 files changed, 17 insertions(+), 10 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 7c785efa0d5a9..d5f908ae9856f 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -144,6 +144,8 @@ min_avoidance_distance: 10.0 # [m] min_nominal_avoidance_speed: 7.0 # [m/s] min_sharp_avoidance_speed: 1.0 # [m/s] + min_slow_down_speed: 1.38 # [m/s] + buf_slow_down_speed: 0.56 # [m/s] # For yield maneuver yield: @@ -153,6 +155,7 @@ stop: min_distance: 10.0 # [m] max_distance: 20.0 # [m] + stop_buffer: 1.0 # [m] constraints: # vehicle slows down under longitudinal constraints diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 0b016db9c9c2c..2485dee6128b0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -196,6 +196,9 @@ struct AvoidanceParameters // maximum stop distance double stop_max_distance; + // stop buffer + double stop_buffer; + // start avoidance after this time to avoid sudden path change double prepare_time; @@ -216,6 +219,12 @@ struct AvoidanceParameters // distance for avoidance. Need a sharp avoidance path to avoid the object. double min_sharp_avoidance_speed; + // minimum slow down speed + double min_slow_down_speed; + + // slow down speed buffer + double buf_slow_down_speed; + // The margin is configured so that the generated avoidance trajectory does not come near to the // road shoulder. double road_shoulder_safety_margin{1.0}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp index 194c62aaeec5a..a756c9518825d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp @@ -74,13 +74,6 @@ class AvoidanceHelper return std::max(getEgoSpeed(), parameters_->min_sharp_avoidance_speed); } - float getMaximumAvoidanceEgoSpeed() const - { - return parameters_->target_velocity_matrix.at(parameters_->col_size - 1); - } - - float getMinimumAvoidanceEgoSpeed() const { return parameters_->target_velocity_matrix.front(); } - double getNominalPrepareDistance() const { const auto & p = parameters_; @@ -103,7 +96,7 @@ class AvoidanceHelper { const auto & p = parameters_; const auto distance_by_jerk = PathShifter::calcLongitudinalDistFromJerk( - shift_length, p->nominal_lateral_jerk, getMinimumAvoidanceEgoSpeed()); + shift_length, p->max_lateral_jerk, p->min_sharp_avoidance_speed); return std::max(p->min_avoidance_distance, distance_by_jerk); } diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 587afb1fdee57..227094577d005 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -3238,8 +3238,8 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; const auto variable = helper_.getMinimumAvoidanceDistance( helper_.getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin)); - const auto constant = - p->min_prepare_distance + object_parameter.safety_buffer_longitudinal + base_link2front; + const auto constant = p->min_prepare_distance + object_parameter.safety_buffer_longitudinal + + base_link2front + p->stop_buffer; return object.longitudinal - std::clamp(variable + constant, p->stop_min_distance, p->stop_max_distance); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index f9a652a4cec27..9f5917dca58b1 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -176,6 +176,7 @@ AvoidanceModuleManager::AvoidanceModuleManager( std::string ns = "avoidance.stop."; p.stop_min_distance = get_parameter(node, ns + "min_distance"); p.stop_max_distance = get_parameter(node, ns + "max_distance"); + p.stop_buffer = get_parameter(node, ns + "stop_buffer"); } // constraints @@ -294,6 +295,7 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "max_distance", p->stop_max_distance); updateParam(parameters, ns + "min_distance", p->stop_min_distance); + updateParam(parameters, ns + "stop_buffer", p->stop_buffer); } { From 0488f302eacc222d33dc7ed2c357fedce35177d8 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Fri, 30 Jun 2023 17:06:20 +0900 Subject: [PATCH 3/6] feat(avoidance): insert slow down speed Signed-off-by: satoshi-ota --- .../avoidance/avoidance_module.cpp | 60 ++++++++++++++++--- .../src/scene_module/avoidance/manager.cpp | 4 ++ 2 files changed, 55 insertions(+), 9 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 227094577d005..ae7d10f9f6f3b 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -620,6 +620,8 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif return; } + insertPrepareVelocity(path); + switch (data.state) { case AvoidanceState::NOT_AVOID: { break; @@ -631,7 +633,6 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif break; } case AvoidanceState::AVOID_PATH_NOT_READY: { - insertPrepareVelocity(path); insertWaitPoint(parameters_->use_constraints_for_decel, path); break; } @@ -3350,23 +3351,64 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const { const auto & data = avoidance_data_; + // do nothing if there is no avoidance target. if (data.target_objects.empty()) { return; } - if (!!data.stop_target_object) { - if (data.stop_target_object.get().reason != AvoidanceDebugFactor::TOO_LARGE_JERK) { - return; - } + // insert slow down speed only when the avoidance maneuver is not initiated. + if (data.avoiding_now) { + return; } - if (data.avoiding_now) { + // insert slow down speed only when no shift line is approved. + if (!path_shifter_.getShiftLines().empty()) { return; } - const auto decel_distance = helper_.getFeasibleDecelDistance(0.0); - utils::avoidance::insertDecelPoint( - getEgoPosition(), decel_distance, 0.0, shifted_path.path, slow_pose_); + const auto object = data.target_objects.front(); + + // calculate shift length for front object. + const auto & vehicle_width = planner_data_->parameters.vehicle_width; + const auto t = utils::getHighestProbLabel(object.object.classification); + const auto object_parameter = parameters_->object_parameters.at(t); + const auto avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor + + object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; + const auto shift_length = + helper_.getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin); + + // check slow down feasibility + const auto min_avoid_distance = helper_.getMinimumAvoidanceDistance(shift_length); + const auto distance_to_object = object.longitudinal; + const auto remaining_distance = distance_to_object - min_avoid_distance; + const auto decel_distance = + helper_.getFeasibleDecelDistance(parameters_->min_sharp_avoidance_speed); + if (remaining_distance < decel_distance) { + return; + } + + // decide slow down lower limit. + const auto lower_speed = object.avoid_required ? 0.0 : parameters_->min_slow_down_speed; + + // insert slow down speed. + 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 shift_longitudinal_distance = distance_to_object - distance_from_ego; + if (shift_longitudinal_distance < min_avoid_distance) { + break; + } + + // target speed with nominal jerk limits. + const double v_target = PathShifter::calcFeasibleVelocityFromJerk( + shift_length, parameters_->nominal_lateral_jerk, shift_longitudinal_distance); + const double v_original = shifted_path.path.points.at(i).point.longitudinal_velocity_mps; + const double v_insert = std::max(v_target - parameters_->buf_slow_down_speed, lower_speed); + + shifted_path.path.points.at(i).point.longitudinal_velocity_mps = std::min(v_original, v_insert); + } } std::shared_ptr AvoidanceModule::get_debug_msg_array() const diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 9f5917dca58b1..55b06fd66fa87 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -163,6 +163,8 @@ AvoidanceModuleManager::AvoidanceModuleManager( p.min_avoidance_distance = get_parameter(node, ns + "min_avoidance_distance"); p.min_nominal_avoidance_speed = get_parameter(node, ns + "min_nominal_avoidance_speed"); p.min_sharp_avoidance_speed = get_parameter(node, ns + "min_sharp_avoidance_speed"); + p.min_slow_down_speed = get_parameter(node, ns + "min_slow_down_speed"); + p.buf_slow_down_speed = get_parameter(node, ns + "buf_slow_down_speed"); } // yield @@ -289,6 +291,8 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "prepare_time", p->prepare_time); + updateParam(parameters, ns + "min_slow_down_speed", p->min_slow_down_speed); + updateParam(parameters, ns + "buf_slow_down_speed", p->buf_slow_down_speed); } { From 98a75c29a00566835372f6f3c648bf332e2bb8cd Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 4 Jul 2023 19:37:41 +0900 Subject: [PATCH 4/6] fix(avoidance): don't set stoppable=true for objects that ego doesn't have to avoid Signed-off-by: satoshi-ota --- .../src/marker_util/avoidance/debug.cpp | 6 ++++-- .../behavior_path_planner/src/utils/avoidance/utils.cpp | 6 ++++++ 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp b/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp index 10fcec3d8a324..0235bbb57f464 100644 --- a/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp +++ b/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp @@ -120,9 +120,11 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st marker.id = uuidToInt32(object.object.object_id); marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; std::ostringstream string_stream; - string_stream << std::fixed << std::setprecision(2); + string_stream << std::fixed << std::setprecision(2) << std::boolalpha; string_stream << "ratio:" << object.shiftable_ratio << " [-]\n" - << "lateral: " << object.lateral << " [-]\n" + << "lateral:" << object.lateral << " [m]\n" + << "necessity:" << object.avoid_required << " [-]\n" + << "stoppable:" << object.is_stoppable << " [-]\n" << "stop_factor:" << object.to_stop_factor_distance << " [m]\n" << "move_time:" << object.move_time << " [s]\n" << "stop_time:" << object.stop_time << " [s]\n"; diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 5fd1457042693..75c5b6de0a821 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -657,6 +657,12 @@ void fillObjectStoppableJudge( { if (!parameters->use_constraints_for_decel) { object_data.is_stoppable = true; + return; + } + + if (!object_data.avoid_required) { + object_data.is_stoppable = false; + return; } const auto id = object_data.object.object_id; From 2b1781cac03455025abf9b3816245b1385315329 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Wed, 5 Jul 2023 08:31:06 +0900 Subject: [PATCH 5/6] refactor(avoidance): rename unreadable variable name Signed-off-by: satoshi-ota --- .../avoidance/avoidance_module.cpp | 20 +++++------ .../src/utils/avoidance/utils.cpp | 34 +++++++++---------- 2 files changed, 27 insertions(+), 27 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index ae7d10f9f6f3b..8b729e4fd8835 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -343,8 +343,8 @@ ObjectData AvoidanceModule::createObjectData( const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; const auto object_closest_index = findNearestIndex(path_points, object_pose.position); const auto object_closest_pose = path_points.at(object_closest_index).point.pose; - const auto t = utils::getHighestProbLabel(object.classification); - const auto object_parameter = parameters_->object_parameters.at(t); + const auto object_type = utils::getHighestProbLabel(object.classification); + const auto object_parameter = parameters_->object_parameters.at(object_type); ObjectData object_data{}; @@ -571,8 +571,8 @@ void AvoidanceModule::fillDebugData(const AvoidancePlanningData & data, DebugDat } const auto o_front = data.stop_target_object.get(); - const auto t = utils::getHighestProbLabel(o_front.object.classification); - const auto object_parameter = parameters_->object_parameters.at(t); + const auto object_type = utils::getHighestProbLabel(o_front.object.classification); + const auto object_parameter = parameters_->object_parameters.at(object_type); const auto & base_link2front = planner_data_->parameters.base_link2front; const auto & vehicle_width = planner_data_->parameters.vehicle_width; @@ -861,8 +861,8 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( const auto nominal_return_distance = helper_.getNominalAvoidanceDistance(return_shift); // use each object param - const auto t = utils::getHighestProbLabel(o.object.classification); - const auto object_parameter = parameters_->object_parameters.at(t); + const auto object_type = utils::getHighestProbLabel(o.object.classification); + const auto object_parameter = parameters_->object_parameters.at(object_type); /** * Is there enough distance from ego to object for avoidance? @@ -3232,8 +3232,8 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const // D4: o_front.longitudinal // D5: base_link2front - const auto t = utils::getHighestProbLabel(object.object.classification); - const auto object_parameter = parameters_->object_parameters.at(t); + const auto object_type = utils::getHighestProbLabel(object.object.classification); + const auto object_parameter = parameters_->object_parameters.at(object_type); const auto avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor + object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; @@ -3370,8 +3370,8 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const // calculate shift length for front object. const auto & vehicle_width = planner_data_->parameters.vehicle_width; - const auto t = utils::getHighestProbLabel(object.object.classification); - const auto object_parameter = parameters_->object_parameters.at(t); + const auto object_type = utils::getHighestProbLabel(object.object.classification); + const auto object_parameter = parameters_->object_parameters.at(object_type); const auto avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor + object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; const auto shift_length = diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 75c5b6de0a821..d92adaae62c97 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -101,28 +101,28 @@ bool isOnRight(const ObjectData & obj) bool isTargetObjectType( const PredictedObject & object, const std::shared_ptr & parameters) { - const auto t = utils::getHighestProbLabel(object.classification); + const auto object_type = utils::getHighestProbLabel(object.classification); - if (parameters->object_parameters.count(t) == 0) { + if (parameters->object_parameters.count(object_type) == 0) { return false; } - return parameters->object_parameters.at(t).is_target; + return parameters->object_parameters.at(object_type).is_target; } bool isVehicleTypeObject(const ObjectData & object) { - const auto t = utils::getHighestProbLabel(object.object.classification); + const auto object_type = utils::getHighestProbLabel(object.object.classification); - if (t == ObjectClassification::UNKNOWN) { + if (object_type == ObjectClassification::UNKNOWN) { return false; } - if (t == ObjectClassification::PEDESTRIAN) { + if (object_type == ObjectClassification::PEDESTRIAN) { return false; } - if (t == ObjectClassification::BICYCLE) { + if (object_type == ObjectClassification::BICYCLE) { return false; } @@ -422,8 +422,8 @@ std::vector generateObstaclePolygonsForDrivableArea( continue; } - const auto t = utils::getHighestProbLabel(object.object.classification); - const auto object_parameter = parameters->object_parameters.at(t); + const auto object_type = utils::getHighestProbLabel(object.object.classification); + const auto object_parameter = parameters->object_parameters.at(object_type); // generate obstacle polygon const double diff_poly_buffer = @@ -535,8 +535,8 @@ void fillObjectEnvelopePolygon( { using boost::geometry::within; - const auto t = utils::getHighestProbLabel(object_data.object.classification); - const auto object_parameter = parameters->object_parameters.at(t); + const auto object_type = utils::getHighestProbLabel(object_data.object.classification); + const auto object_parameter = parameters->object_parameters.at(object_type); const auto & envelope_buffer_margin = object_parameter.envelope_buffer_margin * object_data.distance_factor; @@ -567,8 +567,8 @@ void fillObjectMovingTime( ObjectData & object_data, ObjectDataArray & stopped_objects, const std::shared_ptr & parameters) { - const auto t = utils::getHighestProbLabel(object_data.object.classification); - const auto object_parameter = parameters->object_parameters.at(t); + const auto object_type = utils::getHighestProbLabel(object_data.object.classification); + const auto object_parameter = parameters->object_parameters.at(object_type); const auto & object_vel = object_data.object.kinematics.initial_twist_with_covariance.twist.linear.x; @@ -618,8 +618,8 @@ void fillAvoidanceNecessity( ObjectData & object_data, const ObjectDataArray & registered_objects, const double vehicle_width, const std::shared_ptr & parameters) { - const auto t = utils::getHighestProbLabel(object_data.object.classification); - const auto object_parameter = parameters->object_parameters.at(t); + const auto object_type = utils::getHighestProbLabel(object_data.object.classification); + const auto object_parameter = parameters->object_parameters.at(object_type); const auto safety_margin = 0.5 * vehicle_width + object_parameter.safety_buffer_lateral * object_data.distance_factor; @@ -802,8 +802,8 @@ void filterTargetObjects( const auto object_closest_index = findNearestIndex(path_points, object_pose.position); const auto object_closest_pose = path_points.at(object_closest_index).point.pose; - const auto t = utils::getHighestProbLabel(o.object.classification); - const auto object_parameter = parameters->object_parameters.at(t); + const auto object_type = utils::getHighestProbLabel(o.object.classification); + const auto object_parameter = parameters->object_parameters.at(object_type); if (!isTargetObjectType(o.object, parameters)) { o.reason = AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE; From d5eafc12f1e6b597eea0f5284004e8fe65920c26 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Wed, 5 Jul 2023 08:47:57 +0900 Subject: [PATCH 6/6] docs(avoidance): add new parameter description Signed-off-by: satoshi-ota --- .../behavior_path_planner_avoidance_design.md | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md index b290a1efa90da..e1de6b5a59314 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md @@ -677,14 +677,16 @@ namespace: `avoidance.avoidance.lateral.` namespace: `avoidance.avoidance.longitudinal.` -| Name | Unit | Type | Description | Default value | -| :----------------------------------- | :---- | :----- | :----------------------------------------------------------------------------------------------------- | :------------ | -| prepare_time | [s] | double | Avoidance shift starts from point ahead of this time x ego_speed to avoid sudden path change. | 2.0 | -| longitudinal_collision_safety_buffer | [s] | double | Longitudinal collision buffer between target object and shift line. | 0.0 | -| min_prepare_distance | [m] | double | Minimum distance for "prepare_time" x "ego_speed". | 1.0 | -| min_avoidance_distance | [m] | double | Minimum distance of avoidance path (i.e. this distance is needed even if its lateral jerk is very low) | 10.0 | -| min_nominal_avoidance_speed | [m/s] | double | Minimum speed for jerk calculation in a nominal situation (\*1). | 7.0 | -| min_sharp_avoidance_speed | [m/s] | double | Minimum speed for jerk calculation in a sharp situation (\*1). | 1.0 | +| Name | Unit | Type | Description | Default value | +| :----------------------------------- | :---- | :----- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------- | +| prepare_time | [s] | double | Avoidance shift starts from point ahead of this time x ego_speed to avoid sudden path change. | 2.0 | +| longitudinal_collision_safety_buffer | [s] | double | Longitudinal collision buffer between target object and shift line. | 0.0 | +| min_prepare_distance | [m] | double | Minimum distance for "prepare_time" x "ego_speed". | 1.0 | +| min_avoidance_distance | [m] | double | Minimum distance of avoidance path (i.e. this distance is needed even if its lateral jerk is very low) | 10.0 | +| min_nominal_avoidance_speed | [m/s] | double | Minimum speed for jerk calculation in a nominal situation (\*1). | 7.0 | +| min_sharp_avoidance_speed | [m/s] | double | Minimum speed for jerk calculation in a sharp situation (\*1). | 1.0 | +| min_slow_down_speed | [m/s] | double | Minimum slow speed for avoidance prepare section. | 1.38 (5km/h) | +| buf_slow_down_speed | [m/s] | double | Buffer for controller tracking error. Basically, vehicle always cannot follow velocity profile precisely. Therefore, the module inserts lower speed than target speed that satisfies conditions to avoid object within accel/jerk constraints so that the avoidance path always can be output even if the current speed is a little bit higher than target speed. | 0.57 (2.0km/h) | ### Yield maneuver parameters @@ -702,6 +704,7 @@ namespace: `avoidance.stop.` | :----------- | :--- | :----- | :---------------------------------------------------------------------------------------------------- | :------------ | | min_distance | [m] | double | Minimum stop distance in the situation where avoidance maneuver is not approved or in yield maneuver. | 10.0 | | max_distance | [m] | double | Maximum stop distance in the situation where avoidance maneuver is not approved or in yield maneuver. | 20.0 | +| stop_buffer | [m] | double | Buffer distance in the situation where avoidance maneuver is not approved or in yield maneuver. | 1.0 | ### Constraints parameters