Skip to content

Commit

Permalink
refactor(goal_planner): refactor select path
Browse files Browse the repository at this point in the history 
Signed-off-by: kosuke55 <[email protected]>
  • Loading branch information
@kosuke55
kosuke55 committed Nov 13, 2023
1 parent 5b18bf3 commit 991cee4
Show file tree
Hide file tree
Showing 2 changed files with 100 additions and 71 deletions.
36 changes: 30 additions & 6 deletions ...h_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -168,20 +168,39 @@ class ThreadSafeData
return false;
}

void set_pull_over_path(const PullOverPath & value)
void set_pull_over_path(const PullOverPath & path)
{
const std::lock_guard<std::recursive_mutex> lock(mutex_);
pull_over_path_ = std::make_shared<PullOverPath>(value);
pull_over_path_ = std::make_shared<PullOverPath>(path);
if (path.type != PullOverPlannerType::NONE && path.type != PullOverPlannerType::FREESPACE) {
lane_parking_pull_over_path_ = std::make_shared<PullOverPath>(path);
}

last_path_update_time_ = clock_->now();
}

void set_pull_over_path(const std::shared_ptr<PullOverPath> & value)
void set_pull_over_path(const std::shared_ptr<PullOverPath> & path)
{
const std::lock_guard<std::recursive_mutex> lock(mutex_);
pull_over_path_ = value;
pull_over_path_ = path;
if (path->type != PullOverPlannerType::NONE && path->type != PullOverPlannerType::FREESPACE) {
lane_parking_pull_over_path_ = path;
}
last_path_update_time_ = clock_->now();
}

template <typename... Args>
void set(Args... args)
{
std::lock_guard<std::recursive_mutex> lock(mutex_);
(..., set(args));
}
void set(const GoalCandidates & arg) { set_goal_candidates(arg); }
void set(const std::vector<PullOverPath> & arg) { set_pull_over_path_candidates(arg); }
void set(const std::shared_ptr<PullOverPath> & arg) { set_pull_over_path(arg); }
void set(const PullOverPath & arg) { set_pull_over_path(arg); }
void set(const GoalCandidate & arg) { set_modified_goal_pose(arg); }

void clearPullOverPath()
{
const std::lock_guard<std::recursive_mutex> lock(mutex_);
Expand Down Expand Up @@ -221,6 +240,7 @@ class ThreadSafeData
}

DEFINE_GETTER_WITH_MUTEX(std::shared_ptr<PullOverPath>, pull_over_path)
DEFINE_GETTER_WITH_MUTEX(std::shared_ptr<PullOverPath>, lane_parking_pull_over_path)
DEFINE_GETTER_WITH_MUTEX(std::optional<rclcpp::Time>, last_path_update_time)
DEFINE_GETTER_WITH_MUTEX(std::optional<rclcpp::Time>, last_path_idx_increment_time)

Expand All @@ -231,6 +251,7 @@ class ThreadSafeData

private:
std::shared_ptr<PullOverPath> pull_over_path_{nullptr};
std::shared_ptr<PullOverPath> lane_parking_pull_over_path_{nullptr};
std::vector<PullOverPath> pull_over_path_candidates_;
GoalCandidates goal_candidates_{};
std::optional<GoalCandidate> modified_goal_pose_;
Expand Down Expand Up @@ -375,6 +396,7 @@ class GoalPlannerModule : public SceneModuleInterface
void generateGoalCandidates();

// stop or decelerate
void deceleratePath(PullOverPath & pull_over_path) const;
void decelerateForTurnSignal(const Pose & stop_pose, PathWithLaneId & path) const;
void decelerateBeforeSearchStart(
const Pose & search_start_offset_pose, PathWithLaneId & path) const;
Expand Down Expand Up @@ -409,13 +431,15 @@ class GoalPlannerModule : public SceneModuleInterface

// freespace parking
bool planFreespacePath();
void returnToLaneParking();
bool canReturnToLaneParking();

