From 010965b9b1f8ba8056fdbe5e6a2aa8eb3e68e6a0 Mon Sep 17 00:00:00 2001 From: Ahmed Ebrahim <36835765+ahmeddesokyebrahim@users.noreply.github.com> Date: Thu, 9 May 2024 15:54:27 +0300 Subject: [PATCH] fix(freespace_planner): fix motion_velocity_smoother error while parking (#6713) * 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 | 22 +++++++++++++------ 2 files changed, 16 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..1c2b4513e9c45 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(); @@ -476,6 +481,7 @@ void FreespacePlannerNode::onTimer() // StopTrajectory if (trajectory_.points.size() <= 1) { + is_new_parking_cycle_ = false; return; } @@ -487,6 +493,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()