fix(goal_planner): fix isStopped judgement

Signed-off-by: kosuke55 <[email protected]>
autowarefoundation · Dec 6, 2024 · ab50169 · ab50169
1 parent 3231bbf
commit ab50169
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 6 deletions.
diff --git a/...planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/...planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp
@@ -158,7 +158,8 @@ bool checkOccupancyGridCollision(
 
 bool isStopped(
   std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> & 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/...avior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/...avior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp
@@ -244,7 +244,8 @@ bool checkOccupancyGridCollision(
 
 bool isStopped(
   std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> & 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;
   }