Skip to content

Commit

Permalink
temp
Browse files Browse the repository at this point in the history 
Signed-off-by: kyoichi-sugahara <[email protected]>
  • Loading branch information
@kyoichi-sugahara
kyoichi-sugahara committed Oct 25, 2023
1 parent 32104fe commit ddadde4
Show file tree
Hide file tree
Showing 2 changed files with 107 additions and 59 deletions.
16 changes: 12 additions & 4 deletions ...planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,11 +58,13 @@ struct PullOutStatus
PullOutPath pull_out_path{};
size_t current_path_idx{0};
PlannerType planner_type{PlannerType::NONE};
PathWithLaneId stop_path{};
PathWithLaneId backward_path{};
lanelet::ConstLanelets pull_out_lanes{};
bool found_pull_out_path{false}; // current path is safe against static objects
bool is_safe_dynamic_objects{false}; // current path is safe against dynamic objects
bool driving_forward{false}; // if ego is driving on backward path, this is set to false
bool found_pull_out_path{false}; // pull out path is found
bool is_safe_dynamic_objects{false}; // current path is safe against dynamic objects
bool backward_path_is_enabled{false}; // If true, backward path is enabled
bool driving_forward{false}; // if ego is driving on backward path, this is set to false
bool backward_driving_complete{
false}; // after backward driving is complete, this is set to true (warning: this is set to
// false at next cycle after backward driving is complete)
Expand Down Expand Up @@ -146,6 +148,12 @@ class StartPlannerModule : public SceneModuleInterface
const behavior_path_planner::PullOutPath & path, const Pose & start_pose,
const behavior_path_planner::PlannerType & planner_type);
void updateStatusIfNoSafePathFound();
void updateStatusWithStopPath();

bool foundPullOutPath();
bool isStopPathNecessary();
bool isStopPathGenerated() const;
bool isShiftPathGeneratedFromCurrentPose() const;

std::shared_ptr<StartPlannerParameters> parameters_;
mutable std::shared_ptr<EgoPredictedPathParams> ego_predicted_path_params_;
Expand Down Expand Up @@ -205,7 +213,7 @@ class StartPlannerModule : public SceneModuleInterface
void setDrivableAreaInfo(BehaviorModuleOutput & output) const;

// check if the goal is located behind the ego in the same route segment.
bool IsGoalBehindOfEgoInSameRouteSegment() const;
bool isGoalBehindOfEgoInSameRouteSegment();

// generate BehaviorPathOutput with stopping path and update status
BehaviorModuleOutput generateStopOutput();
Expand Down
150 changes: 95 additions & 55 deletions planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -234,9 +234,8 @@ void StartPlannerModule::updateCurrentState()

