From 510317e92889014eb53194d529b8f52be200bd01 Mon Sep 17 00:00:00 2001 From: Vincent Richard Date: Fri, 12 Jan 2024 16:00:17 +0900 Subject: [PATCH 1/3] feat(freespace_planner): only plan if ego is stopped Fixes #5936 Signed-off-by: Vincent Richard --- .../freespace_planner_node.hpp | 1 + .../freespace_planner_node.cpp | 19 ++++++++++++++++--- 2 files changed, 17 insertions(+), 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 index 9c13f14d180d0..e5dfda844e21e 100644 --- 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 index 6400ae6d135ba..5a35017182146 100644 --- 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 From 17f5674856e8b508454d7576d12ae8868a7277dc Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 12 Jan 2024 07:11:37 +0000 Subject: [PATCH 2/3] style(pre-commit): autofix --- .../src/freespace_planner/freespace_planner_node.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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 5a35017182146..8e91484af1564 100644 --- a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp +++ b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp @@ -465,7 +465,9 @@ void FreespacePlannerNode::onTimer() 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."); + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 1000, + "Waiting for the vehicle to stop before generating a new trajectory."); } } From 81790e1aa9f7634f015f461fde40ce88ba74117a Mon Sep 17 00:00:00 2001 From: Vincent Richard Date: Thu, 8 Feb 2024 15:16:39 +0900 Subject: [PATCH 3/3] Update planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp Add const Co-authored-by: Kosuke Takeuchi --- .../src/freespace_planner/freespace_planner_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 8e91484af1564..f903fbc8e935e 100644 --- a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp +++ b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp @@ -442,7 +442,7 @@ void FreespacePlannerNode::onTimer() } // Must stop before replanning any new trajectory - bool is_reset_required = !reset_in_progress_ && isPlanRequired(); + 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()