From 90bf2d07efebba88571e6e6218fa34a70ce85044 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Sat, 7 Dec 2024 17:35:31 +0900 Subject: [PATCH] fix(goal_planner): fix isStopped judgement (#9585) * fix(goal_planner): fix isStopped judgement Signed-off-by: kosuke55 * fix typo Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- .../goal_planner_module.hpp | 3 ++- .../src/goal_planner_module.cpp | 15 ++++++++++----- 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index 914da46e60180..09acced162abd 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -158,7 +158,8 @@ bool checkOccupancyGridCollision( bool isStopped( std::deque & odometry_buffer, - const nav_msgs::msg::Odometry::ConstSharedPtr self_odometry, const double duration_lower); + const nav_msgs::msg::Odometry::ConstSharedPtr self_odometry, const double duration_lower, + const double velocity_upper); // Flag class for managing whether a certain callback is running in multi-threading class ScopedFlag diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 01d06a4964c61..716047c7ad1f1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -244,7 +244,8 @@ bool checkOccupancyGridCollision( bool isStopped( std::deque & odometry_buffer, - const nav_msgs::msg::Odometry::ConstSharedPtr self_odometry, const double duration_lower) + const nav_msgs::msg::Odometry::ConstSharedPtr self_odometry, const double duration_lower, + const double velocity_upper) { odometry_buffer.push_back(self_odometry); // Delete old data in buffer_stuck_ @@ -259,7 +260,7 @@ bool isStopped( bool is_stopped = true; for (const auto & odometry : odometry_buffer) { const double ego_vel = utils::l2Norm(odometry->twist.twist.linear); - if (ego_vel > duration_lower) { + if (ego_vel > velocity_upper) { is_stopped = false; break; } @@ -879,7 +880,8 @@ bool GoalPlannerModule::canReturnToLaneParking(const PullOverContextData & conte { // return only before starting free space parking if (!isStopped( - odometry_buffer_stopped_, planner_data_->self_odometry, parameters_->th_stopped_time)) { + odometry_buffer_stopped_, planner_data_->self_odometry, parameters_->th_stopped_time, + parameters_->th_stopped_velocity)) { return false; } @@ -1760,7 +1762,9 @@ bool GoalPlannerModule::isStuck( } constexpr double stuck_time = 5.0; - if (!isStopped(odometry_buffer_stuck_, planner_data->self_odometry, stuck_time)) { + if (!isStopped( + odometry_buffer_stuck_, planner_data->self_odometry, stuck_time, + parameters_->th_stopped_velocity)) { return false; } @@ -1801,7 +1805,8 @@ bool GoalPlannerModule::hasFinishedCurrentPath(const PullOverContextData & ctx_d } if (!isStopped( - odometry_buffer_stopped_, planner_data_->self_odometry, parameters_->th_stopped_time)) { + odometry_buffer_stopped_, planner_data_->self_odometry, parameters_->th_stopped_time, + parameters_->th_stopped_velocity)) { return false; }