Skip to content

Commit

Permalink
fix generating goal candidates
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 9815f5f commit 1a13e51
Show file tree
Hide file tree
Showing 2 changed files with 63 additions and 107 deletions.
80 changes: 19 additions & 61 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 @@ -68,64 +68,6 @@ using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams;
using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane;
using tier4_autoware_utils::Polygon2d;

#define DEFINE_SETTER(TYPE, NAME) \
public: \
void set_##NAME(const TYPE & value) \
{ \
NAME##_ = value; \
}

#define DEFINE_GETTER(TYPE, NAME) \
public: \
TYPE get_##NAME() const \
{ \
return NAME##_; \
}

#define DEFINE_SETTER_GETTER(TYPE, NAME) \
DEFINE_SETTER(TYPE, NAME) \
DEFINE_GETTER(TYPE, NAME)

class PullOverStatus
{
public:
// Reset all data members to their initial states
void reset()
{
lane_parking_pull_over_path_ = nullptr;
prev_stop_path_ = nullptr;
prev_stop_path_after_approval_ = nullptr;

is_safe_ = false;
prev_found_path_ = false;
prev_is_safe_ = false;
}

DEFINE_SETTER_GETTER(std::shared_ptr<PullOverPath>, lane_parking_pull_over_path)
DEFINE_SETTER_GETTER(std::shared_ptr<PathWithLaneId>, prev_stop_path)
DEFINE_SETTER_GETTER(std::shared_ptr<PathWithLaneId>, prev_stop_path_after_approval)
DEFINE_SETTER_GETTER(bool, is_safe)
DEFINE_SETTER_GETTER(bool, prev_found_path)
DEFINE_SETTER_GETTER(bool, prev_is_safe)
DEFINE_SETTER_GETTER(Pose, refined_goal_pose)
DEFINE_SETTER_GETTER(Pose, closest_goal_candidate_pose)

private:
std::shared_ptr<PullOverPath> lane_parking_pull_over_path_{nullptr};
std::shared_ptr<PathWithLaneId> prev_stop_path_{nullptr};
std::shared_ptr<PathWithLaneId> prev_stop_path_after_approval_{nullptr};
bool is_safe_{false};
bool prev_found_path_{false};
bool prev_is_safe_{false};

Pose refined_goal_pose_{};
Pose closest_goal_candidate_pose_{};
};

#undef DEFINE_SETTER
#undef DEFINE_GETTER
#undef DEFINE_SETTER_GETTER

#define DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \
public: \
void set_##NAME(const TYPE & value) \
Expand Down Expand Up @@ -288,6 +230,22 @@ struct LastApprovalData
Pose pose{};
};

struct PreviousPullOverData
{
void reset()
{
stop_path = nullptr;
stop_path_after_approval = nullptr;
found_path = false;
is_safe = false;
}

std::shared_ptr<PathWithLaneId> stop_path{nullptr};
std::shared_ptr<PathWithLaneId> stop_path_after_approval{nullptr};
bool found_path{false};
bool is_safe{false};
};

