Skip to content

Commit

Permalink
remove in_terminal condition
Browse files Browse the repository at this point in the history
Signed-off-by: Takamasa Horibe <[email protected]>
  • Loading branch information
TakaHoribe committed Oct 12, 2023
1 parent 91d80be commit 6e36b47
Showing 1 changed file with 64 additions and 67 deletions.
131 changes: 64 additions & 67 deletions planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -206,80 +206,77 @@ void NormalLaneChange::insertStopPoint(

const double stop_point_buffer = getCommonParam().backward_length_buffer_for_end_of_lane;
const auto target_objects = getTargetObjects(status_.current_lanes, status_.target_lanes);
const bool is_in_terminal_section = lanelet::utils::isInLanelet(getEgoPose(), lanelets.back()) ||
distance_to_terminal < lane_change_buffer;
double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer;
if (is_in_terminal_section) {
// calculate minimum distance from path front to the stationary object on the ego lane.
const auto distance_to_ego_lane_obj = [&]() -> double {
double distance_to_obj = distance_to_terminal;
const double distance_to_ego =
utils::getSignedDistance(path.points.front().point.pose, getEgoPose(), lanelets);

for (const auto & object : target_objects.current_lane) {
// check if stationary
const auto obj_v = std::abs(object.initial_twist.twist.linear.x);
if (obj_v > lane_change_parameters_->stop_velocity_threshold) {
continue;
}

// calculate distance from path front to the stationary object polygon on the ego lane.
const auto polygon =
tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape).outer();
for (const auto & polygon_p : polygon) {
const auto p_fp = tier4_autoware_utils::toMsg(polygon_p.to_3d());
const auto lateral_fp = motion_utils::calcLateralOffset(path.points, p_fp);
// calculate minimum distance from path front to the stationary object on the ego lane.
const auto distance_to_ego_lane_obj = [&]() -> double {
double distance_to_obj = distance_to_terminal;
const double distance_to_ego =
utils::getSignedDistance(path.points.front().point.pose, getEgoPose(), lanelets);

// ignore if the point is around the ego path
if (std::abs(lateral_fp) > planner_data_->parameters.vehicle_width) {
continue;
}
for (const auto & object : target_objects.current_lane) {
// check if stationary
const auto obj_v = std::abs(object.initial_twist.twist.linear.x);
if (obj_v > lane_change_parameters_->stop_velocity_threshold) {
continue;
}

const double current_distance_to_obj = calcSignedArcLength(path.points, 0, p_fp);
// calculate distance from path front to the stationary object polygon on the ego lane.
const auto polygon =
tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape).outer();
for (const auto & polygon_p : polygon) {
const auto p_fp = tier4_autoware_utils::toMsg(polygon_p.to_3d());
const auto lateral_fp = motion_utils::calcLateralOffset(path.points, p_fp);

// ignore backward object
if (current_distance_to_obj < distance_to_ego) {
continue;
}
distance_to_obj = std::min(distance_to_obj, current_distance_to_obj);
// ignore if the point is around the ego path
if (std::abs(lateral_fp) > planner_data_->parameters.vehicle_width) {
continue;
}

const double current_distance_to_obj = calcSignedArcLength(path.points, 0, p_fp);

// ignore backward object
if (current_distance_to_obj < distance_to_ego) {
continue;
}
distance_to_obj = std::min(distance_to_obj, current_distance_to_obj);
}
return distance_to_obj;
}();

// Need to stop before blocking obstacle
if (distance_to_ego_lane_obj < distance_to_terminal) {
// consider rss distance when the LC need to avoid obstacles
const auto rss_dist = calcRssDistance(
0.0, planner_data_->parameters.minimum_lane_changing_velocity,
lane_change_parameters_->rss_params);

const auto stopping_distance_for_obj =
distance_to_ego_lane_obj - lane_change_buffer - stop_point_buffer - rss_dist;

// If the target lane in the lane change section is blocked by a stationary obstacle, there
// is no reason for stopping with a lane change margin. Instead, stop right behind the
// obstacle.
// ----------------------------------------------------------
// [obj]>
// ----------------------------------------------------------
// [ego]> | <--- lane change margin ---> [obj]>
// ----------------------------------------------------------
const bool has_blocking_target_lane_obj = std::any_of(
target_objects.target_lane.begin(), target_objects.target_lane.end(), [&](const auto & o) {
const auto v = std::abs(o.initial_twist.twist.linear.x);
if (v > lane_change_parameters_->stop_velocity_threshold) {
return false;
}
const double distance_to_target_lane_obj = utils::getSignedDistance(
path.points.front().point.pose, o.initial_pose.pose, status_.target_lanes);
return stopping_distance_for_obj < distance_to_target_lane_obj &&
distance_to_target_lane_obj < distance_to_ego_lane_obj;
});

if (!has_blocking_target_lane_obj) {
stopping_distance = stopping_distance_for_obj;
}
}
return distance_to_obj;
}();

// Need to stop before blocking obstacle
if (distance_to_ego_lane_obj < distance_to_terminal) {
// consider rss distance when the LC need to avoid obstacles
const auto rss_dist = calcRssDistance(
0.0, planner_data_->parameters.minimum_lane_changing_velocity,
lane_change_parameters_->rss_params);

const auto stopping_distance_for_obj =
distance_to_ego_lane_obj - lane_change_buffer - stop_point_buffer - rss_dist;

// If the target lane in the lane change section is blocked by a stationary obstacle, there
// is no reason for stopping with a lane change margin. Instead, stop right behind the
// obstacle.
// ----------------------------------------------------------
// [obj]>
// ----------------------------------------------------------
// [ego]> | <--- lane change margin ---> [obj]>
// ----------------------------------------------------------
const bool has_blocking_target_lane_obj = std::any_of(
target_objects.target_lane.begin(), target_objects.target_lane.end(), [&](const auto & o) {
const auto v = std::abs(o.initial_twist.twist.linear.x);
if (v > lane_change_parameters_->stop_velocity_threshold) {
return false;
}
const double distance_to_target_lane_obj = utils::getSignedDistance(
path.points.front().point.pose, o.initial_pose.pose, status_.target_lanes);
return stopping_distance_for_obj < distance_to_target_lane_obj &&
distance_to_target_lane_obj < distance_to_ego_lane_obj;
});

if (!has_blocking_target_lane_obj) {
stopping_distance = stopping_distance_for_obj;
}

Check notice on line 280 in planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Complex Method

NormalLaneChange::insertStopPoint already has high cyclomatic complexity, and now it increases in Lines of Code from 74 to 75. 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.

Check notice on line 280 in planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Bumpy Road Ahead

NormalLaneChange::insertStopPoint increases from 2 to 3 logical blocks with deeply nested code, threshold is one single block per function. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.

Check notice on line 280 in planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Deep, Nested Complexity

NormalLaneChange::insertStopPoint is no longer above the threshold for nested complexity depth. This function contains deeply nested logic such as if statements and/or loops. The deeper the nesting, the lower the code health.
}

Expand Down

0 comments on commit 6e36b47

Please sign in to comment.