Skip to content

Commit b52d323

Browse files
authored
refactor(goal_planner): refactoring plan flow and add post process (#5554)
* refactor(goal_planner): refactoring plan flow and add post process Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> do not decel when searching Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> rename planPullOver Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * add plan flow Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * add reason of early return Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * typo Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * fix path_candidate_ Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * pub path candidate after approval Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> --------- Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
1 parent d4d0804 commit b52d323

File tree

2 files changed

+154
-150
lines changed

2 files changed

+154
-150
lines changed

planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp

+26-5
Original file line numberDiff line numberDiff line change
@@ -287,27 +287,47 @@ class GoalPlannerModule : public SceneModuleInterface
287287
}
288288
}
289289

290-
CandidateOutput planCandidate() const override;
291290
BehaviorModuleOutput plan() override;
292291
BehaviorModuleOutput planWaitingApproval() override;
293292
bool isExecutionRequested() const override;
294293
bool isExecutionReady() const override;
295294
void processOnExit() override;
296295
void updateData() override;
296+
void postProcess() override;
297297
void setParameters(const std::shared_ptr<GoalPlannerParameters> & parameters);
298298
void acceptVisitor(
299299
[[maybe_unused]] const std::shared_ptr<SceneModuleVisitor> & visitor) const override
300300
{
301301
}
302+
CandidateOutput planCandidate() const override { return CandidateOutput{}; }
302303

303304
private:
305+
/* 
306+
* state transitions and plan function used in each state
307+
*
308+
* +--------------------------+
309+
* | RUNNING |
310+
* | planPullOverAsCandidate()|
311+
* +------------+-------------+
312+
* | hasDecidedPath()
313+
* 2 v
314+
* +--------------------------+
315+
* | WAITING_APPROVAL |
316+
* | planPullOverAsCandidate()|
317+
* +------------+-------------+
318+
* | isActivated()
319+
* 3 v
320+
* +--------------------------+
321+
* | RUNNING |
322+
* | planPullOverAsOutput() |
323+
* +--------------------------+
324+
*/
325+
304326
// The start_planner activates when it receives a new route,
305327
// so there is no need to terminate the goal planner.
306328
// If terminating it, it may switch to lane following and could generate an inappropriate path.
307329
bool canTransitSuccessState() override { return false; }
308-
309330
bool canTransitFailureState() override { return false; }
310-
311331
bool canTransitIdleToRunningState() override { return true; }
312332

313333
mutable StartGoalPlannerData goal_planner_data_;
@@ -411,8 +431,9 @@ class GoalPlannerModule : public SceneModuleInterface
411431
void returnToLaneParking();
412432

413433
// plan pull over path
414-
BehaviorModuleOutput planWithGoalModification();
415-
BehaviorModuleOutput planWaitingApprovalWithGoalModification();
434+
BehaviorModuleOutput planPullOver();
435+
BehaviorModuleOutput planPullOverAsOutput();
436+
BehaviorModuleOutput planPullOverAsCandidate();
416437
void selectSafePullOverPath();
417438
std::vector<PullOverPath> sortPullOverPathCandidatesByGoalPriority(
418439
const std::vector<PullOverPath> & pull_over_path_candidates,

0 commit comments

Comments
 (0)