Skip to content

Commit

Permalink
move dead line process
Browse files Browse the repository at this point in the history
Signed-off-by: kosuke55 <[email protected]>
  • Loading branch information
kosuke55 committed Jan 13, 2024
1 parent dc5767b commit dc39d5a
Showing 1 changed file with 13 additions and 13 deletions.
26 changes: 13 additions & 13 deletions planning/behavior_path_avoidance_module/src/utils.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -2124,19 +2124,6 @@ double calcDistanceToAvoidStartLine(
}
}

// dead line for goal
if (
utils::isAllowedGoalModification(planner_data->route_handler) &&
parameters->enable_dead_line_for_goal) {
if (planner_data->route_handler->isInGoalRouteSection(lanelets.back())) {
const auto & ego_pos = planner_data->self_odometry->pose.pose.position;
const auto to_goal_distance =
calcSignedArcLength(path.points, ego_pos, path.points.size() - 1);
distance_to_return_dead_line = std::min(
distance_to_return_dead_line, to_goal_distance - parameters->dead_line_buffer_for_goal);
}
}

return distance_to_return_dead_line;
}

Expand All @@ -2161,6 +2148,19 @@ double calcDistanceToReturnDeadLine(
}
}

// dead line for goal
if (
utils::isAllowedGoalModification(planner_data->route_handler) &&
parameters->enable_dead_line_for_goal) {
if (planner_data->route_handler->isInGoalRouteSection(lanelets.back())) {
const auto & ego_pos = planner_data->self_odometry->pose.pose.position;
const auto to_goal_distance =
calcSignedArcLength(path.points, ego_pos, path.points.size() - 1);
distance_to_return_dead_line = std::min(
distance_to_return_dead_line, to_goal_distance - parameters->dead_line_buffer_for_goal);
}
}

return distance_to_return_dead_line;
}

Expand Down

0 comments on commit dc39d5a

Please sign in to comment.