Skip to content

Commit

Permalink
refactor(start planner): refactor for clarity, readability and maybe …
Browse files Browse the repository at this point in the history 
…performance (autowarefoundation#6655)

* refactor plane method

Signed-off-by: Daniel Sanchez <[email protected]>

* delete unused method

Signed-off-by: Daniel Sanchez <[email protected]>

* skip unnecessary loop

Signed-off-by: Daniel Sanchez <[email protected]>

---------

Signed-off-by: Daniel Sanchez <[email protected]>
  • Loading branch information
@danielsanchezaran
danielsanchezaran authored Mar 27, 2024
1 parent e84aaee commit dd7c252
Show file tree
Hide file tree
Showing 2 changed files with 37 additions and 64 deletions.
1 change: 0 additions & 1 deletion ..._start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -297,7 +297,6 @@ class StartPlannerModule : public SceneModuleInterface
const std::vector<PoseWithVelocityStamped> & ego_predicted_path) const;
bool isSafePath() const;
void setDrivableAreaInfo(BehaviorModuleOutput & output) const;
lanelet::ConstLanelets createDepartureCheckLanes() const;

// check if the goal is located behind the ego in the same route segment.
bool isGoalBehindOfEgoInSameRouteSegment() const;
Expand Down
100 changes: 37 additions & 63 deletions planning/behavior_path_start_planner_module/src/start_planner_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -508,47 +508,45 @@ BehaviorModuleOutput StartPlannerModule::plan()
return output;
}

