Skip to content

Cheery-pick upstream #5712, add debug info #6

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
adivardi wants to merge 2 commits into
base: enway-devel
Choose a base branch
from
Open

Cheery-pick upstream #5712, add debug info #6

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
b14390e
Fix: reset controller_server loop_rate when missed desired rate #5712…
gennartan Nov 26, 2025
e0a099e
debug empty path issue
adivardi Dec 8, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 3 additions & 1 deletion nav2_controller/src/controller_server.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down Expand Up @@ -645,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;
Expand All @@ -657,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 {
Expand Down
7 changes: 5 additions & 2 deletions nav2_mppi_controller/src/path_handler.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand All @@ -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;
Expand Down
Loading