Skip to content

Commit

Permalink
refactor(goal_planner): rebuild state transition (#5371)" (#5399)" (#…
Browse files Browse the repository at this point in the history 
…5417)

* refactor(goal_planner): rebuild state transition (#5371)" (#5399)"

This reverts commit 92f78a4.

* fix: increment path idx

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* refactor

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

---------

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
  • Loading branch information
@kosuke55
kosuke55 authored Oct 27, 2023
1 parent 29d34df commit 25e8b3f
Show file tree
Hide file tree
Showing 2 changed files with 36 additions and 56 deletions.
31 changes: 17 additions & 14 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 @@ -135,10 +135,8 @@ class PullOverStatus
DEFINE_SETTER_GETTER(bool, has_decided_velocity)
DEFINE_SETTER_GETTER(bool, has_requested_approval)
DEFINE_SETTER_GETTER(bool, is_ready)
DEFINE_SETTER_GETTER(std::shared_ptr<rclcpp::Time>, last_approved_time)
DEFINE_SETTER_GETTER(std::shared_ptr<rclcpp::Time>, last_increment_time)
DEFINE_SETTER_GETTER(std::shared_ptr<rclcpp::Time>, last_path_update_time)
DEFINE_SETTER_GETTER(std::shared_ptr<Pose>, last_approved_pose)
DEFINE_SETTER_GETTER(std::optional<GoalCandidate>, modified_goal_pose)
DEFINE_SETTER_GETTER(Pose, refined_goal_pose)
DEFINE_SETTER_GETTER(GoalCandidates, goal_candidates)
Expand Down Expand Up @@ -166,10 +164,8 @@ class PullOverStatus
bool is_ready_{false};

// save last time and pose
std::shared_ptr<rclcpp::Time> last_approved_time_;
std::shared_ptr<rclcpp::Time> last_increment_time_;
std::shared_ptr<rclcpp::Time> last_path_update_time_;
std::shared_ptr<Pose> last_approved_pose_;

// goal modification
std::optional<GoalCandidate> modified_goal_pose_;
Expand Down Expand Up @@ -199,6 +195,14 @@ struct GoalPlannerDebugData
std::vector<Polygon2d> ego_polygons_expanded{};
};

struct LastApprovalData
{
LastApprovalData(rclcpp::Time time, Pose pose) : time(time), pose(pose) {}

rclcpp::Time time{};
Pose pose{};
};

class GoalPlannerModule : public SceneModuleInterface
{
public:
Expand All @@ -219,14 +223,11 @@ class GoalPlannerModule : public SceneModuleInterface
}
}

// TODO(someone): remove this, and use base class function
[[deprecated]] BehaviorModuleOutput run() override;
bool isExecutionRequested() const override;
bool isExecutionReady() const override;
// TODO(someone): remove this, and use base class function
[[deprecated]] ModuleStatus updateState() override;
CandidateOutput planCandidate() const override;
BehaviorModuleOutput plan() override;
BehaviorModuleOutput planWaitingApproval() override;
bool isExecutionRequested() const override;
bool isExecutionReady() const override;
void processOnExit() override;
void updateData() override;
void setParameters(const std::shared_ptr<GoalPlannerParameters> & parameters);
Expand All @@ -235,15 +236,15 @@ class GoalPlannerModule : public SceneModuleInterface
{
}

// not used, but need to override
CandidateOutput planCandidate() const override { return CandidateOutput{}; };

private:
// The start_planner activates when it receives a new route,
// so there is no need to terminate the goal planner.
// If terminating it, it may switch to lane following and could generate an inappropriate path.
bool canTransitSuccessState() override { return false; }

bool canTransitFailureState() override { return false; }

bool canTransitIdleToRunningState() override { return false; }
bool canTransitIdleToRunningState() override { return true; }

mutable StartGoalPlannerData goal_planner_data_;

Expand Down Expand Up @@ -276,6 +277,8 @@ class GoalPlannerModule : public SceneModuleInterface
std::recursive_mutex mutex_;
PullOverStatus status_;

std::unique_ptr<LastApprovalData> last_approval_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.
const double approximate_pull_over_distance_{20.0};
Expand Down
61 changes: 19 additions & 42 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 @@ -227,18 +227,6 @@ void GoalPlannerModule::onFreespaceParkingTimer()
}
}

BehaviorModuleOutput GoalPlannerModule::run()
{
current_state_ = ModuleStatus::RUNNING;
updateOccupancyGrid();

if (!isActivated()) {
return planWaitingApproval();
}

return plan();
}

