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..3c1e2c464826d 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 stopped_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..4d95d65f59524 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_uppper) { 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_uppper) { 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; }