PathWithLaneId path;
const auto path = std::invoke([&]() {
if (!status_.driving_forward && !status_.backward_driving_complete) {
return status_.backward_path;
}

// Check if backward motion is finished
if (status_.driving_forward || status_.backward_driving_complete) {
// Increment path index if the current path is finished
if (hasFinishedCurrentPath()) {
RCLCPP_INFO(getLogger(), "Increment path index");
incrementPathIndex();
}

if (!status_.is_safe_dynamic_objects && !isWaitingApproval() && !status_.stop_pose) {
if (isWaitingApproval()) return getCurrentPath();

if (status_.stop_pose) {
// Delete stop point if conditions are met
if (status_.is_safe_dynamic_objects && isStopped()) {
status_.stop_pose = std::nullopt;
}
stop_pose_ = status_.stop_pose;
return *status_.prev_stop_path_after_approval;
}

if (!status_.is_safe_dynamic_objects) {
auto current_path = getCurrentPath();
const auto stop_path =
behavior_path_planner::utils::parking_departure::generateFeasibleStopPath(
current_path, planner_data_, stop_pose_, parameters_->maximum_deceleration_for_stop,
parameters_->maximum_jerk_for_stop);

if (!stop_path.has_value()) return current_path;
// Insert stop point in the path if needed
if (stop_path) {
RCLCPP_ERROR_THROTTLE(
getLogger(), *clock_, 5000, "Insert stop point in the path because of dynamic objects");
path = *stop_path;
status_.prev_stop_path_after_approval = std::make_shared<PathWithLaneId>(path);
status_.stop_pose = stop_pose_;
} else {
path = current_path;
}
} else if (!isWaitingApproval() && status_.stop_pose) {
// Delete stop point if conditions are met
if (status_.is_safe_dynamic_objects && isStopped()) {
status_.stop_pose = std::nullopt;
path = getCurrentPath();
}
path = *status_.prev_stop_path_after_approval;
stop_pose_ = status_.stop_pose;
} else {
path = getCurrentPath();
RCLCPP_ERROR_THROTTLE(
getLogger(), *clock_, 5000, "Insert stop point in the path because of dynamic objects");
status_.prev_stop_path_after_approval = std::make_shared<PathWithLaneId>(stop_path.value());
status_.stop_pose = stop_pose_;
return stop_path.value();
}
} else {
path = status_.backward_path;
}
return getCurrentPath();
});

output.path = path;
output.reference_path = getPreviousModuleOutput().reference_path;
Expand Down Expand Up @@ -768,8 +766,9 @@ bool StartPlannerModule::findPullOutPath(
const Pose & refined_start_pose, const Pose & goal_pose, const double collision_check_margin)
{
// if start_pose_candidate is far from refined_start_pose, backward driving is necessary
constexpr double epsilon = 0.01;
const bool backward_is_unnecessary =
tier4_autoware_utils::calcDistance2d(start_pose_candidate, refined_start_pose) < 0.01;
tier4_autoware_utils::calcDistance2d(start_pose_candidate, refined_start_pose) < epsilon;

planner->setCollisionCheckMargin(collision_check_margin);
planner->setPlannerData(planner_data_);
Expand Down Expand Up @@ -856,26 +855,24 @@ lanelet::ConstLanelets StartPlannerModule::getPathRoadLanes(const PathWithLaneId
const auto & lanelet_layer = route_handler->getLaneletMapPtr()->laneletLayer;

std::vector<lanelet::Id> lane_ids;
lanelet::ConstLanelets path_lanes;

for (const auto & p : path.points) {
for (const auto & id : p.lane_ids) {
if (id == lanelet::InvalId) {
continue;
}
if (route_handler->isShoulderLanelet(lanelet_layer.get(id))) {
const auto lanelet = lanelet_layer.get(id);
if (route_handler->isShoulderLanelet(lanelet)) {
continue;
}
if (std::find(lane_ids.begin(), lane_ids.end(), id) == lane_ids.end()) {
lane_ids.push_back(id);
path_lanes.push_back(lanelet);
}
}
}

lanelet::ConstLanelets path_lanes;
path_lanes.reserve(lane_ids.size());
for (const auto & id : lane_ids) {
path_lanes.push_back(lanelet_layer.get(id));
}

return path_lanes;
}

Expand Down Expand Up @@ -954,11 +951,12 @@ void StartPlannerModule::updatePullOutStatus()

if (hasFinishedBackwardDriving()) {
updateStatusAfterBackwardDriving();
} else {
status_.backward_path = start_planner_utils::getBackwardPath(
*route_handler, pull_out_lanes, current_pose, status_.pull_out_start_pose,
parameters_->backward_velocity);
return;
}
status_.backward_path = start_planner_utils::getBackwardPath(
*route_handler, pull_out_lanes, current_pose, status_.pull_out_start_pose,
parameters_->backward_velocity);
return;
}

void StartPlannerModule::updateStatusAfterBackwardDriving()
Expand Down Expand Up @@ -1047,7 +1045,7 @@ std::vector<Pose> StartPlannerModule::searchPullOutStartPoseCandidates(
if (distance_from_lane_end < parameters_->ignore_distance_from_lane_end) {
RCLCPP_WARN_THROTTLE(
getLogger(), *clock_, 5000,
"the ego is too close to the lane end, so needs backward driving");
"the ego vehicle is too close to the lane end, so backwards driving is necessary");
continue;
}

Expand Down Expand Up @@ -1408,30 +1406,6 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons
}
}

lanelet::ConstLanelets StartPlannerModule::createDepartureCheckLanes() const
{
const double backward_path_length =
planner_data_->parameters.backward_path_length + parameters_->max_back_distance;
const auto pull_out_lanes =
start_planner_utils::getPullOutLanes(planner_data_, backward_path_length);
if (pull_out_lanes.empty()) {
return lanelet::ConstLanelets{};
}
const auto road_lanes = utils::getExtendedCurrentLanes(
planner_data_, backward_path_length, std::numeric_limits<double>::max(),
/*forward_only_in_route*/ true);
// extract shoulder lanes from pull out lanes
lanelet::ConstLanelets shoulder_lanes;
std::copy_if(
pull_out_lanes.begin(), pull_out_lanes.end(), std::back_inserter(shoulder_lanes),
[this](const auto & pull_out_lane) {
return planner_data_->route_handler->isShoulderLanelet(pull_out_lane);
});
const auto departure_check_lanes = utils::transformToLanelets(
utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes));
return departure_check_lanes;
}

void StartPlannerModule::setDebugData()
{
using lanelet::visualization::laneletsAsTriangleMarkerArray;
Expand Down

0 comments on commit dd7c252

Please sign in to comment.