BehaviorModuleOutput StartPlannerModule::plan()
{
if (IsGoalBehindOfEgoInSameRouteSegment()) {
RCLCPP_WARN_THROTTLE(
getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now");
// TODO(Sugahara): rename function name later
if (isStopPathGenerated()) {
const auto output = generateStopOutput();
setDebugData(); // use status updated in generateStopOutput()
updateRTCStatus(0, 0);
Expand All @@ -250,56 +249,50 @@ BehaviorModuleOutput StartPlannerModule::plan()
}

BehaviorModuleOutput output;
if (!status_.found_pull_out_path) {
RCLCPP_WARN_THROTTLE(
getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path");
const auto output = generateStopOutput();
setDebugData(); // use status updated in generateStopOutput()
updateRTCStatus(0, 0);
return output;
}

PathWithLaneId path;

// Check if backward motion is finished
if (status_.driving_forward) {
if (!status_.driving_forward) {
path = status_.backward_path;
} else {
// Increment path index if the current path is finished
// can be moved to porsProcess()?
if (hasFinishedCurrentPath()) {
RCLCPP_INFO(getLogger(), "Increment path index");
incrementPathIndex();
}

if (!status_.is_safe_dynamic_objects && !isWaitingApproval() && !status_.has_stop_point) {
auto current_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);

// 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_.has_stop_point = true;
} else {
path = current_path;
}
} else if (!isWaitingApproval() && status_.has_stop_point) {
// Delete stop point if conditions are met
if (status_.is_safe_dynamic_objects && isStopped()) {
status_.has_stop_point = false;
path = getCurrentPath();
}
path = *status_.prev_stop_path_after_approval;
} else {
path = getCurrentPath();
}
} else {
path = status_.backward_path;
}

// if (!status_.is_safe_dynamic_objects && !isWaitingApproval() && !status_.has_stop_point) {
// auto current_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);

// // 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_.has_stop_point = true;
// } else {
// path = current_path;
// }
// } else if (!isWaitingApproval() && status_.has_stop_point) {
// // Delete stop point if conditions are met
// if (status_.is_safe_dynamic_objects && isStopped()) {
// status_.has_stop_point = false;
// path = getCurrentPath();
// }
// path = *status_.prev_stop_path_after_approval;
// } else {
// path = getCurrentPath();
// }
// }

output.path = std::make_shared<PathWithLaneId>(path);
output.reference_path = getPreviousModuleOutput().reference_path;
output.turn_signal_info = calcTurnSignalInfo();
Expand All @@ -317,7 +310,7 @@ BehaviorModuleOutput StartPlannerModule::plan()
return SteeringFactor::STRAIGHT;
});

if (status_.driving_forward) {
if (isShiftPathGeneratedFromCurrentPose()) {
const double start_distance = motion_utils::calcSignedArcLength(
path.points, planner_data_->self_odometry->pose.pose.position,
status_.pull_out_path.start_pose.position);
Expand Down Expand Up @@ -346,16 +339,35 @@ BehaviorModuleOutput StartPlannerModule::plan()
return output;
}

bool StartPlannerModule::isStopPathGenerated() const
{
return status_.stop_path != PathWithLaneId{};
}

bool StartPlannerModule::isStopPathNecessary()
{
return (isGoalBehindOfEgoInSameRouteSegment() || foundPullOutPath());
}

bool isShiftPathGeneratedFromCurrentPose() const
{
return !isStopPathGenerated() && !status_.backward_path_is_enabled;
}

void StartPlannerModule::postProcess()
{
// .is_safe_static_objects
// .back_finished　の2つを更新する
const std::lock_guard<std::mutex> lock(mutex_);
switch (status_.planner_type) {
case PlannerType::NONE:
status_.found_pull_out_path = false;
status_.driving_forward = false;
break;
case PlannerType::STOP:
status_.back_finished = true;
status_.found_pull_out_path = false;
status_.driving_forward = true;
// is_safe_static_objects can be set to false? in this case.
status_.is_safe_static_objects = false;
const Pose & current_pose = planner_data_->self_odometry->pose.pose;
status_.pull_out_start_pose = current_pose;
status_.pull_out_path.start_pose = current_pose;
Expand All @@ -365,9 +377,8 @@ void StartPlannerModule::postProcess()
case PlannerType::FREESPACE:
case PlannerType::SHIFT:
case PlannerType::GEOMETRIC:

status_.is_safe_static_objects = true;
status_.back_finished = true;
status_.found_pull_out_path = true;
status_.driving_forward = status_.backward_path_is_enabled;
break;

default:
Expand Down Expand Up @@ -414,7 +425,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
{
updatePullOutStatus();

if (IsGoalBehindOfEgoInSameRouteSegment()) {
if (isGoalBehindOfEgoInSameRouteSegment()) {
RCLCPP_WARN_THROTTLE(
getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now");
clearWaitingApproval();
Expand Down Expand Up @@ -603,7 +614,7 @@ void StartPlannerModule::updateStatusWithCurrentPath(
const behavior_path_planner::PlannerType & planner_type)
{
const std::lock_guard<std::mutex> lock(mutex_);
status_.driving_forward = true;
status_.backward_path_is_enabled = false;
status_.found_pull_out_path = true;
status_.pull_out_path = path;
status_.pull_out_start_pose = start_pose;
Expand All @@ -615,7 +626,8 @@ void StartPlannerModule::updateStatusWithNextPath(
const behavior_path_planner::PlannerType & planner_type)
{
const std::lock_guard<std::mutex> lock(mutex_);
status_.driving_forward = false;
status_.backward_path_is_enabled = true;
status_.stop_path = PathWithLaneId{};
status_.found_pull_out_path = true;
status_.pull_out_path = path;
status_.pull_out_start_pose = start_pose;
Expand All @@ -626,11 +638,33 @@ void StartPlannerModule::updateStatusIfNoSafePathFound()
{
if (status_.planner_type != PlannerType::FREESPACE) {
const std::lock_guard<std::mutex> lock(mutex_);
status_.found_pull_out_path = false;
status_.backward_path_is_enabled = false;
status_.stop_path = PathWithLaneId{};
status_.planner_type = PlannerType::NONE;
}
}

void StartPlannerModule::updateStatusWithStopPath()
{
const std::lock_guard<std::mutex> lock(mutex_);
status_.backward_path_is_enabled = false;
status_.stop_path = generateStopPath();
status_.found_pull_out_path = false;
status_.planner_type = PlannerType::STOP;
}

bool StartPlannerModule::foundPullOutPath()
{
// If planner_type is NONE or STOP, it means that no pull out path was found
const bool pull_out_path_found =
status_.planner_type != PlannerType::NONE && status_.planner_type != PlannerType::STOP;
if (!pull_out_path_found) {
RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "pull out path is not found");
updateStatusWithStopPath();
}
return pull_out_path_found;
}

PathWithLaneId StartPlannerModule::generateStopPath() const
{
const auto & current_pose = planner_data_->self_odometry->pose.pose;
Expand Down Expand Up @@ -757,6 +791,8 @@ void StartPlannerModule::updatePullOutStatus()
planWithPriority(
start_pose_candidates, *refined_start_pose, goal_pose, parameters_->search_priority);

isStopPathNecessary();

if (isBackwardDrivingComplete()) {
updateStatusAfterBackwardDriving();
current_state_ = ModuleStatus::SUCCESS; // for breaking loop
Expand Down Expand Up @@ -1158,7 +1194,7 @@ bool StartPlannerModule::isSafePath() const
return is_safe_dynamic_objects;
}

bool StartPlannerModule::IsGoalBehindOfEgoInSameRouteSegment() const
bool StartPlannerModule::isGoalBehindOfEgoInSameRouteSegment()
{
const auto & rh = planner_data_->route_handler;

Expand All @@ -1180,6 +1216,11 @@ bool StartPlannerModule::IsGoalBehindOfEgoInSameRouteSegment() const
ego_lane_path.points, getEgoPosition(), rh->getGoalPose().position);

const bool is_goal_behind_of_ego = (dist_ego_to_goal < 0.0);
if (is_goal_behind_of_ego) {
RCLCPP_WARN_THROTTLE(
getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now");
updateStatusWithStopPath();
}
return is_goal_behind_of_ego;
}

Expand All @@ -1196,7 +1237,6 @@ BehaviorModuleOutput StartPlannerModule::generateStopOutput()

{
const std::lock_guard<std::mutex> lock(mutex_);
status_.driving_forward = true;
status_.planner_type = PlannerType::STOP;
status_.pull_out_path.partial_paths.clear();
status_.pull_out_path.partial_paths.push_back(stop_path);
Expand Down

0 comments on commit ddadde4

Please sign in to comment.