fix(freespace_planner): prevent publishing stop trajectory with only one point in a new parking cycle

Signed-off-by: Ahmed Ebrahim <ahmed.ebrahim@leodrive.ai>

fix(freespace_planner): prevent publishing stop trajectory with only one point in a new parking cycle

Signed-off-by: Ahmed Ebrahim <ahmed.ebrahim@leodrive.ai>

Author: ahmeddesokyebrahim

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 is_new_parking_cycle_ = true;
 
   LaneletRoute::ConstSharedPtr route_;
   OccupancyGrid::ConstSharedPtr occupancy_grid_;

planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp

@@ -321,6 +321,8 @@ void FreespacePlannerNode::onRoute(const LaneletRoute::ConstSharedPtr msg)
   goal_pose_.header = msg->header;
   goal_pose_.pose = msg->goal_pose;
 
+  is_new_parking_cycle_ = true;
+
   reset();
 }
 
@@ -445,14 +447,16 @@ void FreespacePlannerNode::onTimer()
   }
 
   if (isPlanRequired()) {
-    // Stop before planning new trajectory
-    const auto stop_trajectory = partial_trajectory_.points.empty()
-                                   ? createStopTrajectory(current_pose_)
-                                   : createStopTrajectory(partial_trajectory_);
-    trajectory_pub_->publish(stop_trajectory);
-    debug_pose_array_pub_->publish(trajectory2PoseArray(stop_trajectory));
-    debug_partial_pose_array_pub_->publish(trajectory2PoseArray(stop_trajectory));
-
+    // Stop before planning new trajectory, except in a new parking cycle as the vehicle already
+    // stops.
+    if (!is_new_parking_cycle_) {
+      const auto stop_trajectory = partial_trajectory_.points.empty()
+                                     ? createStopTrajectory(current_pose_)
+                                     : createStopTrajectory(partial_trajectory_);
+      trajectory_pub_->publish(stop_trajectory);
+      debug_pose_array_pub_->publish(trajectory2PoseArray(stop_trajectory));
+      debug_partial_pose_array_pub_->publish(trajectory2PoseArray(stop_trajectory));
+    }
     reset();
 
     // Plan new trajectory
@@ -472,6 +476,8 @@ void FreespacePlannerNode::onTimer()
   trajectory_pub_->publish(partial_trajectory_);
   debug_pose_array_pub_->publish(trajectory2PoseArray(trajectory_));
   debug_partial_pose_array_pub_->publish(trajectory2PoseArray(partial_trajectory_));
+
+  is_new_parking_cycle_ = false;
 }
 
 void FreespacePlannerNode::planTrajectory()