feat(freespace_planner): only plan if ego is stopped

Fixes #5936 Signed-off-by: Vincent Richard <richard-v@macnica.co.jp>
autowarefoundation · Jan 29, 2024 · 510317e · 510317e
1 parent f514bca
commit 510317e
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 3 deletions.
diff --git a/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp b/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp
@@ -133,6 +133,7 @@ class FreespacePlannerNode : public rclcpp::Node
   size_t prev_target_index_;
   size_t target_index_;
   bool is_completed_ = false;
+  bool reset_in_progress_ = false;
 
   LaneletRoute::ConstSharedPtr route_;
   OccupancyGrid::ConstSharedPtr occupancy_grid_;

diff --git a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp
@@ -441,7 +441,9 @@ void FreespacePlannerNode::onTimer()
     return;
   }
 
-  if (isPlanRequired()) {
+  // Must stop before replanning any new trajectory
+  bool is_reset_required = !reset_in_progress_ && isPlanRequired();
+  if (is_reset_required) {
     // Stop before planning new trajectory
     const auto stop_trajectory = partial_trajectory_.points.empty()
                                    ? createStopTrajectory(current_pose_)
@@ -452,8 +454,19 @@ void FreespacePlannerNode::onTimer()
 
     reset();
 
-    // Plan new trajectory
-    planTrajectory();
+    reset_in_progress_ = true;
+  }
+
+  if (reset_in_progress_) {
+    const auto is_stopped = isStopped(odom_buffer_, node_param_.th_stopped_velocity_mps);
+    if (is_stopped) {
+      // Plan new trajectory
+      planTrajectory();
+      reset_in_progress_ = false;
+    } else {
+      // Will keep current stop trajectory
+      RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Waiting for the vehicle to stop before generating a new trajectory.");
+    }
   }
 
   // StopTrajectory