From b14390e4f0f2abb7badc9816e37f806e5ccebb7c Mon Sep 17 00:00:00 2001 From: Saitama Date: Wed, 26 Nov 2025 20:17:03 +0100 Subject: [PATCH 1/2] Fix: reset controller_server loop_rate when missed desired rate #5712 (#5723) When a single iteration takes longer than the expected period, this impacts the next iterations behavior since they will have less time to perform the control logic, thus increasing the actual controller_frequency. By resetting the loop_rate on sleep() failure, this ensures that each iteration will have a full period to exectue its logic. Signed-off-by: agennart Co-authored-by: agennart --- nav2_controller/src/controller_server.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 98a6eda8629..2a985282aa2 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -511,6 +511,7 @@ void ControllerServer::computeControl() get_logger(), "Control loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz.", controller_frequency_, 1 / cycle_duration.seconds()); + loop_rate.reset(); } } } catch (nav2_core::InvalidController & e) { From e0a099ec9eff0f97408026bd9c257f2d54a39471 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 8 Dec 2025 11:31:46 +0100 Subject: [PATCH 2/2] debug empty path issue Squashed commit of the following: commit 5f4c34575ef6b702e1938579fd04d8c22c2309b9 Author: Adi Vardi Date: Wed Nov 26 18:01:42 2025 +0100 add more info to debug commit 40363cbf00a85f85cc3fbae20eab75959a479fac Author: Adi Vardi Date: Tue Nov 25 13:43:29 2025 +0100 Add lower bound as well commit 27dc6a413244ccb4acb3e8e420ed7a993436bd1a Author: Adi Vardi Date: Tue Nov 25 13:40:33 2025 +0100 Add more debug info to controller error --- nav2_controller/src/controller_server.cpp | 3 ++- nav2_mppi_controller/src/path_handler.cpp | 7 +++++-- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 2a985282aa2..e3c32530f4c 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -646,7 +646,7 @@ void ControllerServer::computeAndPublishVelocity() // other types will not be resolved with more attempts } catch (nav2_core::NoValidControl & e) { if (failure_tolerance_ > 0 || failure_tolerance_ == -1.0) { - RCLCPP_WARN(this->get_logger(), "%s", e.what()); + RCLCPP_WARN(this->get_logger(), "Controller error, but still below failure tolerance. Error: %s", e.what()); cmd_vel_2d.twist.angular.x = 0; cmd_vel_2d.twist.angular.y = 0; cmd_vel_2d.twist.angular.z = 0; @@ -658,6 +658,7 @@ void ControllerServer::computeAndPublishVelocity() if ((now() - last_valid_cmd_time_).seconds() > failure_tolerance_ && failure_tolerance_ != -1.0) { + RCLCPP_ERROR(this->get_logger(), "Controller error and failure tolerance exceeded. Error: %s", e.what()); throw nav2_core::PatienceExceeded("Controller patience exceeded"); } } else { diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp index 55931f6bc62..966b2e7ff3e 100644 --- a/nav2_mppi_controller/src/path_handler.cpp +++ b/nav2_mppi_controller/src/path_handler.cpp @@ -128,7 +128,7 @@ nav_msgs::msg::Path PathHandler::transformPath( geometry_msgs::msg::PoseStamped global_pose = transformToGlobalPlanFrame(robot_pose); auto [transformed_plan, lower_bound] = getGlobalPlanConsideringBoundsInCostmapFrame(global_pose); - + const auto prev_size = global_plan_up_to_inversion_.poses.size(); prunePlan(global_plan_up_to_inversion_, lower_bound); if (enforce_path_inversion_ && inversion_locale_ != 0u) { @@ -140,7 +140,10 @@ nav_msgs::msg::Path PathHandler::transformPath( } if (transformed_plan.poses.empty()) { - throw nav2_core::InvalidPath("Resulting plan has 0 poses in it."); + throw nav2_core::InvalidPath("Resulting plan has 0 poses in it, while global plan has " + + std::to_string(global_plan_.poses.size()) + " poses. lower_bound index: " + + std::to_string(lower_bound - global_plan_.poses.begin()) + + ". prev_size: " + std::to_string(prev_size) + "."); } return transformed_plan;