From 7c09742b3765121a226a648c07ee59c2a2a4028e Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 30 Jan 2024 18:48:09 +0900 Subject: [PATCH] fix(goal_planner): fix deviated path update (#6221) Signed-off-by: kosuke55 --- .../goal_planner_module.hpp | 1 + .../src/goal_planner_module.cpp | 11 +++++++++++ 2 files changed, 12 insertions(+) diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp index 2c839f582be12..71c309581f251 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -493,6 +493,7 @@ class GoalPlannerModule : public SceneModuleInterface std::optional last_previous_module_output_{}; bool hasPreviousModulePathShapeChanged() const; bool hasDeviatedFromLastPreviousModulePath() const; + bool hasDeviatedFromCurrentPreviousModulePath() const; // timer for generating pull over path candidates in a separate thread void onTimer(); diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index 00810432a6f82..806ee6e973ab3 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -161,6 +161,13 @@ bool GoalPlannerModule::hasDeviatedFromLastPreviousModulePath() const planner_data_->self_odometry->pose.pose.position)) > 0.3; } +bool GoalPlannerModule::hasDeviatedFromCurrentPreviousModulePath() const +{ + return std::abs(motion_utils::calcLateralOffset( + getPreviousModuleOutput().path.points, + planner_data_->self_odometry->pose.pose.position)) > 0.3; +} + // generate pull over candidate paths void GoalPlannerModule::onTimer() { @@ -179,6 +186,10 @@ void GoalPlannerModule::onTimer() // check if new pull over path candidates are needed to be generated const bool need_update = std::invoke([&]() { + if (hasDeviatedFromCurrentPreviousModulePath()) { + RCLCPP_ERROR(getLogger(), "has deviated from current previous module path"); + return false; + } if (thread_safe_data_.get_pull_over_path_candidates().empty()) { return true; }