Skip to content

Commit

Permalink
fix(goal_planner): fix sudden stop and simpify process (autowarefound…
Browse files Browse the repository at this point in the history 
…ation#6120)

fix(goal_planner): fix sudden stop and simpify process

Signed-off-by: kosuke55 <[email protected]>
  • Loading branch information
@kosuke55
kosuke55 committed Jan 28, 2024
1 parent 3072da5 commit 6fa140d
Show file tree
Hide file tree
Showing 2 changed files with 18 additions and 94 deletions.
21 changes: 3 additions & 18 deletions ...ath_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -240,15 +240,11 @@ struct PreviousPullOverData

void reset()
{
stop_path = nullptr;
stop_path_after_approval = nullptr;
found_path = false;
safety_status = SafetyStatus{};
has_decided_path = false;
}

std::shared_ptr<PathWithLaneId> stop_path{nullptr};
std::shared_ptr<PathWithLaneId> stop_path_after_approval{nullptr};
bool found_path{false};
SafetyStatus safety_status{};
bool has_decided_path{false};
Expand Down Expand Up @@ -387,7 +383,7 @@ class GoalPlannerModule : public SceneModuleInterface
ThreadSafeData thread_safe_data_;

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

// 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 @@ -428,7 +424,7 @@ class GoalPlannerModule : public SceneModuleInterface
void decelerateBeforeSearchStart(
const Pose & search_start_offset_pose, PathWithLaneId & path) const;
PathWithLaneId generateStopPath() const;
PathWithLaneId generateFeasibleStopPath() const;
PathWithLaneId generateFeasibleStopPath(const PathWithLaneId & path) const;

void keepStoppedWithCurrentPath(PathWithLaneId & path) const;
double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const;
Expand Down Expand Up @@ -477,19 +473,8 @@ class GoalPlannerModule : public SceneModuleInterface

// output setter
void setOutput(BehaviorModuleOutput & output) const;
void setStopPath(BehaviorModuleOutput & output) const;
void updatePreviousData(const BehaviorModuleOutput & output);
void updatePreviousData();

/**
* @brief Sets a stop path in the current path based on safety conditions and previous paths.
*
* This function sets a stop path in the current path. Depending on whether the previous safety
* judgement against dynamic objects were safe or if a previous stop path existed, it either
* generates a new stop path or uses the previous stop path.
*
* @param output BehaviorModuleOutput
*/
void setStopPathFromCurrentPath(BehaviorModuleOutput & output) const;
void setModifiedGoal(BehaviorModuleOutput & output) const;
void setTurnSignalInfo(BehaviorModuleOutput & output) const;

Expand Down
91 changes: 15 additions & 76 deletions planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -806,7 +806,9 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) const

if (!thread_safe_data_.foundPullOverPath()) {
// situation : not safe against static objects use stop_path
setStopPath(output);
output.path = generateStopPath();
RCLCPP_WARN_THROTTLE(
getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path");
setDrivableAreaInfo(output);
return;
}
Expand All @@ -816,7 +818,11 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) const
// situation : not safe against dynamic objects after approval
// insert stop point in current path if ego is able to stop with acceleration and jerk
// constraints
setStopPathFromCurrentPath(output);
output.path =
generateFeasibleStopPath(thread_safe_data_.get_pull_over_path()->getCurrentPath());
RCLCPP_WARN_THROTTLE(
getLogger(), *clock_, 5000, "Not safe against dynamic objects, generate stop path");
debug_stop_pose_with_info_.set(std::string("feasible stop after approval"));
} else {
// situation : (safe against static and dynamic objects) or (safe against static objects and
// before approval) don't stop
Expand All @@ -836,58 +842,6 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) const
}
}