void GoalPlannerModule::updateData()
{
// Initialize Occupancy Grid Map
Expand Down Expand Up @@ -287,6 +275,8 @@ void GoalPlannerModule::processOnExit()
resetPathCandidate();
resetPathReference();
debug_marker_.markers.clear();
status_.reset();
last_approval_data_.reset();
}

bool GoalPlannerModule::isExecutionRequested() const
Expand Down Expand Up @@ -448,13 +438,6 @@ Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const
return refined_goal_pose;
}

ModuleStatus GoalPlannerModule::updateState()
{
// start_planner module will be run when setting new goal, so not need finishing pull_over module.
// Finishing it causes wrong lane_following path generation.
return current_state_;
}

bool GoalPlannerModule::planFreespacePath()
{
goal_searcher_->setPlannerData(planner_data_);
Expand Down Expand Up @@ -914,6 +897,12 @@ void GoalPlannerModule::decideVelocity()
status_.set_has_decided_velocity(true);
}

CandidateOutput GoalPlannerModule::planCandidate() const
{
return CandidateOutput(
status_.get_pull_over_path() ? status_.get_pull_over_path()->getFullPath() : PathWithLaneId());
}

BehaviorModuleOutput GoalPlannerModule::planWithGoalModification()
{
// if pull over path candidates generation is not finished, use previous module output
Expand All @@ -931,14 +920,9 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification()

// Use decided path
if (status_.get_has_decided_path()) {
if (isActivated() && isWaitingApproval()) {
{
const std::lock_guard<std::recursive_mutex> lock(mutex_);
status_.set_last_approved_time(std::make_shared<rclcpp::Time>(clock_->now()));
status_.set_last_approved_pose(
std::make_shared<Pose>(planner_data_->self_odometry->pose.pose));
}
clearWaitingApproval();
if (isActivated() && !last_approval_data_) {
last_approval_data_ =
std::make_unique<LastApprovalData>(clock_->now(), planner_data_->self_odometry->pose.pose);
decideVelocity();
}
transitionToNextPathIfFinishingCurrentPath();
Expand All @@ -954,7 +938,7 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification()
// set output and status
BehaviorModuleOutput output{};
setOutput(output);
path_candidate_ = std::make_shared<PathWithLaneId>(status_.get_pull_over_path()->getFullPath());
path_candidate_ = std::make_shared<PathWithLaneId>(planCandidate().path_candidate);
path_reference_ = getPreviousModuleOutput().reference_path;

// return to lane parking if it is possible
Expand Down Expand Up @@ -1005,16 +989,11 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification(
return getPreviousModuleOutput();
}

waitApproval();

BehaviorModuleOutput out;
out.modified_goal = plan().modified_goal; // update status_
out.path = std::make_shared<PathWithLaneId>(generateStopPath());
out.reference_path = getPreviousModuleOutput().reference_path;
path_candidate_ =
status_.get_is_safe_static_objects()
? std::make_shared<PathWithLaneId>(status_.get_pull_over_path()->getFullPath())
: out.path;
path_candidate_ = std::make_shared<PathWithLaneId>(planCandidate().path_candidate);
path_reference_ = getPreviousModuleOutput().reference_path;
const auto distance_to_path_change = calcDistanceToPathChange();

Expand Down Expand Up @@ -1191,12 +1170,11 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath()

void GoalPlannerModule::transitionToNextPathIfFinishingCurrentPath()
{
if (isActivated() && status_.get_last_approved_time()) {
if (isActivated() && last_approval_data_) {
// if using arc_path and finishing current_path, get next path
// enough time for turn signal
const bool has_passed_enough_time =
(clock_->now() - *status_.get_last_approved_time()).seconds() >
planner_data_->parameters.turn_signal_search_time;
const bool has_passed_enough_time = (clock_->now() - last_approval_data_->time).seconds() >
planner_data_->parameters.turn_signal_search_time;

if (hasFinishedCurrentPath() && has_passed_enough_time && status_.get_require_increment()) {
if (incrementPathIndex()) {
Expand Down Expand Up @@ -1331,10 +1309,9 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const
{
// ego decelerates so that current pose is the point `turn_light_on_threshold_time` seconds
// before starting pull_over
turn_signal.desired_start_point =
status_.get_last_approved_pose() && status_.get_has_decided_path()
? *status_.get_last_approved_pose()
: current_pose;
turn_signal.desired_start_point = last_approval_data_ && status_.get_has_decided_path()
? last_approval_data_->pose
: current_pose;
turn_signal.desired_end_point = end_pose;
turn_signal.required_start_point = start_pose;
turn_signal.required_end_point = end_pose;
Expand Down

0 comments on commit 25e8b3f

Please sign in to comment.