// plan pull over path
BehaviorModuleOutput planRunning();
BehaviorModuleOutput planPullOver();
BehaviorModuleOutput planPullOverAsCandidate();
void selectSafePullOverPath();
std::optional<std::pair<PullOverPath, GoalCandidate>> selectPullOverPath(
const std::vector<PullOverPath> & pull_over_path_candidates,
const GoalCandidates & goal_candidates) const;
std::vector<PullOverPath> sortPullOverPathCandidatesByGoalPriority(
const std::vector<PullOverPath> & pull_over_path_candidates,
const GoalCandidates & goal_candidates) const;
Expand Down
135 changes: 70 additions & 65 deletions planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@ namespace behavior_path_planner
GoalPlannerModule::GoalPlannerModule(
const std::string & name, rclcpp::Node & node,
const std::shared_ptr<GoalPlannerParameters> & parameters,
const std::unordered_map<std::string, std::shared_ptr<RTCInterface> > & rtc_interface_ptr_map)
const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map)
: SceneModuleInterface{name, node, rtc_interface_ptr_map},
parameters_{parameters},
vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()},
Expand Down Expand Up @@ -515,32 +515,31 @@ bool GoalPlannerModule::planFreespacePath()
return false;
}

void GoalPlannerModule::returnToLaneParking()
bool GoalPlannerModule::canReturnToLaneParking()
{
// return only before starting free space parking
if (!isStopped()) {
return;
return false;
}

if (!status_.get_lane_parking_pull_over_path()) {
return;
if (!thread_safe_data_.get_lane_parking_pull_over_path()) {
return false;
}

const PathWithLaneId path = status_.get_lane_parking_pull_over_path()->getFullPath();
const PathWithLaneId path = thread_safe_data_.get_lane_parking_pull_over_path()->getFullPath();
if (checkCollision(path)) {
return;
return false;
}

const Point & current_point = planner_data_->self_odometry->pose.pose.position;
constexpr double th_distance = 0.5;
const bool is_close_to_path =
std::abs(motion_utils::calcLateralOffset(path.points, current_point)) < th_distance;
if (!is_close_to_path) {
return;
return false;
}

thread_safe_data_.set_pull_over_path(status_.get_lane_parking_pull_over_path());
RCLCPP_INFO(getLogger(), "return to lane parking");
return true;
}

void GoalPlannerModule::generateGoalCandidates()
Expand Down Expand Up @@ -616,23 +615,10 @@ std::vector<PullOverPath> GoalPlannerModule::sortPullOverPathCandidatesByGoalPri
return sorted_pull_over_path_candidates;
}

void GoalPlannerModule::selectSafePullOverPath()
std::optional<std::pair<PullOverPath, GoalCandidate>> GoalPlannerModule::selectPullOverPath(
const std::vector<PullOverPath> & pull_over_path_candidates,
const GoalCandidates & goal_candidates) const
{
// select safe lane pull over path from candidates
std::vector<PullOverPath> pull_over_path_candidates{};
GoalCandidates goal_candidates{};
{
const std::lock_guard<std::recursive_mutex> lock(mutex_);
goal_searcher_->setPlannerData(planner_data_);
goal_candidates = thread_safe_data_.get_goal_candidates();
goal_searcher_->update(goal_candidates);
thread_safe_data_.set_goal_candidates(goal_candidates);
thread_safe_data_.set_pull_over_path_candidates(sortPullOverPathCandidatesByGoalPriority(
thread_safe_data_.get_pull_over_path_candidates(), thread_safe_data_.get_goal_candidates()));
pull_over_path_candidates = thread_safe_data_.get_pull_over_path_candidates();
thread_safe_data_.clearPullOverPath();
}

for (const auto & pull_over_path : pull_over_path_candidates) {
// check if goal is safe
const auto goal_candidate_it = std::find_if(
Expand All @@ -651,42 +637,10 @@ void GoalPlannerModule::selectSafePullOverPath()
continue;
}

// found safe pull over path
{
const std::lock_guard<std::recursive_mutex> lock(mutex_);
thread_safe_data_.set_pull_over_path(pull_over_path);
thread_safe_data_.set_modified_goal_pose(*goal_candidate_it);
status_.set_lane_parking_pull_over_path(thread_safe_data_.get_pull_over_path());
}
break;
return std::make_pair(pull_over_path, *goal_candidate_it);
}

if (!thread_safe_data_.foundPullOverPath()) {
return;
}

// decelerate before the search area start
const auto decel_pose = calcLongitudinalOffsetPose(
thread_safe_data_.get_pull_over_path()->getFullPath().points,
status_.get_closest_goal_candidate_pose().position, -approximate_pull_over_distance_);
auto & first_path = thread_safe_data_.get_pull_over_path()->partial_paths.front();
if (decel_pose) {
decelerateBeforeSearchStart(*decel_pose, first_path);
return;
}

// if already passed the search start offset pose, set pull_over_velocity to first_path.
const auto min_decel_distance = calcFeasibleDecelDistance(
planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk,
parameters_->pull_over_velocity);
for (auto & p : first_path.points) {
const double distance_from_ego = calcSignedArcLengthFromEgo(first_path, p.point.pose);
if (min_decel_distance && distance_from_ego < *min_decel_distance) {
continue;
}
p.point.longitudinal_velocity_mps = std::min(
p.point.longitudinal_velocity_mps, static_cast<float>(parameters_->pull_over_velocity));
}
return {};

Check notice on line 643 in planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Complex Method 

GoalPlannerModule::selectSafePullOverPath is no longer above the threshold for cyclomatic complexity. 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 643 in planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Bumpy Road Ahead 

GoalPlannerModule::selectSafePullOverPath is no longer above the threshold for logical blocks with deeply nested code. 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.
}

