Skip to content

Commit

Permalink
refactor(avoidance): refactor decel profile generation logic
Browse files Browse the repository at this point in the history 
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
  • Loading branch information
@satoshi-ota
satoshi-ota committed Dec 7, 2023
1 parent 703905e commit 443f656
Show file tree
Hide file tree
Showing 2 changed files with 102 additions and 43 deletions.
4 changes: 4 additions & 0 deletions planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -243,6 +243,10 @@ class AvoidanceModule : public SceneModuleInterface
*/
void insertYieldVelocity(ShiftedPath & shifted_path) const;

void insertDecelProfile(
PathWithLaneId & path, const double shift_length, const double to_stop_line,
const double lower_speed) const;

/**
* @brief calculate stop distance based on object's overhang.
* @param stop distance.
Expand Down
141 changes: 98 additions & 43 deletions planning/behavior_path_avoidance_module/src/scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1513,33 +1513,37 @@ void AvoidanceModule::insertReturnDeadLine(
utils::avoidance::insertDecelPoint(
getEgoPosition(), to_stop_line - parameters_->stop_buffer, 0.0, shifted_path.path, stop_pose_);

// insert slow down speed.
const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk(
shift_length, helper_->getLateralMinJerkLimit(), to_stop_line);
if (current_target_velocity < getEgoSpeed()) {
RCLCPP_DEBUG(getLogger(), "current velocity exceeds target slow down speed.");
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 shift_longitudinal_distance = to_stop_line - distance_from_ego;
if (shift_longitudinal_distance < 0.0) {
break;
}

// target speed with nominal jerk limits.
const double v_target = PathShifter::calcFeasibleVelocityFromJerk(
shift_length, helper_->getLateralMinJerkLimit(), 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, parameters_->min_slow_down_speed);

shifted_path.path.points.at(i).point.longitudinal_velocity_mps = std::min(v_original, v_insert);
}
// // insert slow down speed.
// const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk(
// shift_length, helper_->getLateralMinJerkLimit(), to_stop_line);
// if (current_target_velocity < getEgoSpeed()) {
// RCLCPP_DEBUG(getLogger(), "current velocity exceeds target slow down speed.");
// return;
// }

insertDecelProfile(
shifted_path.path, shift_length, to_stop_line, parameters_->min_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 = to_stop_line - distance_from_ego;
// if (shift_longitudinal_distance < 0.0) {
// break;
// }

// // target speed with nominal jerk limits.
// const double v_target = PathShifter::calcFeasibleVelocityFromJerk(
// shift_length, helper_->getLateralMinJerkLimit(), 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, parameters_->min_slow_down_speed);

// shifted_path.path.points.at(i).point.longitudinal_velocity_mps = std::min(v_original,
// v_insert);
// }
}

void AvoidanceModule::insertWaitPoint(
Expand Down Expand Up @@ -1691,45 +1695,96 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const
const auto min_avoid_distance = helper_->getMinAvoidanceDistance(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_->velocity_map.front());
if (remaining_distance < decel_distance) {
const auto lower_speed = object.avoid_required ? 0.0 : parameters_->min_slow_down_speed;
// const auto decel_distance = helper_->getFeasibleDecelDistance(lower_speed);
// if (remaining_distance < decel_distance) {
// return;
// }
if (distance_to_object < min_avoid_distance) {
RCLCPP_DEBUG(getLogger(), "less than minimum avoidance distance.");
return;
}

// decide slow down lower limit.
const auto lower_speed = object.avoid_required ? 0.0 : parameters_->min_slow_down_speed;
// If the stop distance is not enough for comfortable stop, don't insert wait point.
const auto is_comfortable_stop =
helper_->getFeasibleDecelDistance(lower_speed) < remaining_distance;
if (!is_comfortable_stop) {
RCLCPP_DEBUG(getLogger(), "stop distance is not enough.");
return;
}

// // insert slow down speed.
// constexpr double BUFFER = 1.0; // [m/s]
// const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk(
// shift_length, helper_->getLateralMinJerkLimit(), distance_to_object);
// if (current_target_velocity + BUFFER < getEgoSpeed()) {
// utils::avoidance::insertDecelPoint(
// getEgoPosition(), decel_distance, parameters_->velocity_map.front(), shifted_path.path,
// slow_pose_);
// return;
// }

insertDecelProfile(shifted_path.path, shift_length, distance_to_object, lower_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, helper_->getLateralMinJerkLimit(), 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);
// }

// slow_pose_ = motion_utils::calcLongitudinalOffsetPose(
// shifted_path.path.points, start_idx, distance_to_object);
}

void AvoidanceModule::insertDecelProfile(
PathWithLaneId & path, const double shift_length, const double to_stop_line,
const double lower_speed) const
{
const auto decel_distance = helper_->getFeasibleDecelDistance(lower_speed);

// insert slow down speed.
const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk(
shift_length, helper_->getLateralMinJerkLimit(), distance_to_object);
shift_length, helper_->getLateralMinJerkLimit(), to_stop_line);
if (current_target_velocity < getEgoSpeed() + parameters_->buf_slow_down_speed) {
utils::avoidance::insertDecelPoint(
getEgoPosition(), decel_distance, parameters_->velocity_map.front(), shifted_path.path,
slow_pose_);
getEgoPosition(), decel_distance, lower_speed, path, slow_pose_);
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);
const auto start_idx = planner_data_->findEgoIndex(path.points);
for (size_t i = start_idx; i < path.points.size(); ++i) {
const auto distance_from_ego = calcSignedArcLength(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) {
const auto shift_longitudinal_distance = to_stop_line - distance_from_ego;
if (shift_longitudinal_distance < 0.0) {
break;
}

// target speed with nominal jerk limits.
const double v_target = PathShifter::calcFeasibleVelocityFromJerk(
shift_length, helper_->getLateralMinJerkLimit(), shift_longitudinal_distance);
const double v_original = shifted_path.path.points.at(i).point.longitudinal_velocity_mps;
const double v_original = 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);
path.points.at(i).point.longitudinal_velocity_mps = std::min(v_original, v_insert);
}

slow_pose_ = motion_utils::calcLongitudinalOffsetPose(
shifted_path.path.points, start_idx, distance_to_object);
slow_pose_ = motion_utils::calcLongitudinalOffsetPose(path.points, start_idx, to_stop_line);
}

std::shared_ptr<AvoidanceDebugMsgArray> AvoidanceModule::get_debug_msg_array() const
Expand Down

0 comments on commit 443f656

Please sign in to comment.