From fa8ec657c57f10087062a49f9b1eb10bc00aac87 Mon Sep 17 00:00:00 2001 From: Dhruv Patel Date: Tue, 11 Nov 2025 21:21:27 +0100 Subject: [PATCH 1/6] Fix(macOS): Use Mach Precedence Policy for Soft Real-Time Thread Priority Signed-off-by: Dhruv Patel --- .../include/nav2_ros_common/node_utils.hpp | 33 +++++++++++++++++++ 1 file changed, 33 insertions(+) diff --git a/nav2_ros_common/include/nav2_ros_common/node_utils.hpp b/nav2_ros_common/include/nav2_ros_common/node_utils.hpp index 59ed17c27b1..0ea32007cb3 100644 --- a/nav2_ros_common/include/nav2_ros_common/node_utils.hpp +++ b/nav2_ros_common/include/nav2_ros_common/node_utils.hpp @@ -26,6 +26,15 @@ #include "rcl_interfaces/srv/list_parameters.hpp" #include "pluginlib/exceptions.hpp" +#ifdef __APPLE__ + #include + #include + #include +#else + #include + #include +#endif + using std::chrono::high_resolution_clock; using std::to_string; using std::string; @@ -337,6 +346,29 @@ inline std::string get_plugin_type_param( */ inline void setSoftRealTimePriority() { +#ifdef __APPLE__ + // macOS: Use Mach thread API (approximate real-time priority) + thread_port_t thread = pthread_mach_thread_np(pthread_self()); + thread_precedence_policy_data_t policy; + policy.importance = 63; // 0–63, higher = more priority + kern_return_t result = thread_policy_set( + thread, + THREAD_PRECEDENCE_POLICY, + (thread_policy_t)&policy, + THREAD_PRECEDENCE_POLICY_COUNT); + + if (result != KERN_SUCCESS) { + // Construct the message once with the full context + std::string errmsg = + "Failed to set THREAD_PRECEDENCE_POLICY on macOS. " + "Thread priority remains at default. " + "Mach Error Code: "; + + // Append the numerical result code and throw + throw std::runtime_error(errmsg + std::to_string(result)); + } +#else + // Linux: True real-time scheduling (requires privileges) sched_param sch; sch.sched_priority = 49; if (sched_setscheduler(0, SCHED_FIFO, &sch) == -1) { @@ -346,6 +378,7 @@ inline void setSoftRealTimePriority() "realtime prioritization! Error: "); throw std::runtime_error(errmsg + std::strerror(errno)); } +#endif } template From e8a13c8d8e8ae74f9beaf1627c6b5f3b32f685d2 Mon Sep 17 00:00:00 2001 From: Dhruv Patel Date: Tue, 11 Nov 2025 21:43:30 +0100 Subject: [PATCH 2/6] precommit checks Signed-off-by: Dhruv Patel --- .../include/nav2_ros_common/node_utils.hpp | 41 ++++++++++--------- 1 file changed, 22 insertions(+), 19 deletions(-) diff --git a/nav2_ros_common/include/nav2_ros_common/node_utils.hpp b/nav2_ros_common/include/nav2_ros_common/node_utils.hpp index 0ea32007cb3..134b1fb4cfb 100644 --- a/nav2_ros_common/include/nav2_ros_common/node_utils.hpp +++ b/nav2_ros_common/include/nav2_ros_common/node_utils.hpp @@ -256,8 +256,9 @@ inline ParamType declare_or_get_parameter( } auto return_value = param_interface - ->declare_parameter(parameter_name, rclcpp::ParameterValue{default_value}, - parameter_descriptor) + ->declare_parameter( + parameter_name, rclcpp::ParameterValue{default_value}, + parameter_descriptor) .get(); const bool no_param_override = param_interface->get_parameter_overrides().find(parameter_name) == @@ -265,8 +266,8 @@ inline ParamType declare_or_get_parameter( if (no_param_override) { if (warn_if_no_override) { RCLCPP_WARN_STREAM( - logger, - "Failed to get param " << parameter_name << " from overrides, using default value."); + logger, + "Failed to get param " << parameter_name << " from overrides, using default value."); } if (strict_param_loading) { std::string description = "Parameter " + parameter_name + @@ -305,7 +306,8 @@ inline ParamType declare_or_get_parameter( declare_parameter_if_not_declared(node, "strict_param_loading", rclcpp::ParameterValue(false)); bool strict_param_loading{false}; node->get_parameter("strict_param_loading", strict_param_loading); - return declare_or_get_parameter(node->get_logger(), node->get_node_parameters_interface(), + return declare_or_get_parameter( + node->get_logger(), node->get_node_parameters_interface(), parameter_name, default_value, warn_if_no_override, strict_param_loading, parameter_descriptor); } @@ -352,21 +354,21 @@ inline void setSoftRealTimePriority() thread_precedence_policy_data_t policy; policy.importance = 63; // 0–63, higher = more priority kern_return_t result = thread_policy_set( - thread, - THREAD_PRECEDENCE_POLICY, - (thread_policy_t)&policy, - THREAD_PRECEDENCE_POLICY_COUNT); + thread, + THREAD_PRECEDENCE_POLICY, + (thread_policy_t)&policy, + THREAD_PRECEDENCE_POLICY_COUNT); if (result != KERN_SUCCESS) { - // Construct the message once with the full context - std::string errmsg = - "Failed to set THREAD_PRECEDENCE_POLICY on macOS. " - "Thread priority remains at default. " - "Mach Error Code: "; + // Construct the message once with the full context + std::string errmsg = + "Failed to set THREAD_PRECEDENCE_POLICY on macOS. " + "Thread priority remains at default. " + "Mach Error Code: "; - // Append the numerical result code and throw - throw std::runtime_error(errmsg + std::to_string(result)); - } + // Append the numerical result code and throw + throw std::runtime_error(errmsg + std::to_string(result)); + } #else // Linux: True real-time scheduling (requires privileges) sched_param sch; @@ -418,8 +420,9 @@ inline void replaceOrAddArgument( std::vector & arguments, const std::string & option, const std::string & arg_name, const std::string & new_argument) { - auto argument = std::find_if(arguments.begin(), arguments.end(), - [arg_name](const std::string & value){return value.find(arg_name) != std::string::npos;}); + auto argument = std::find_if( + arguments.begin(), arguments.end(), + [arg_name](const std::string & value) {return value.find(arg_name) != std::string::npos;}); if (argument != arguments.end()) { *argument = new_argument; } else { From d2b4ddb4d45ea20a04f5b007390ee972bbfd615b Mon Sep 17 00:00:00 2001 From: Dhruv Patel Date: Fri, 14 Nov 2025 21:11:54 +0100 Subject: [PATCH 3/6] Fix: Use explicit cast to microseconds for sleep in LoopRate Signed-off-by: Dhruv Patel --- .../include/nav2_behavior_tree/utils/loop_rate.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/utils/loop_rate.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/utils/loop_rate.hpp index d7ebd5b4ac9..728855f9807 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/utils/loop_rate.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/utils/loop_rate.hpp @@ -61,7 +61,7 @@ class LoopRate auto time_to_sleep = next_interval - now; std::chrono::nanoseconds time_to_sleep_ns(time_to_sleep.nanoseconds()); // Sleep (can get interrupted by emitWakeUpSignal()) - tree_->sleep(time_to_sleep_ns); + tree_->sleep(std::chrono::duration_cast(time_to_sleep_ns)); return true; } From 8b069b91525166e0c04b5018a41a60b94d3a2a89 Mon Sep 17 00:00:00 2001 From: Dhruv Patel Date: Wed, 19 Nov 2025 19:49:36 +0100 Subject: [PATCH 4/6] Use of THREAD_TIME_CONSTRAINT_POLICY in setSoftRealTimePriority() Signed-off-by: Dhruv Patel --- .../include/nav2_ros_common/node_utils.hpp | 27 ++++++++++--------- 1 file changed, 15 insertions(+), 12 deletions(-) diff --git a/nav2_ros_common/include/nav2_ros_common/node_utils.hpp b/nav2_ros_common/include/nav2_ros_common/node_utils.hpp index 134b1fb4cfb..baafe5cd3c6 100644 --- a/nav2_ros_common/include/nav2_ros_common/node_utils.hpp +++ b/nav2_ros_common/include/nav2_ros_common/node_utils.hpp @@ -349,25 +349,28 @@ inline std::string get_plugin_type_param( inline void setSoftRealTimePriority() { #ifdef __APPLE__ - // macOS: Use Mach thread API (approximate real-time priority) + // macOS: Use Mach thread API to approximate real-time scheduling thread_port_t thread = pthread_mach_thread_np(pthread_self()); - thread_precedence_policy_data_t policy; - policy.importance = 63; // 0–63, higher = more priority + + thread_time_constraint_policy_data_t policy; + policy.period = 1000; // in microseconds (1 kHz loop) + policy.computation = 800; // expected compute time per period + policy.constraint = 1000; // max latency + policy.preemptible = 1; // allow preemption by higher-priority threads + kern_return_t result = thread_policy_set( thread, - THREAD_PRECEDENCE_POLICY, + THREAD_TIME_CONSTRAINT_POLICY, (thread_policy_t)&policy, - THREAD_PRECEDENCE_POLICY_COUNT); + THREAD_TIME_CONSTRAINT_POLICY_COUNT + ); if (result != KERN_SUCCESS) { - // Construct the message once with the full context std::string errmsg = - "Failed to set THREAD_PRECEDENCE_POLICY on macOS. " - "Thread priority remains at default. " - "Mach Error Code: "; - - // Append the numerical result code and throw - throw std::runtime_error(errmsg + std::to_string(result)); + "Failed to set THREAD_TIME_CONSTRAINT_POLICY on macOS. " + "Thread remains at default priority. Mach Error Code: " + + std::to_string(result); + throw std::runtime_error(errmsg); } #else // Linux: True real-time scheduling (requires privileges) From b8cb843ef0e9bbbd66d6c42b5f5023e018a2b3a0 Mon Sep 17 00:00:00 2001 From: Dhruv Patel Date: Wed, 19 Nov 2025 19:50:48 +0100 Subject: [PATCH 5/6] Pre-Commit Changes Signed-off-by: Dhruv Patel --- nav2_amcl/src/amcl_node.cpp | 33 ++-- nav2_amcl/src/map/map_cspace.cpp | 27 ++-- nav2_amcl/test/test_dynamic_parameters.cpp | 2 +- .../behavior_tree_engine.hpp | 12 +- .../nav2_behavior_tree/bt_action_node.hpp | 2 +- .../bt_action_server_impl.hpp | 32 ++-- .../action/compute_path_to_pose_action.hpp | 2 +- .../plugins/action/compute_route_action.hpp | 3 +- .../remove_in_collision_goals_action.hpp | 12 +- .../action/remove_passed_goals_action.hpp | 20 ++- .../condition/is_path_valid_condition.hpp | 4 +- .../condition/is_pose_occupied_condition.hpp | 8 +- .../condition/is_stopped_condition.hpp | 10 +- .../plugins/decorator/goal_updater_node.hpp | 10 +- .../action/compute_path_to_pose_action.cpp | 6 +- .../condition/are_poses_near_condition.cpp | 4 +- .../condition/is_pose_occupied_condition.cpp | 4 +- .../plugins/control/persistent_sequence.cpp | 4 +- .../plugins/decorator/goal_updater_node.cpp | 21 +-- .../src/behavior_tree_engine.cpp | 29 ++-- .../action/test_controller_selector_node.cpp | 4 +- .../test_remove_in_collision_goals_action.cpp | 10 +- .../test_remove_passed_goals_action.cpp | 10 +- .../plugins/condition/test_is_stopped.cpp | 3 +- nav2_behavior_tree/test/test_json_utils.cpp | 141 ++++++++++++------ .../plugins/drive_on_heading.hpp | 3 +- .../include/nav2_behaviors/timed_behavior.hpp | 3 +- .../src/navigators/navigate_through_poses.cpp | 38 +++-- .../src/navigators/navigate_to_pose.cpp | 29 ++-- nav2_collision_monitor/src/pointcloud.cpp | 5 +- nav2_collision_monitor/src/polygon.cpp | 2 +- nav2_collision_monitor/src/source.cpp | 2 +- .../src/velocity_polygon.cpp | 4 +- .../test/kinematics_test.cpp | 2 +- .../test/test_constrained_smoother.cpp | 4 +- nav2_controller/src/controller_server.cpp | 8 +- .../test/test_dynamic_parameters.cpp | 2 +- .../nav2_core/behavior_tree_navigator.hpp | 2 +- .../plugins/costmap_filters/binary_filter.cpp | 5 +- .../costmap_filters/costmap_filter.cpp | 3 +- .../costmap_filters/keepout_filter.cpp | 7 +- .../plugins/costmap_filters/speed_filter.cpp | 5 +- nav2_costmap_2d/plugins/obstacle_layer.cpp | 8 +- .../plugins/plugin_container_layer.cpp | 17 ++- nav2_costmap_2d/plugins/static_layer.cpp | 5 +- nav2_costmap_2d/src/costmap_2d_publisher.cpp | 8 +- nav2_costmap_2d/src/costmap_2d_ros.cpp | 25 ++-- .../test/integration/dyn_params_tests.cpp | 2 +- .../test/integration/footprint_tests.cpp | 2 +- .../test/integration/inflation_tests.cpp | 2 +- .../test/integration/obstacle_tests.cpp | 2 +- .../integration/plugin_container_tests.cpp | 110 +++++++++----- .../test/integration/range_tests.cpp | 2 +- .../integration/test_costmap_2d_publisher.cpp | 2 +- .../integration/test_costmap_subscriber.cpp | 6 +- .../test_costmap_topic_collision_checker.cpp | 2 +- .../test/unit/binary_filter_test.cpp | 9 +- .../test/unit/coordinate_transform_test.cpp | 2 +- .../test/unit/copy_window_test.cpp | 2 +- .../test/unit/costmap_conversion_test.cpp | 2 +- .../test/unit/costmap_cost_service_test.cpp | 2 +- .../test/unit/declare_parameter_test.cpp | 2 +- .../test/unit/denoise_layer_test.cpp | 2 +- .../simple_non_charging_dock.hpp | 12 +- .../opennav_docking/src/controller.cpp | 7 +- .../opennav_docking/src/docking_server.cpp | 7 +- .../opennav_docking/test/test_controller.cpp | 16 +- .../test/test_dock_database.cpp | 7 +- .../test/test_docking_server_unit.cpp | 2 +- .../opennav_docking/test/test_navigator.cpp | 17 ++- .../test/test_simple_charging_dock.cpp | 9 +- .../test/test_simple_non_charging_dock.cpp | 9 +- .../opennav_docking/test/test_utils.cpp | 2 +- .../dwb_critics/src/goal_align.cpp | 8 +- .../dwb_critics/src/oscillation.cpp | 7 +- .../dwb_plugins/src/kinematic_parameters.cpp | 12 +- .../src/following_server.cpp | 14 +- .../src/graceful_controller.cpp | 7 +- .../src/parameter_handler.cpp | 8 +- .../src/lifecycle_manager.cpp | 3 +- nav2_map_server/src/map_io.cpp | 5 +- .../src/vo_server/vector_object_server.cpp | 11 +- .../test/component/test_map_saver_node.cpp | 2 +- .../test/component/test_map_server_node.cpp | 2 +- .../unit/test_costmap_filter_info_server.cpp | 2 +- nav2_map_server/test/unit/test_map_io.cpp | 2 +- .../benchmark/optimizer_benchmark.cpp | 3 +- .../nav2_mppi_controller/motion_models.hpp | 12 +- .../nav2_mppi_controller/tools/utils.hpp | 27 ++-- nav2_mppi_controller/src/critic_manager.cpp | 2 +- .../src/critics/constraint_critic.cpp | 2 +- .../src/critics/cost_critic.cpp | 27 ++-- .../src/critics/goal_angle_critic.cpp | 2 +- .../src/critics/goal_critic.cpp | 2 +- .../src/critics/obstacles_critic.cpp | 14 +- .../src/critics/path_align_critic.cpp | 15 +- .../src/critics/path_angle_critic.cpp | 10 +- .../src/critics/prefer_forward_critic.cpp | 4 +- nav2_mppi_controller/src/noise_generator.cpp | 8 +- nav2_mppi_controller/src/optimizer.cpp | 18 ++- nav2_mppi_controller/src/path_handler.cpp | 11 +- .../test/controller_state_transition_test.cpp | 2 +- .../test/critic_manager_test.cpp | 2 +- nav2_mppi_controller/test/critics_tests.cpp | 6 +- nav2_mppi_controller/test/models_test.cpp | 2 +- .../test/motion_model_tests.cpp | 2 +- .../test/noise_generator_test.cpp | 8 +- .../test/optimizer_smoke_test.cpp | 5 +- .../test/optimizer_unit_tests.cpp | 26 ++-- .../test/parameter_handler_test.cpp | 22 ++- .../test/path_handler_test.cpp | 2 +- .../test/trajectory_visualizer_tests.cpp | 2 +- nav2_mppi_controller/test/utils_test.cpp | 19 ++- nav2_navfn_planner/src/navfn.cpp | 12 +- nav2_navfn_planner/src/navfn_planner.cpp | 2 +- .../test/test_dynamic_parameters.cpp | 2 +- nav2_planner/src/planner_server.cpp | 3 +- nav2_planner/test/test_dynamic_parameters.cpp | 2 +- .../src/collision_checker.cpp | 5 +- .../src/parameter_handler.cpp | 12 +- .../src/path_handler.cpp | 8 +- .../test/test_regulated_pp.cpp | 2 +- .../nav2_ros_common/interface_factories.hpp | 5 +- .../nav2_ros_common/service_client.hpp | 8 +- .../nav2_ros_common/service_server.hpp | 5 +- .../nav2_ros_common/simple_action_server.hpp | 8 +- nav2_ros_common/test/test_node_utils.cpp | 2 +- nav2_ros_common/test/test_service_client.cpp | 6 +- nav2_ros_common/test/test_service_server.cpp | 6 +- .../test/test_shim_controller.cpp | 2 +- .../include/nav2_route/goal_intent_search.hpp | 11 +- .../plugins/route_operation_client.hpp | 6 +- nav2_route/src/goal_intent_extractor.cpp | 4 +- .../edge_cost_functions/costmap_scorer.cpp | 3 +- .../dynamic_edges_scorer.cpp | 7 +- .../route_operations/collision_monitor.cpp | 3 +- .../route_operations/rerouting_service.cpp | 5 +- nav2_route/src/route_server.cpp | 3 +- nav2_rviz_plugins/src/utils.cpp | 14 +- .../nav2_smac_planner/goal_manager.hpp | 13 +- nav2_smac_planner/src/a_star.cpp | 3 +- nav2_smac_planner/src/analytic_expansion.cpp | 6 +- nav2_smac_planner/src/collision_checker.cpp | 2 +- nav2_smac_planner/src/node_lattice.cpp | 5 +- nav2_smac_planner/src/smac_planner_2d.cpp | 2 +- nav2_smac_planner/src/smac_planner_hybrid.cpp | 25 ++-- .../src/smac_planner_lattice.cpp | 24 +-- nav2_smac_planner/src/smoother.cpp | 5 +- nav2_smac_planner/test/test_a_star.cpp | 25 ++-- .../test/test_collision_checker.cpp | 2 +- .../test/test_costmap_downsampler.cpp | 2 +- nav2_smac_planner/test/test_goal_manager.cpp | 7 +- nav2_smac_planner/test/test_node2d.cpp | 2 +- nav2_smac_planner/test/test_nodebasic.cpp | 2 +- nav2_smac_planner/test/test_nodehybrid.cpp | 2 +- nav2_smac_planner/test/test_nodelattice.cpp | 2 +- nav2_smac_planner/test/test_smac_2d.cpp | 2 +- nav2_smac_planner/test/test_smac_hybrid.cpp | 4 +- nav2_smac_planner/test/test_smac_lattice.cpp | 10 +- nav2_smac_planner/test/test_smoother.cpp | 2 +- nav2_smoother/src/savitzky_golay_smoother.cpp | 23 +-- .../test/test_savitzky_golay_smoother.cpp | 2 +- nav2_smoother/test/test_simple_smoother.cpp | 2 +- .../behavior_tree/test_behavior_tree_node.cpp | 61 ++++---- .../localization/test_localization_node.cpp | 2 +- .../src/theta_star_planner.cpp | 2 +- .../test/test_theta_star.cpp | 2 +- .../include/nav2_util/geometry_utils.hpp | 9 +- .../include/nav2_util/odometry_utils.hpp | 28 ++-- nav2_util/src/path_utils.cpp | 6 +- nav2_util/src/robot_utils.cpp | 2 +- .../test/test_base_footprint_publisher.cpp | 2 +- nav2_util/test/test_geometry_utils.cpp | 14 +- nav2_util/test/test_lifecycle_cli_node.cpp | 2 +- nav2_util/test/test_odometry_utils.cpp | 2 +- nav2_util/test/test_path_utils.cpp | 26 ++-- .../src/velocity_smoother.cpp | 13 +- .../test/test_velocity_smoother.cpp | 6 +- .../test/test_dynamic_parameters.cpp | 2 +- .../test/test_task_executors.cpp | 2 +- 180 files changed, 981 insertions(+), 697 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 15c572d2ed9..27125f67a07 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1008,32 +1008,32 @@ rcl_interfaces::msg::SetParametersResult AmclNode::validateParameterUpdatesCallb (param_name != "laser_min_range" || param_name != "laser_max_range")) { RCLCPP_WARN( - get_logger(), "The value of parameter '%s' is incorrectly set to %f, " - "it should be >=0. Ignoring parameter update.", - param_name.c_str(), parameter.as_double()); + get_logger(), "The value of parameter '%s' is incorrectly set to %f, " + "it should be >=0. Ignoring parameter update.", + param_name.c_str(), parameter.as_double()); result.successful = false; } } else if (param_type == ParameterType::PARAMETER_INTEGER) { if (parameter.as_int() <= 0.0 && param_name == "resample_interval") { RCLCPP_WARN( - get_logger(), "The value of resample_interval is incorrectly set, " - "it should be >0. Ignoring parameter update."); + get_logger(), "The value of resample_interval is incorrectly set, " + "it should be >0. Ignoring parameter update."); result.successful = false; } else if (parameter.as_int() < 0.0) { RCLCPP_WARN( - get_logger(), "The value of parameter '%s' is incorrectly set to %ld, " - "it should be >=0. Ignoring parameter update.", - param_name.c_str(), parameter.as_int()); + get_logger(), "The value of parameter '%s' is incorrectly set to %ld, " + "it should be >=0. Ignoring parameter update.", + param_name.c_str(), parameter.as_int()); result.successful = false; } else if (param_name == "max_particles" && parameter.as_int() < min_particles_) { RCLCPP_WARN( - get_logger(), "The value of max_particles is incorrectly set, " - "it should be larger than min_particles. Ignoring parameter update."); + get_logger(), "The value of max_particles is incorrectly set, " + "it should be larger than min_particles. Ignoring parameter update."); result.successful = false; } else if (param_name == "min_particles" && parameter.as_int() > max_particles_) { RCLCPP_WARN( - get_logger(), "The value of min_particles is incorrectly set, " - "it should be smaller than max particles. Ignoring parameter update."); + get_logger(), "The value of min_particles is incorrectly set, " + "it should be smaller than max particles. Ignoring parameter update."); result.successful = false; } } @@ -1397,17 +1397,20 @@ AmclNode::initServices() { global_loc_srv_ = create_service( "reinitialize_global_localization", - std::bind(&AmclNode::globalLocalizationCallback, this, std::placeholders::_1, + std::bind( + &AmclNode::globalLocalizationCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); initial_guess_srv_ = create_service( "set_initial_pose", - std::bind(&AmclNode::initialPoseReceivedSrv, this, std::placeholders::_1, std::placeholders::_2, + std::bind( + &AmclNode::initialPoseReceivedSrv, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); nomotion_update_srv_ = create_service( "request_nomotion_update", - std::bind(&AmclNode::nomotionUpdateCallback, this, std::placeholders::_1, std::placeholders::_2, + std::bind( + &AmclNode::nomotionUpdateCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); } diff --git a/nav2_amcl/src/map/map_cspace.cpp b/nav2_amcl/src/map/map_cspace.cpp index c59eac96327..7b179b9e0f7 100644 --- a/nav2_amcl/src/map/map_cspace.cpp +++ b/nav2_amcl/src/map/map_cspace.cpp @@ -132,7 +132,8 @@ void enqueue( map->cells[map_index].occ_dist = distance * map->scale; - Q.emplace(CellData{map, static_cast(i), static_cast(j), + Q.emplace( + CellData{map, static_cast(i), static_cast(j), static_cast(src_i), static_cast(src_j), map->cells[map_index].occ_dist}); @@ -179,27 +180,27 @@ void map_update_cspace(map_t * map, double max_occ_dist) CellData current_cell = Q.top(); if (current_cell.i_ > 0) { enqueue( - map, current_cell.i_ - 1, current_cell.j_, - current_cell.src_i_, current_cell.src_j_, - Q, cdm, marked); + map, current_cell.i_ - 1, current_cell.j_, + current_cell.src_i_, current_cell.src_j_, + Q, cdm, marked); } if (current_cell.j_ > 0) { enqueue( - map, current_cell.i_, current_cell.j_ - 1, - current_cell.src_i_, current_cell.src_j_, - Q, cdm, marked); + map, current_cell.i_, current_cell.j_ - 1, + current_cell.src_i_, current_cell.src_j_, + Q, cdm, marked); } if (static_cast(current_cell.i_) < map->size_x - 1) { enqueue( - map, current_cell.i_ + 1, current_cell.j_, - current_cell.src_i_, current_cell.src_j_, - Q, cdm, marked); + map, current_cell.i_ + 1, current_cell.j_, + current_cell.src_i_, current_cell.src_j_, + Q, cdm, marked); } if (static_cast(current_cell.j_) < map->size_y - 1) { enqueue( - map, current_cell.i_, current_cell.j_ + 1, - current_cell.src_i_, current_cell.src_j_, - Q, cdm, marked); + map, current_cell.i_, current_cell.j_ + 1, + current_cell.src_i_, current_cell.src_j_, + Q, cdm, marked); } Q.pop(); diff --git a/nav2_amcl/test/test_dynamic_parameters.cpp b/nav2_amcl/test/test_dynamic_parameters.cpp index e2234d5f577..5f7ec468242 100644 --- a/nav2_amcl/test/test_dynamic_parameters.cpp +++ b/nav2_amcl/test/test_dynamic_parameters.cpp @@ -140,7 +140,7 @@ TEST(WPTest, test_dynamic_parameters) EXPECT_EQ(amcl->get_parameter("min_particles").as_int(), 100); } -int main(int argc, char **argv) +int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp index 2371975317c..e6618b63739 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp @@ -93,12 +93,12 @@ class BehaviorTreeEngine */ std::string extractBehaviorTreeID(const std::string & file_or_id); - /** - * @brief Function to create a BT from a BehaviorTree ID - * @param tree_id BehaviorTree ID - * @param blackboard Blackboard for BT - * @return BT::Tree Created behavior tree - */ + /** + * @brief Function to create a BT from a BehaviorTree ID + * @param tree_id BehaviorTree ID + * @param blackboard Blackboard for BT + * @return BT::Tree Created behavior tree + */ BT::Tree createTree( const std::string & tree_id, BT::Blackboard::Ptr blackboard); diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index 682dc7a9a09..8ecee6aaa7e 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -404,7 +404,7 @@ class BtActionNode : public BT::ActionNodeBase }; send_goal_options.feedback_callback = [this](typename rclcpp_action::ClientGoalHandle::SharedPtr, - const std::shared_ptr feedback) { + const std::shared_ptr feedback) { feedback_ = feedback; emitWakeUpSignal(); }; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 74d3700620e..40c220ec33a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -77,9 +77,10 @@ BtActionServer::BtActionServer( }; if (node->has_parameter("error_code_names")) { - throw std::runtime_error("parameter 'error_code_names' has been replaced by " - " 'error_code_name_prefixes' and MUST be removed.\n" - " Please review migration guide and update your configuration."); + throw std::runtime_error( + "parameter 'error_code_names' has been replaced by " + " 'error_code_name_prefixes' and MUST be removed.\n" + " Please review migration guide and update your configuration."); } // Declare and get error code name prefixes parameter @@ -100,8 +101,9 @@ BtActionServer::BtActionServer( << "Make sure these match your BT and there are not other sources of error codes you" << "reported to your application"); } else { - RCLCPP_INFO_STREAM(logger_, "Error_code parameters were set to:" - << error_code_name_prefixes_str); + RCLCPP_INFO_STREAM( + logger_, "Error_code parameters were set to:" + << error_code_name_prefixes_str); } } @@ -123,7 +125,8 @@ bool BtActionServer::on_configure() // Use suffix '_rclcpp_node' to keep parameter file consistency #1773 auto new_arguments = node->get_node_options().arguments(); - nav2::replaceOrAddArgument(new_arguments, "-r", "__node", std::string("__node:=") + + nav2::replaceOrAddArgument( + new_arguments, "-r", "__node", std::string("__node:=") + std::string(node->get_name()) + "_" + client_node_name + "_rclcpp_node"); auto options = node->get_node_options(); options = options.arguments(new_arguments); @@ -271,7 +274,8 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml continue; } if (registered_ids.count(id)) { - RCLCPP_WARN(logger_, "Skipping conflicting BT file %s (duplicate ID %s)", + RCLCPP_WARN( + logger_, "Skipping conflicting BT file %s (duplicate ID %s)", entry.path().c_str(), id.c_str()); continue; } @@ -311,7 +315,8 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml register_all_bt_files(); } } catch (const std::exception & e) { - setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE, + setInternalError( + ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE, "Exception registering behavior trees: " + std::string(e.what())); return false; } @@ -327,10 +332,11 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml blackboard->set("server_timeout", default_server_timeout_); blackboard->set("bt_loop_duration", bt_loop_duration_); blackboard->set( - "wait_for_service_timeout", wait_for_service_timeout_); + "wait_for_service_timeout", wait_for_service_timeout_); } } catch (const std::exception & e) { - setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE, + setInternalError( + ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE, std::string("Exception when creating BT tree from ID: ") + e.what()); return false; } @@ -405,7 +411,8 @@ void BtActionServer::executeCallback() case nav2_behavior_tree::BtStatus::FAILED: action_server_->terminate_current(result); - RCLCPP_ERROR(logger_, "Goal failed error_code:%d error_msg:'%s'", result->error_code, + RCLCPP_ERROR( + logger_, "Goal failed error_code:%d error_msg:'%s'", result->error_code, result->error_msg.c_str()); break; @@ -425,7 +432,8 @@ void BtActionServer::setInternalError( { internal_error_code_ = error_code; internal_error_msg_ = error_msg; - RCLCPP_ERROR(logger_, "Setting internal error error_code:%d, error_msg:%s", + RCLCPP_ERROR( + logger_, "Setting internal error error_code:%d, error_msg:%s", internal_error_code_, internal_error_msg_.c_str()); } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp index ca2d623cbff..4f516470c13 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp @@ -94,7 +94,7 @@ class ComputePathToPoseAction : public BtActionNode( "start", "Used as the planner start pose instead of the current robot pose, if use_start is" - " not false (i.e. not provided or set to true)"), + " not false (i.e. not provided or set to true)"), BT::InputPort( "use_start", "For using or not using (i.e. ignoring) the provided start pose"), BT::InputPort( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_route_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_route_action.hpp index 7fbbfdf2741..b8c02f8b1ae 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_route_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_route_action.hpp @@ -103,7 +103,8 @@ class ComputeRouteAction : public BtActionNode "use_poses", false, "Whether to use poses or IDs for start and goal"), BT::OutputPort( "route", "The route computed by ComputeRoute node"), - BT::OutputPort("planning_time", + BT::OutputPort( + "planning_time", "Time taken to compute route"), BT::OutputPort("path", "Path created by ComputeRoute node"), BT::OutputPort( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp index a76394e2ffe..71a924cdfc1 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp @@ -64,7 +64,8 @@ class RemoveInCollisionGoals : public BtServiceNode return providedBasicPorts( { - BT::InputPort("input_goals", + BT::InputPort( + "input_goals", "Original goals to remove from"), BT::InputPort( "cost_threshold", 254.0, @@ -73,11 +74,14 @@ class RemoveInCollisionGoals : public BtServiceNode BT::InputPort( "consider_unknown_as_obstacle", false, "Whether to consider unknown cost as obstacle"), - BT::OutputPort("output_goals", + BT::OutputPort( + "output_goals", "Goals with in-collision goals removed"), - BT::InputPort>("input_waypoint_statuses", + BT::InputPort>( + "input_waypoint_statuses", "Original waypoint_statuses to mark waypoint status from"), - BT::OutputPort>("output_waypoint_statuses", + BT::OutputPort>( + "output_waypoint_statuses", "Waypoint_statuses with in-collision waypoints marked") }); } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp index a651318379f..de4e366f802 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp @@ -56,16 +56,20 @@ class RemovePassedGoals : public BT::ActionNodeBase BT::RegisterJsonDefinition>(); return { - BT::InputPort("input_goals", - "Original goals to remove viapoints from"), - BT::OutputPort("output_goals", - "Goals with passed viapoints removed"), + BT::InputPort( + "input_goals", + "Original goals to remove viapoints from"), + BT::OutputPort( + "output_goals", + "Goals with passed viapoints removed"), BT::InputPort("radius", 0.5, "radius to goal for it to be considered for removal"), BT::InputPort("robot_base_frame", "Robot base frame"), - BT::InputPort>("input_waypoint_statuses", - "Original waypoint_statuses to mark waypoint status from"), - BT::OutputPort>("output_waypoint_statuses", - "Waypoint_statuses with passed waypoints marked") + BT::InputPort>( + "input_waypoint_statuses", + "Original waypoint_statuses to mark waypoint status from"), + BT::OutputPort>( + "output_waypoint_statuses", + "Waypoint_statuses with passed waypoints marked") }; } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp index 334eaefd6f0..d5df8b2838f 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp @@ -75,8 +75,8 @@ class IsPathValidCondition : public BT::ConditionNode BT::InputPort("server_timeout"), BT::InputPort("max_cost", 253, "Maximum cost of the path"), BT::InputPort( - "consider_unknown_as_obstacle", false, - "Whether to consider unknown cost as obstacle") + "consider_unknown_as_obstacle", false, + "Whether to consider unknown cost as obstacle") }; } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.hpp index 7ad6bb014a5..b29b4b90bd0 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.hpp @@ -74,12 +74,12 @@ class IsPoseOccupiedCondition : public BT::ConditionNode BT::InputPort("pose", "Pose to check if occupied"), BT::InputPort("service_name", "global_costmap/get_cost_global_costmap"), BT::InputPort( - "cost_threshold", 254.0, - "Cost threshold for considering a pose occupied"), + "cost_threshold", 254.0, + "Cost threshold for considering a pose occupied"), BT::InputPort("use_footprint", true, "Whether to use footprint cost"), BT::InputPort( - "consider_unknown_as_obstacle", false, - "Whether to consider unknown cost as obstacle"), + "consider_unknown_as_obstacle", false, + "Whether to consider unknown cost as obstacle"), BT::InputPort("server_timeout"), }; } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp index 2d1bbe31449..8886de27608 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp @@ -71,10 +71,12 @@ class IsStoppedCondition : public BT::ConditionNode BT::RegisterJsonDefinition(); return { - BT::InputPort("velocity_threshold", 0.01, - "Velocity threshold below which robot is considered stopped"), - BT::InputPort("duration_stopped", 1000ms, - "Duration (ms) the velocity must remain below the threshold"), + BT::InputPort( + "velocity_threshold", 0.01, + "Velocity threshold below which robot is considered stopped"), + BT::InputPort( + "duration_stopped", 1000ms, + "Duration (ms) the velocity must remain below the threshold"), }; } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp index 61a909c53c6..e17ecf11f23 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp @@ -64,10 +64,12 @@ class GoalUpdater : public BT::DecoratorNode return { BT::InputPort("input_goal", "Original Goal"), BT::InputPort("input_goals", "Original Goals"), - BT::OutputPort("output_goal", - "Received Goal by subscription"), - BT::OutputPort("output_goals", - "Received Goals by subscription") + BT::OutputPort( + "output_goal", + "Received Goal by subscription"), + BT::OutputPort( + "output_goals", + "Received Goals by subscription") }; } diff --git a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp index 0d80527c913..3ff618a007b 100644 --- a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp @@ -41,9 +41,9 @@ void ComputePathToPoseAction::on_tick() // in case we don't have a "start" pose goal_.use_start = false; RCLCPP_ERROR( - node_->get_logger(), - "use_start is set to true but no start pose was provided, falling back to default " - "behavior, i.e. using the current robot pose"); + node_->get_logger(), + "use_start is set to true but no start pose was provided, falling back to default " + "behavior, i.e. using the current robot pose"); } } else { // else if "use_start" is not provided, but "start" is, then use it in order to not change diff --git a/nav2_behavior_tree/plugins/condition/are_poses_near_condition.cpp b/nav2_behavior_tree/plugins/condition/are_poses_near_condition.cpp index b915f6e5b7a..c4f570eed60 100644 --- a/nav2_behavior_tree/plugins/condition/are_poses_near_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/are_poses_near_condition.cpp @@ -63,9 +63,9 @@ bool ArePosesNearCondition::arePosesNearby() if (pose1.header.frame_id != pose2.header.frame_id) { if (!nav2_util::transformPoseInTargetFrame( - pose1, pose1, *tf_, global_frame_, transform_tolerance_) || + pose1, pose1, *tf_, global_frame_, transform_tolerance_) || !nav2_util::transformPoseInTargetFrame( - pose2, pose2, *tf_, global_frame_, transform_tolerance_)) + pose2, pose2, *tf_, global_frame_, transform_tolerance_)) { RCLCPP_ERROR(node_->get_logger(), "Failed to transform poses to the same frame"); return false; diff --git a/nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.cpp b/nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.cpp index 8d253e5e0a1..855b66e011a 100644 --- a/nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_pose_occupied_condition.cpp @@ -58,14 +58,14 @@ BT::NodeStatus IsPoseOccupiedCondition::tick() auto response = client_->invoke(request, server_timeout_); - if(!response->success) { + if (!response->success) { RCLCPP_ERROR( node_->get_logger(), "GetCosts service call failed"); return BT::NodeStatus::FAILURE; } - if((response->costs[0] == 255 && !consider_unknown_as_obstacle_) || + if ((response->costs[0] == 255 && !consider_unknown_as_obstacle_) || response->costs[0] < cost_threshold_) { return BT::NodeStatus::FAILURE; diff --git a/nav2_behavior_tree/plugins/control/persistent_sequence.cpp b/nav2_behavior_tree/plugins/control/persistent_sequence.cpp index f38b78dc85d..2a73639b99b 100644 --- a/nav2_behavior_tree/plugins/control/persistent_sequence.cpp +++ b/nav2_behavior_tree/plugins/control/persistent_sequence.cpp @@ -31,8 +31,8 @@ BT::NodeStatus PersistentSequenceNode::tick() int current_child_idx; if (!getInput("current_child_idx", current_child_idx)) { throw BT::RuntimeError( - "Missing required input [current_child_idx] in PersistentSequenceNode. " - "Set via