std::vector<DrivableLanes> GoalPlannerModule::generateDrivableLanes() const
Expand Down Expand Up @@ -843,7 +797,7 @@ void GoalPlannerModule::updateSteeringFactor(
pose, distance, SteeringFactor::GOAL_PLANNER, steering_factor_direction, type, "");
}

// NOTE: this function returns true once it returns true. Because selectSafePullOverPath() will
// NOTE: this function returns true once it returns true. Because selectPullOverPath() will
// not select new path.
bool GoalPlannerModule::hasDecidedPath() const
{
Expand Down Expand Up @@ -922,17 +876,42 @@ BehaviorModuleOutput GoalPlannerModule::planPullOver()
if (!hasDecidedPath() && needPathUpdate(1.0 /*path_update_duration*/)) {
// if the final path is not decided and enough time has passed since last path update,
// select safe path from lane parking pull over path candidates
// and set it to thread_safe_data_.get_pull_over_path()
selectSafePullOverPath();
// and set it to thread_safe_data_

thread_safe_data_.clearPullOverPath();

// update goal candidates
goal_searcher_->setPlannerData(planner_data_);
auto goal_candidates = thread_safe_data_.get_goal_candidates();
goal_searcher_->update(goal_candidates);

// update pull over path candidates
const auto pull_over_path_candidates = sortPullOverPathCandidatesByGoalPriority(
thread_safe_data_.get_pull_over_path_candidates(), goal_candidates);

// select pull over path which is safe against static objects and get it's goal
const auto path_and_goal_opt = selectPullOverPath(pull_over_path_candidates, goal_candidates);

// update thread_safe_data_
if (path_and_goal_opt) {
auto [pull_over_path, modified_goal] = *path_and_goal_opt;
deceleratePath(pull_over_path);
thread_safe_data_.set(
goal_candidates, pull_over_path_candidates, pull_over_path, modified_goal);
} else {
thread_safe_data_.set(goal_candidates, pull_over_path_candidates);
}
}

// set output and status
BehaviorModuleOutput output{};
setOutput(output);

// return to lane parking if it is possible
if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE) {
returnToLaneParking();
if (
thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE &&
canReturnToLaneParking()) {
thread_safe_data_.set_pull_over_path(thread_safe_data_.get_lane_parking_pull_over_path());
}

// For debug
Expand Down Expand Up @@ -1439,6 +1418,32 @@ double GoalPlannerModule::calcSignedArcLengthFromEgo(
path.points, current_pose.position, ego_idx, pose.position, target_idx);
}

void GoalPlannerModule::deceleratePath(PullOverPath & pull_over_path) const
{
// decelerate before the search area start
const auto decel_pose = calcLongitudinalOffsetPose(
pull_over_path.getFullPath().points, status_.get_closest_goal_candidate_pose().position,
-approximate_pull_over_distance_);
auto & first_path = pull_over_path.partial_paths.front();
if (decel_pose) {
decelerateBeforeSearchStart(*decel_pose, first_path);
return;
}

// if already passed the search start offset pose, set pull_over_velocity to first_path.
const auto min_decel_distance = calcFeasibleDecelDistance(
planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk,
parameters_->pull_over_velocity);
for (auto & p : first_path.points) {
const double distance_from_ego = calcSignedArcLengthFromEgo(first_path, p.point.pose);
if (min_decel_distance && distance_from_ego < *min_decel_distance) {
continue;
}
p.point.longitudinal_velocity_mps = std::min(
p.point.longitudinal_velocity_mps, static_cast<float>(parameters_->pull_over_velocity));
}
}

void GoalPlannerModule::decelerateForTurnSignal(const Pose & stop_pose, PathWithLaneId & path) const
{
const double time = planner_data_->parameters.turn_signal_search_time;
Expand Down

0 comments on commit 991cee4

Please sign in to comment.