void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) const
{
if (prev_data_.found_path || !prev_data_.stop_path) {
// safe -> not_safe or no prev_stop_path: generate new stop_path
output.path = 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 = *prev_data_.stop_path;
RCLCPP_WARN_THROTTLE(
getLogger(), *clock_, 5000, "Not found safe pull_over path, use previous stop path");
// stop_pose_ is removed in manager every loop, so need to set every loop.
const auto stop_pose_opt = utils::getFirstStopPoseFromPath(output.path);
if (stop_pose_opt) {
debug_stop_pose_with_info_.set(stop_pose_opt.value(), std::string("keep previous stop pose"));
}
}
}

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 (prev_data_.safety_status.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::parking_departure::generateFeasibleStopPath(
current_path, planner_data_, stop_pose_, parameters_->maximum_deceleration_for_stop,
parameters_->maximum_jerk_for_stop);
if (stop_path) {
output.path = *stop_path;
debug_stop_pose_with_info_.set(std::string("feasible stop after approval"));
RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, generate stop path");
} else {
output.path = thread_safe_data_.get_pull_over_path()->getCurrentPath();
RCLCPP_WARN_THROTTLE(
getLogger(), *clock_, 5000,
"Collision detected, no feasible stop path found, cannot stop.");
}
} else {
// not_safe safe(no feasible stop path found) -> not_safe: use previous stop path
output.path = *prev_data_.stop_path_after_approval;
// stop_pose_ is removed in manager every loop, so need to set every loop.
const auto stop_pose_opt = utils::getFirstStopPoseFromPath(output.path);
if (stop_pose_opt) {
debug_stop_pose_with_info_.set(
stop_pose_opt.value(), std::string("keep feasible stop after approval"));
}
RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, use previous stop path");
}
}

void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const
{
if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE) {
Expand Down Expand Up @@ -1096,7 +1050,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput()
path_candidate_ =
std::make_shared<PathWithLaneId>(thread_safe_data_.get_pull_over_path()->getFullPath());

updatePreviousData(output);
updatePreviousData();

return output;
}
Expand All @@ -1123,12 +1077,8 @@ void GoalPlannerModule::postProcess()
setStopReason(StopReason::GOAL_PLANNER, thread_safe_data_.get_pull_over_path()->getFullPath());
}

void GoalPlannerModule::updatePreviousData(const BehaviorModuleOutput & output)
void GoalPlannerModule::updatePreviousData()
{
if (prev_data_.found_path || !prev_data_.stop_path) {
prev_data_.stop_path = std::make_shared<PathWithLaneId>(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.
prev_data_.found_path = thread_safe_data_.foundPullOverPath();
Expand All @@ -1152,17 +1102,6 @@ void GoalPlannerModule::updatePreviousData(const BehaviorModuleOutput & output)
prev_data_.safety_status.safe_start_time = std::nullopt;
}
prev_data_.safety_status.is_safe = is_safe;

// If safety check is enabled, save current path as stop_path_after_approval
// This path is set only once after approval.
if (!isActivated() || (!is_safe && prev_data_.stop_path_after_approval)) {
return;
}
auto stop_path = std::make_shared<PathWithLaneId>(output.path);
for (auto & point : stop_path->points) {
point.point.longitudinal_velocity_mps = 0.0;
}
prev_data_.stop_path_after_approval = stop_path;
}

BehaviorModuleOutput GoalPlannerModule::planWaitingApproval()
Expand Down Expand Up @@ -1269,7 +1208,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const
return std::make_pair(decel_pose.value(), "stop at search start pose");
});
if (!stop_pose_with_info) {
const auto feasible_stop_path = generateFeasibleStopPath();
const auto feasible_stop_path = generateFeasibleStopPath(getPreviousModuleOutput().path);
// override stop pose info debug string
debug_stop_pose_with_info_.set(std::string("feasible stop: not calculate stop pose"));
return feasible_stop_path;
Expand All @@ -1284,7 +1223,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const
const bool is_stopped = std::abs(current_vel) < eps_vel;
const double buffer = is_stopped ? stop_distance_buffer_ : 0.0;
if (min_stop_distance && ego_to_stop_distance + buffer < *min_stop_distance) {
const auto feasible_stop_path = generateFeasibleStopPath();
const auto feasible_stop_path = generateFeasibleStopPath(getPreviousModuleOutput().path);
debug_stop_pose_with_info_.set(
std::string("feasible stop: stop pose is closer than min_stop_distance"));
return feasible_stop_path;
Expand Down Expand Up @@ -1316,17 +1255,17 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const
return stop_path;
}

PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() const
PathWithLaneId GoalPlannerModule::generateFeasibleStopPath(const PathWithLaneId & path) const
{
// calc minimum stop distance under maximum deceleration
const auto min_stop_distance = calcFeasibleDecelDistance(
planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0);
if (!min_stop_distance) {
return getPreviousModuleOutput().path;
return path;
}

// set stop point
auto stop_path = getPreviousModuleOutput().path;
auto stop_path = path;
const auto & current_pose = planner_data_->self_odometry->pose.pose;
const auto stop_idx =
motion_utils::insertStopPoint(current_pose, *min_stop_distance, stop_path.points);
Expand Down

0 comments on commit 6fa140d

Please sign in to comment.