Skip to content

Commit

Permalink
fixup! refactor(avoidance): refactor decel profile generation logic
Browse files Browse the repository at this point in the history
  • Loading branch information
@satoshi-ota
satoshi-ota committed Dec 7, 2023
1 parent 443f656 commit 8fda45d
Show file tree
Hide file tree
Showing 2 changed files with 13 additions and 73 deletions.
8 changes: 7 additions & 1 deletion 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,8 +243,14 @@ class AvoidanceModule : public SceneModuleInterface
*/
void insertYieldVelocity(ShiftedPath & shifted_path) const;

/**
* @brief insert deceleration profile from shift length.
* @param shift length.
* @param decel section length.
* @param lower speed limit.
*/
void insertDecelProfile(
PathWithLaneId & path, const double shift_length, const double to_stop_line,
PathWithLaneId & path, const double shift_length, const double decel_section_length,
const double lower_speed) const;

/**
Expand Down
78 changes: 6 additions & 72 deletions planning/behavior_path_avoidance_module/src/scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1513,37 +1513,8 @@ 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;
// }

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 @@ -1696,10 +1667,6 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const
const auto distance_to_object = object.longitudinal;
const auto remaining_distance = distance_to_object - min_avoid_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;
Expand All @@ -1713,52 +1680,18 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const
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);
}

Check notice on line 1684 in planning/behavior_path_avoidance_module/src/scene.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Complex Method 

AvoidanceModule::insertPrepareVelocity decreases in cyclomatic complexity from 12 to 9, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

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

// insert slow down speed.
const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk(
shift_length, helper_->getLateralMinJerkLimit(), to_stop_line);
shift_length, helper_->getLateralMinJerkLimit(), decel_section_length);
if (current_target_velocity < getEgoSpeed() + parameters_->buf_slow_down_speed) {
utils::avoidance::insertDecelPoint(
getEgoPosition(), decel_distance, lower_speed, path, slow_pose_);
Expand All @@ -1770,7 +1703,7 @@ void AvoidanceModule::insertDecelProfile(
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 = to_stop_line - distance_from_ego;
const auto shift_longitudinal_distance = decel_section_length - distance_from_ego;
if (shift_longitudinal_distance < 0.0) {
break;
}
Expand All @@ -1784,7 +1717,8 @@ void AvoidanceModule::insertDecelProfile(
path.points.at(i).point.longitudinal_velocity_mps = std::min(v_original, v_insert);
}

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

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

0 comments on commit 8fda45d

Please sign in to comment.