Skip to content

Commit

Permalink
refactor(behavior_path_planner): use updateData() (autowarefoundation…
Browse files Browse the repository at this point in the history
…#5165)

use updateData

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
  • Loading branch information
kyoichi-sugahara authored Sep 29, 2023
1 parent e32ed72 commit ec30503
Show file tree
Hide file tree
Showing 2 changed files with 39 additions and 14 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -63,6 +63,9 @@ struct PullOutStatus
bool is_safe_dynamic_objects{false}; // current path is safe against dynamic objects
bool back_finished{false}; // if backward driving is not required, this is also set to true
// todo: rename to clear variable name.
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)
Pose pull_out_start_pose{};

PullOutStatus() {}
Expand All @@ -87,12 +90,13 @@ class StartPlannerModule : public SceneModuleInterface
bool isExecutionRequested() const override;
bool isExecutionReady() const override;
// TODO(someone): remove this, and use base class function
[[deprecated]] ModuleStatus updateState() override;
[[deprecated]] void updateCurrentState() override;
BehaviorModuleOutput plan() override;
BehaviorModuleOutput planWaitingApproval() override;
CandidateOutput planCandidate() const override;
void processOnEntry() override;
void processOnExit() override;
void updateData() override;

void setParameters(const std::shared_ptr<StartPlannerParameters> & parameters)
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -96,6 +96,7 @@ void StartPlannerModule::onFreespacePlannerTimer()

BehaviorModuleOutput StartPlannerModule::run()
{
updateData();
if (!isActivated()) {
return planWaitingApproval();
}
Expand All @@ -120,6 +121,23 @@ void StartPlannerModule::processOnExit()
debug_marker_.markers.clear();
}

void StartPlannerModule::updateData()
{
if (isBackwardDrivingComplete()) {
updateStatusAfterBackwardDriving();
} else {
status_.backward_driving_complete = false;
}

const bool has_received_new_route =
!planner_data_->prev_route_id ||
*planner_data_->prev_route_id != planner_data_->route_handler->getRouteUuid();

if (has_received_new_route) {
status_ = PullOutStatus();
}
}

bool StartPlannerModule::isExecutionRequested() const
{
const Pose & current_pose = planner_data_->self_odometry->pose.pose;
Expand Down Expand Up @@ -178,26 +196,32 @@ bool StartPlannerModule::isExecutionReady() const
return true;
}

ModuleStatus StartPlannerModule::updateState()
void StartPlannerModule::updateCurrentState()
{
RCLCPP_DEBUG(getLogger(), "START_PLANNER updateState");
RCLCPP_DEBUG(getLogger(), "START_PLANNER updateCurrentState");

const auto print = [this](const auto & from, const auto & to) {
RCLCPP_DEBUG(getLogger(), "[start_planner] Transit from %s to %s.", from.data(), to.data());
};

const auto & from = current_state_;
// current_state_ = updateState();

if (isActivated() && !isWaitingApproval()) {
current_state_ = ModuleStatus::RUNNING;
} else {
current_state_ = ModuleStatus::IDLE;
}

// TODO(someone): move to canTransitSuccessState
if (hasFinishedPullOut()) {
return ModuleStatus::SUCCESS;
current_state_ = ModuleStatus::SUCCESS;
}

if (isBackwardDrivingComplete()) {
updateStatusAfterBackwardDriving();
return ModuleStatus::SUCCESS; // for breaking loop
// TODO(someone): move to canTransitSuccessState
if (status_.backward_driving_complete) {
current_state_ = ModuleStatus::SUCCESS; // for breaking loop
}

return current_state_;
print(magic_enum::enum_name(from), magic_enum::enum_name(current_state_));
}

BehaviorModuleOutput StartPlannerModule::plan()
Expand Down Expand Up @@ -614,10 +638,6 @@ void StartPlannerModule::updatePullOutStatus()
!planner_data_->prev_route_id ||
*planner_data_->prev_route_id != planner_data_->route_handler->getRouteUuid();

if (has_received_new_route) {
status_ = PullOutStatus();
}

// save pull out lanes which is generated using current pose before starting pull out
// (before approval)
status_.pull_out_lanes = start_planner_utils::getPullOutLanes(
Expand Down Expand Up @@ -672,6 +692,7 @@ void StartPlannerModule::updatePullOutStatus()
void StartPlannerModule::updateStatusAfterBackwardDriving()
{
status_.back_finished = true;
status_.backward_driving_complete = true;
// request start_planner approval
waitApproval();
// To enable approval of the forward path, the RTC status is removed.
Expand Down

0 comments on commit ec30503

Please sign in to comment.