From 26363716536425ff6479610c18639c853e7537be Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 5 Mar 2024 19:28:08 +0100 Subject: [PATCH 1/3] Fix BT.CPP import (#4165) Signed-off-by: Tony Najjar --- nav2_behavior_tree/src/generate_nav2_tree_nodes_xml.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nav2_behavior_tree/src/generate_nav2_tree_nodes_xml.cpp b/nav2_behavior_tree/src/generate_nav2_tree_nodes_xml.cpp index 01bdf6a6854..4992e2e7775 100644 --- a/nav2_behavior_tree/src/generate_nav2_tree_nodes_xml.cpp +++ b/nav2_behavior_tree/src/generate_nav2_tree_nodes_xml.cpp @@ -17,10 +17,10 @@ #include #include -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" -#include "behaviortree_cpp_v3/utils/shared_library.h" -#include "behaviortree_cpp_v3/xml_parsing.h" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/utils/shared_library.h" +#include "behaviortree_cpp/xml_parsing.h" #include "plugins_list.hpp" From c9d9b5c698d1f5aff060806481d18ba7e9b18cb2 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 7 Mar 2024 14:12:33 -0800 Subject: [PATCH 2/3] Update default recommendation from Obstacles to Cost critic in MPPI (#4170) Signed-off-by: Steve Macenski --- nav2_mppi_controller/README.md | 34 +++++++++++++++++----------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 2a9a25a8638..3bf779e0938 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -201,7 +201,7 @@ controller_server: time_step: 3 AckermannConstraints: min_turning_r: 0.2 - critics: ["ConstraintCritic", "ObstaclesCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] + critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] ConstraintCritic: enabled: true cost_power: 1 @@ -221,24 +221,24 @@ controller_server: cost_power: 1 cost_weight: 5.0 threshold_to_consider: 0.5 - ObstaclesCritic: - enabled: true - cost_power: 1 - repulsion_weight: 1.5 - critical_weight: 20.0 - consider_footprint: false - collision_cost: 10000.0 - collision_margin_distance: 0.1 - near_goal_distance: 0.5 - # Option to replace Obstacles and use Cost instead - # CostCritic: + # ObstaclesCritic: # enabled: true # cost_power: 1 - # cost_weight: 3.81 - # critical_cost: 300.0 - # consider_footprint: true - # collision_cost: 1000000.0 - # near_goal_distance: 1.0 + # repulsion_weight: 1.5 + # critical_weight: 20.0 + # consider_footprint: false + # collision_cost: 10000.0 + # collision_margin_distance: 0.1 + # near_goal_distance: 0.5 + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 3.81 + critical_cost: 300.0 + consider_footprint: true + collision_cost: 1000000.0 + near_goal_distance: 1.0 + PathAlignCritic: PathAlignCritic: enabled: true cost_power: 1 From 47374622dee01a27e5f9b8ae08f3d19a15de9b3a Mon Sep 17 00:00:00 2001 From: Reza Kermani Date: Fri, 8 Mar 2024 13:25:25 -0500 Subject: [PATCH 3/3] nav2_controller: add loop rate log (#4171) * update smac_planner README Signed-off-by: ARK3r * added current controller loop rate logging Signed-off-by: ARK3r * linting Signed-off-by: ARK3r * uncrustify lint Signed-off-by: ARK3r * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski --------- Signed-off-by: ARK3r Signed-off-by: Steve Macenski Co-authored-by: Steve Macenski --- nav2_controller/src/controller_server.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index fa25e0b1379..85aae171cc7 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -419,6 +419,7 @@ void ControllerServer::computeControl() { std::lock_guard lock(dynamic_params_lock_); + auto start_time = this->now(); RCLCPP_INFO(get_logger(), "Received a goal, begin computing control effort."); try { @@ -479,10 +480,12 @@ void ControllerServer::computeControl() break; } + auto cycle_duration = this->now() - start_time; if (!loop_rate.sleep()) { RCLCPP_WARN( - get_logger(), "Control loop missed its desired rate of %.4fHz", - controller_frequency_); + get_logger(), + "Control loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz.", + controller_frequency_, 1 / cycle_duration.seconds()); } } } catch (nav2_core::InvalidController & e) {