class GoalPlannerModule : public SceneModuleInterface
{
public:
Expand Down Expand Up @@ -361,10 +319,10 @@ class GoalPlannerModule : public SceneModuleInterface
tier4_autoware_utils::LinearRing2d vehicle_footprint_;

std::recursive_mutex mutex_;
PullOverStatus status_;
ThreadSafeData thread_safe_data_;

std::unique_ptr<LastApprovalData> last_approval_data_{nullptr};
PreviousPullOverData prev_data_{nullptr};

// approximate distance from the start point to the end point of pull_over.
// this is used as an assumed value to decelerate, etc., before generating the actual path.
Expand Down Expand Up @@ -393,7 +351,7 @@ class GoalPlannerModule : public SceneModuleInterface

// goal seach
Pose calcRefinedGoal(const Pose & goal_pose) const;
void generateGoalCandidates();
GoalCandidates generateGoalCandidates() const;

// stop or decelerate
void deceleratePath(PullOverPath & pull_over_path) const;
Expand All @@ -418,7 +376,6 @@ class GoalPlannerModule : public SceneModuleInterface
bool hasDecidedPath() const;
void decideVelocity();
bool foundPullOverPath() const;
void updateStatus(const BehaviorModuleOutput & output);

// validation
bool hasEnoughDistance(const PullOverPath & pull_over_path) const;
Expand Down Expand Up @@ -451,6 +408,7 @@ class GoalPlannerModule : public SceneModuleInterface
// output setter
void setOutput(BehaviorModuleOutput & output) const;
void setStopPath(BehaviorModuleOutput & output) const;
void updatePreviousData(const BehaviorModuleOutput & output);

/**
* @brief Sets a stop path in the current path based on safety conditions and previous paths.
Expand Down
90 changes: 44 additions & 46 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 @@ -121,7 +121,7 @@ GoalPlannerModule::GoalPlannerModule(
goal_planner_data_.collision_check);
}

status_.reset();
prev_data_.reset();
}

// This function is needed for waiting for planner_data_
Expand Down Expand Up @@ -240,7 +240,7 @@ void GoalPlannerModule::onFreespaceParkingTimer()

void GoalPlannerModule::updateData()
{
if (getCurrentStatus() == ModuleStatus::IDLE) {
if (getCurrentStatus() == ModuleStatus::IDLE && !isExecutionRequested()) {
return;
}

Expand All @@ -259,7 +259,13 @@ void GoalPlannerModule::updateData()

updateOccupancyGrid();

generateGoalCandidates();
// update goal searcher and generate goal candidates
if (thread_safe_data_.get_goal_candidates().empty()) {
goal_searcher_->setPlannerData(planner_data_);
goal_searcher_->setReferenceGoal(
calcRefinedGoal(planner_data_->route_handler->getOriginalGoalPose()));
thread_safe_data_.set_goal_candidates(generateGoalCandidates());
}

if (!isActivated()) {
return;
Expand Down Expand Up @@ -305,7 +311,7 @@ void GoalPlannerModule::processOnExit()
resetPathCandidate();
resetPathReference();
debug_marker_.markers.clear();
status_.reset();
prev_data_.reset();
last_approval_data_.reset();
}

Expand Down Expand Up @@ -542,33 +548,24 @@ bool GoalPlannerModule::canReturnToLaneParking()
return true;
}

void GoalPlannerModule::generateGoalCandidates()
GoalCandidates GoalPlannerModule::generateGoalCandidates() const
{
if (!thread_safe_data_.get_goal_candidates().empty()) {
return;
}

// calculate goal candidates
const auto & route_handler = planner_data_->route_handler;
const Pose goal_pose = route_handler->getOriginalGoalPose();
status_.set_refined_goal_pose(calcRefinedGoal(goal_pose));
if (goal_planner_utils::isAllowedGoalModification(route_handler)) {
const std::lock_guard<std::recursive_mutex> lock(mutex_);
goal_searcher_->setPlannerData(planner_data_);
goal_searcher_->setReferenceGoal(status_.get_refined_goal_pose());
thread_safe_data_.set_goal_candidates(goal_searcher_->search());
status_.set_closest_goal_candidate_pose(
goal_searcher_->getClosetGoalCandidateAlongLanes(thread_safe_data_.get_goal_candidates())
.goal_pose);
} else {
GoalCandidate goal_candidate{};
goal_candidate.goal_pose = goal_pose;
goal_candidate.distance_from_original_goal = 0.0;
GoalCandidates goal_candidates{};
goal_candidates.push_back(goal_candidate);
thread_safe_data_.set_goal_candidates(goal_candidates);
status_.set_closest_goal_candidate_pose(goal_pose);
return goal_searcher_->search();
}

// NOTE:
// currently since pull over is performed only when isAllowedGoalModification is true,
// never be in the following process.
GoalCandidate goal_candidate{};
goal_candidate.goal_pose = route_handler->getOriginalGoalPose();
goal_candidate.distance_from_original_goal = 0.0;
GoalCandidates goal_candidates{};
goal_candidates.push_back(goal_candidate);

return goal_candidates;
}

BehaviorModuleOutput GoalPlannerModule::plan()
Expand Down Expand Up @@ -692,14 +689,14 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) const

void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) const
{
if (status_.get_prev_found_path() || !status_.get_prev_stop_path()) {
if (prev_data_.found_path || !prev_data_.stop_path) {
// safe -> not_safe or no prev_stop_path: generate new stop_path
output.path = std::make_shared<PathWithLaneId>(generateStopPath());
RCLCPP_WARN_THROTTLE(
getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path");
} else {
// not_safe -> not_safe: use previous stop path
output.path = status_.get_prev_stop_path();
output.path = prev_data_.stop_path;
// stop_pose_ is removed in manager every loop, so need to set every loop.
stop_pose_ = utils::getFirstStopPoseFromPath(*output.path);
RCLCPP_WARN_THROTTLE(
Expand All @@ -710,15 +707,14 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) const
void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output) const
{
// safe or not safe(no feasible stop_path found) -> not_safe: try to find new feasible stop_path
if (status_.get_prev_is_safe() || !status_.get_prev_stop_path_after_approval()) {
if (prev_data_.is_safe || !prev_data_.stop_path_after_approval) {
auto current_path = thread_safe_data_.get_pull_over_path()->getCurrentPath();
const auto stop_path =
behavior_path_planner::utils::start_goal_planner_common::generateFeasibleStopPath(
current_path, planner_data_, *stop_pose_, parameters_->maximum_deceleration_for_stop,
parameters_->maximum_jerk_for_stop);
if (stop_path) {
output.path = std::make_shared<PathWithLaneId>(*stop_path);
// status_.set_prev_stop_path_after_approval(output.path);
RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, generate stop path");
} else {
output.path =
Expand All @@ -727,10 +723,9 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output
getLogger(), *clock_, 5000,
"Collision detected, no feasible stop path found, cannot stop.");
}
// status_.set_last_path_update_time(std::make_shared<rclcpp::Time>(clock_->now()));
} else {
// not_safe safe(no feasible stop path found) -> not_safe: use previous stop path
output.path = status_.get_prev_stop_path_after_approval();
output.path = prev_data_.stop_path_after_approval;
// stop_pose_ is removed in manager every loop, so need to set every loop.
stop_pose_ = utils::getFirstStopPoseFromPath(*output.path);
RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, use previous stop path");
Expand Down Expand Up @@ -925,7 +920,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOver()
return output;
}

updateStatus(output);
updatePreviousData(output);

Check notice on line 923 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::planWithGoalModification 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 warning on line 923 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)

❌ New issue: Complex Method 

GoalPlannerModule::planPullOver has a cyclomatic complexity of 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.

return output;
}
Expand All @@ -952,33 +947,32 @@ void GoalPlannerModule::postProcess()
setStopReason(StopReason::GOAL_PLANNER, thread_safe_data_.get_pull_over_path()->getFullPath());
}

void GoalPlannerModule::updateStatus(const BehaviorModuleOutput & output)
void GoalPlannerModule::updatePreviousData(const BehaviorModuleOutput & output)
{
if (status_.get_prev_found_path() || !status_.get_prev_stop_path()) {
status_.set_prev_stop_path(output.path);
if (prev_data_.found_path || !prev_data_.stop_path) {
prev_data_.stop_path = output.path;
}

// for the next loop setOutput().
// this is used to determine whether to generate a new stop path or keep the current stop path.
status_.set_prev_found_path(thread_safe_data_.foundPullOverPath());
status_.set_prev_is_safe(
parameters_->safety_check_params.enable_safety_check ? isSafePath() : true);
prev_data_.found_path = thread_safe_data_.foundPullOverPath();
prev_data_.is_safe = parameters_->safety_check_params.enable_safety_check ? isSafePath() : true;

if (!isActivated()) {
return;
}

if (
!parameters_->safety_check_params.enable_safety_check || isSafePath() ||
(!status_.get_prev_is_safe() && status_.get_prev_stop_path_after_approval())) {
(!prev_data_.is_safe && prev_data_.stop_path_after_approval)) {

Check notice on line 967 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)

ℹ New issue: Complex Conditional 

GoalPlannerModule::updatePreviousData has 1 complex conditionals with 3 branches, threshold = 2. A complex conditional is an expression inside a branch (e.g. if, for, while) which consists of multiple, logical operators such as AND/OR. The more logical operators in an expression, the more severe the code smell.
return;
}
status_.set_prev_is_safe(false);
prev_data_.is_safe = false;
const bool is_stop_path = std::any_of(
output.path->points.begin(), output.path->points.end(),
[](const auto & point) { return point.point.longitudinal_velocity_mps == 0.0; });
if (is_stop_path) {
status_.set_prev_stop_path_after_approval(output.path);
prev_data_.stop_path_after_approval = output.path;
}
}

Check warning on line 977 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)

❌ New issue: Complex Method 

GoalPlannerModule::updatePreviousData has a cyclomatic complexity of 10, 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.

Expand Down Expand Up @@ -1051,8 +1045,10 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const
// calculate search start offset pose from the closest goal candidate pose with
// approximate_pull_over_distance_ ego vehicle decelerates to this position. or if no feasible
// stop point is found, stop at this position.
const auto closest_goal_candidate =
goal_searcher_->getClosetGoalCandidateAlongLanes(thread_safe_data_.get_goal_candidates());
const auto decel_pose = calcLongitudinalOffsetPose(
reference_path.points, status_.get_closest_goal_candidate_pose().position,
reference_path.points, closest_goal_candidate.goal_pose.position,
-approximate_pull_over_distance_);

// if not approved stop road lane.
Expand Down Expand Up @@ -1421,8 +1417,10 @@ double GoalPlannerModule::calcSignedArcLengthFromEgo(
void GoalPlannerModule::deceleratePath(PullOverPath & pull_over_path) const
{
// decelerate before the search area start
const auto closest_goal_candidate =
goal_searcher_->getClosetGoalCandidateAlongLanes(thread_safe_data_.get_goal_candidates());
const auto decel_pose = calcLongitudinalOffsetPose(
pull_over_path.getFullPath().points, status_.get_closest_goal_candidate_pose().position,
pull_over_path.getFullPath().points, closest_goal_candidate.goal_pose.position,
-approximate_pull_over_distance_);
auto & first_path = pull_over_path.partial_paths.front();
if (decel_pose) {
Expand Down Expand Up @@ -1652,7 +1650,7 @@ bool GoalPlannerModule::isSafePath() const
pull_over_lanes, route_handler, filtered_objects, objects_filtering_params_);

const double hysteresis_factor =
status_.get_prev_is_safe() ? 1.0 : parameters_->hysteresis_factor_expand_rate;
prev_data_.is_safe ? 1.0 : parameters_->hysteresis_factor_expand_rate;

utils::start_goal_planner_common::updateSafetyCheckTargetObjectsData(
goal_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path);
Expand Down Expand Up @@ -1716,7 +1714,7 @@ void GoalPlannerModule::setDebugData()
// Visualize pull over areas
const auto color = hasDecidedPath() ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow
: createMarkerColor(0.0, 1.0, 0.0, 0.999); // green
const double z = status_.get_refined_goal_pose().position.z;
const double z = planner_data_->route_handler->getGoalPose().position.z;
add(goal_planner_utils::createPullOverAreaMarkerArray(
goal_searcher_->getAreaPolygons(), header, color, z));

Expand Down

0 comments on commit 1a13e51

Please sign in to comment.