From e7f019916ccf0fb137785abfeb54314ecab52efb Mon Sep 17 00:00:00 2001 From: AhmedEbrahim Date: Fri, 29 Mar 2024 16:49:24 +0200 Subject: [PATCH 1/2] fix(freespace_planner): prevent publishing stop trajectory with only one point in a new parking cycle Signed-off-by: AhmedEbrahim fix(freespace_planner): prevent publishing stop trajectory with only one point in a new parking cycle Signed-off-by: Ahmed Ebrahim --- .../freespace_planner_node.hpp | 1 + .../freespace_planner_node.cpp | 21 ++++++++++++------- 2 files changed, 15 insertions(+), 7 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 index e5dfda844e21e..2c10b1b491ace 100644 --- a/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp +++ b/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp @@ -134,6 +134,7 @@ class FreespacePlannerNode : public rclcpp::Node size_t target_index_; bool is_completed_ = false; bool reset_in_progress_ = false; + bool is_new_parking_cycle_ = true; 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 index 503216fa68bc8..1a1033f15325d 100644 --- a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp +++ b/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(); } @@ -447,13 +449,16 @@ void FreespacePlannerNode::onTimer() // Must stop before replanning any new trajectory const 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_) - : 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(); @@ -487,6 +492,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() From 01bfb5a9d3e29b658e82e1a8648e947a9c788d7c Mon Sep 17 00:00:00 2001 From: AhmedEbrahim Date: Fri, 29 Mar 2024 22:24:11 +0200 Subject: [PATCH 2/2] fix(freespace_planner): mark flag with true when trajectory is less than or equal 1 after first cycle - fixing unit test issue. Signed-off-by: AhmedEbrahim --- .../src/freespace_planner/freespace_planner_node.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp index 1a1033f15325d..1c2b4513e9c45 100644 --- a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp +++ b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp @@ -481,6 +481,7 @@ void FreespacePlannerNode::onTimer() // StopTrajectory if (trajectory_.points.size() <= 1) { + is_new_parking_cycle_ = false; return; }