From a6bcb349b6e5ab755c3d036149f042d5d3b3873b Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Fri, 26 Jan 2024 09:07:32 +0100 Subject: [PATCH 1/5] Add generate_nav2_tree_nodes_xml Signed-off-by: Davide Faconti --- nav2_behavior_tree/CMakeLists.txt | 10 ++++ nav2_behavior_tree/plugins_list.hpp.in | 6 ++ .../src/generate_nav2_tree_nodes_xml.cpp | 47 +++++++++++++++ .../behavior_tree/test_behavior_tree_node.cpp | 58 ++----------------- 4 files changed, 69 insertions(+), 52 deletions(-) create mode 100644 nav2_behavior_tree/plugins_list.hpp.in create mode 100644 nav2_behavior_tree/src/generate_nav2_tree_nodes_xml.cpp diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 96fc154013..42911c68ea 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -219,6 +219,14 @@ install(TARGETS ${library_name} RUNTIME DESTINATION bin ) +add_executable(generate_nav2_tree_nodes_xml src/generate_nav2_tree_nodes_xml.cpp) +ament_target_dependencies(generate_nav2_tree_nodes_xml ${dependencies}) +# we will embed the list of plugin names inside a header file +configure_file(plugins_list.hpp.in ${CMAKE_CURRENT_BINARY_DIR}/gen/plugins_list.hpp) +target_include_directories(generate_nav2_tree_nodes_xml PRIVATE ${CMAKE_CURRENT_BINARY_DIR}/gen) +install(TARGETS generate_nav2_tree_nodes_xml + DESTINATION lib/${PROJECT_NAME} ) + install(DIRECTORY include/ DESTINATION include/ ) @@ -228,6 +236,8 @@ install(DIRECTORY test/utils/ ) install(FILES nav2_tree_nodes.xml DESTINATION share/${PROJECT_NAME}) +install(FILES ${CMAKE_CURRENT_BINARY_DIR}/gen/plugins_list.hpp DESTINATION include/${PROJECT_NAME}) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/nav2_behavior_tree/plugins_list.hpp.in b/nav2_behavior_tree/plugins_list.hpp.in new file mode 100644 index 0000000000..d93a484cad --- /dev/null +++ b/nav2_behavior_tree/plugins_list.hpp.in @@ -0,0 +1,6 @@ + +// This was automativally generated by cmake +namespace nav2::details +{ + const char* BT_PLUGIN_LIST = "@plugin_libs@"; +} diff --git a/nav2_behavior_tree/src/generate_nav2_tree_nodes_xml.cpp b/nav2_behavior_tree/src/generate_nav2_tree_nodes_xml.cpp new file mode 100644 index 0000000000..1c7a3c44f1 --- /dev/null +++ b/nav2_behavior_tree/src/generate_nav2_tree_nodes_xml.cpp @@ -0,0 +1,47 @@ +// Copyright (c) 2024 Davide Faconti +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#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 "plugins_list.hpp" + +int main() +{ + BT::BehaviorTreeFactory factory; + + std::vector plugins_list; + boost::split(plugins_list, nav2::details::BT_PLUGIN_LIST, boost::is_any_of(";")); + + for (const auto & plugin : plugins_list) { + std::cout << "Loading: " << plugin << "\n"; + factory.registerFromPlugin(BT::SharedLibrary::getOSName(plugin)); + } + std::cout << "\nGenerating file: nav2_tree_nodes.xml\n" + << "\nCompare it with the one in the git repo and update the latter if necessary.\n"; + + std::ofstream xml_file; + xml_file.open("nav2_tree_nodes.xml"); + xml_file << BT::writeTreeNodesModelXML(factory) << std::endl; + xml_file.close(); + + return 0; +} diff --git a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp index 48b97848f8..0825f99fbe 100644 --- a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp +++ b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include "gtest/gtest.h" @@ -31,6 +32,8 @@ #include "nav2_util/odometry_utils.hpp" +#include "nav2_behavior_tree/plugins_list.hpp" + #include "rclcpp/rclcpp.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" @@ -58,58 +61,9 @@ class BehaviorTreeHandler odom_smoother_ = std::make_shared(node_); - const std::vector plugin_libs = { - "nav2_compute_path_to_pose_action_bt_node", - "nav2_compute_path_through_poses_action_bt_node", - "nav2_smooth_path_action_bt_node", - "nav2_follow_path_action_bt_node", - "nav2_spin_action_bt_node", - "nav2_wait_action_bt_node", - "nav2_assisted_teleop_action_bt_node", - "nav2_back_up_action_bt_node", - "nav2_drive_on_heading_bt_node", - "nav2_clear_costmap_service_bt_node", - "nav2_is_stuck_condition_bt_node", - "nav2_goal_reached_condition_bt_node", - "nav2_initial_pose_received_condition_bt_node", - "nav2_goal_updated_condition_bt_node", - "nav2_globally_updated_goal_condition_bt_node", - "nav2_is_path_valid_condition_bt_node", - "nav2_are_error_codes_active_condition_bt_node", - "nav2_would_a_controller_recovery_help_condition_bt_node", - "nav2_would_a_planner_recovery_help_condition_bt_node", - "nav2_would_a_smoother_recovery_help_condition_bt_node", - "nav2_reinitialize_global_localization_service_bt_node", - "nav2_rate_controller_bt_node", - "nav2_distance_controller_bt_node", - "nav2_speed_controller_bt_node", - "nav2_truncate_path_action_bt_node", - "nav2_truncate_path_local_action_bt_node", - "nav2_goal_updater_node_bt_node", - "nav2_recovery_node_bt_node", - "nav2_pipeline_sequence_bt_node", - "nav2_round_robin_node_bt_node", - "nav2_transform_available_condition_bt_node", - "nav2_time_expired_condition_bt_node", - "nav2_path_expiring_timer_condition", - "nav2_distance_traveled_condition_bt_node", - "nav2_single_trigger_bt_node", - "nav2_is_battery_low_condition_bt_node", - "nav2_navigate_through_poses_action_bt_node", - "nav2_navigate_to_pose_action_bt_node", - "nav2_remove_passed_goals_action_bt_node", - "nav2_planner_selector_bt_node", - "nav2_controller_selector_bt_node", - "nav2_goal_checker_selector_bt_node", - "nav2_controller_cancel_bt_node", - "nav2_path_longer_on_approach_bt_node", - "nav2_assisted_teleop_cancel_bt_node", - "nav2_wait_cancel_bt_node", - "nav2_spin_cancel_bt_node", - "nav2_back_up_cancel_bt_node", - "nav2_drive_on_heading_cancel_bt_node", - "nav2_goal_updated_controller_bt_node" - }; + std::vector plugin_libs; + boost::split(plugin_libs, nav2::details::BT_PLUGIN_LIST, boost::is_any_of(";")); + for (const auto & p : plugin_libs) { factory_.registerFromPlugin(BT::SharedLibrary::getOSName(p)); } From c8b4eb5934a2923805873bffb08e4015e055ea5d Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Mon, 19 Feb 2024 13:21:48 +0100 Subject: [PATCH 2/5] behavior_tree: removed list of plugins from yaml Signed-off-by: Davide Faconti --- nav2_behavior_tree/CMakeLists.txt | 16 +++-- nav2_behavior_tree/plugins_list.hpp.in | 2 +- .../src/generate_nav2_tree_nodes_xml.cpp | 2 +- .../params/nav2_multirobot_params_1.yaml | 57 ++-------------- .../params/nav2_multirobot_params_2.yaml | 57 ++-------------- .../params/nav2_multirobot_params_all.yaml | 57 ++-------------- nav2_bringup/params/nav2_params.yaml | 57 ++-------------- nav2_bt_navigator/src/bt_navigator.cpp | 67 +++---------------- .../behavior_tree/test_behavior_tree_node.cpp | 2 +- .../src/costmap_filters/keepout_params.yaml | 49 ++------------ .../costmap_filters/speed_global_params.yaml | 50 ++------------ .../costmap_filters/speed_local_params.yaml | 50 ++------------ .../gps_navigation/nav2_no_map_params.yaml | 57 ++-------------- 13 files changed, 60 insertions(+), 463 deletions(-) diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 42911c68ea..c6465c2c8f 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -219,13 +219,16 @@ install(TARGETS ${library_name} RUNTIME DESTINATION bin ) +# we will embed the list of plugin names inside a header file +set(GENERATED_DIR ${CMAKE_CURRENT_BINARY_DIR}/gen) +configure_file(plugins_list.hpp.in ${GENERATED_DIR}/plugins_list.hpp) + add_executable(generate_nav2_tree_nodes_xml src/generate_nav2_tree_nodes_xml.cpp) ament_target_dependencies(generate_nav2_tree_nodes_xml ${dependencies}) -# we will embed the list of plugin names inside a header file -configure_file(plugins_list.hpp.in ${CMAKE_CURRENT_BINARY_DIR}/gen/plugins_list.hpp) -target_include_directories(generate_nav2_tree_nodes_xml PRIVATE ${CMAKE_CURRENT_BINARY_DIR}/gen) -install(TARGETS generate_nav2_tree_nodes_xml - DESTINATION lib/${PROJECT_NAME} ) +# allow generate_nav2_tree_nodes_xml to find plugins_list.hpp +target_include_directories(generate_nav2_tree_nodes_xml PRIVATE ${GENERATED_DIR}) +install(TARGETS generate_nav2_tree_nodes_xml DESTINATION lib/${PROJECT_NAME}) + install(DIRECTORY include/ DESTINATION include/ @@ -236,8 +239,7 @@ install(DIRECTORY test/utils/ ) install(FILES nav2_tree_nodes.xml DESTINATION share/${PROJECT_NAME}) -install(FILES ${CMAKE_CURRENT_BINARY_DIR}/gen/plugins_list.hpp DESTINATION include/${PROJECT_NAME}) - +install(FILES ${GENERATED_DIR}/plugins_list.hpp DESTINATION include/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/nav2_behavior_tree/plugins_list.hpp.in b/nav2_behavior_tree/plugins_list.hpp.in index d93a484cad..148583b927 100644 --- a/nav2_behavior_tree/plugins_list.hpp.in +++ b/nav2_behavior_tree/plugins_list.hpp.in @@ -2,5 +2,5 @@ // This was automativally generated by cmake namespace nav2::details { - const char* BT_PLUGIN_LIST = "@plugin_libs@"; + const char* BT_BUILTIN_PLUGINS = "@plugin_libs@"; } 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 1c7a3c44f1..01bdf6a685 100644 --- a/nav2_behavior_tree/src/generate_nav2_tree_nodes_xml.cpp +++ b/nav2_behavior_tree/src/generate_nav2_tree_nodes_xml.cpp @@ -29,7 +29,7 @@ int main() BT::BehaviorTreeFactory factory; std::vector plugins_list; - boost::split(plugins_list, nav2::details::BT_PLUGIN_LIST, boost::is_any_of(";")); + boost::split(plugins_list, nav2::details::BT_BUILTIN_PLUGINS, boost::is_any_of(";")); for (const auto & plugin : plugins_list) { std::cout << "Loading: " << plugin << "\n"; diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index baceb0e531..555f3ff8be 100644 --- a/nav2_bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml @@ -56,58 +56,11 @@ bt_navigator: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_would_a_smoother_recovery_help_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_is_battery_charging_condition_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + plugin_lib_names: [] + error_code_names: - compute_path_error_code - follow_path_error_code diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml index 583bb64b56..c3399bbeb1 100644 --- a/nav2_bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml @@ -56,58 +56,11 @@ bt_navigator: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_would_a_smoother_recovery_help_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_is_battery_charging_condition_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + plugin_lib_names: [] + error_code_names: - compute_path_error_code - follow_path_error_code diff --git a/nav2_bringup/params/nav2_multirobot_params_all.yaml b/nav2_bringup/params/nav2_multirobot_params_all.yaml index 5fb4b9fe63..1dac20e5e3 100644 --- a/nav2_bringup/params/nav2_multirobot_params_all.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_all.yaml @@ -56,58 +56,11 @@ bt_navigator: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_would_a_smoother_recovery_help_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_is_battery_charging_condition_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + plugin_lib_names: [] + error_code_names: - compute_path_error_code - follow_path_error_code diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 0e3427a17b..ac8900076f 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -56,58 +56,11 @@ bt_navigator: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_would_a_smoother_recovery_help_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_is_battery_charging_condition_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + plugin_lib_names: [] + error_code_names: - compute_path_error_code - follow_path_error_code diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 82beb93999..2b2c116eeb 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -18,12 +18,15 @@ #include #include #include +#include #include "nav2_util/geometry_utils.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_behavior_tree/bt_utils.hpp" +#include "nav2_behavior_tree/plugins_list.hpp" + using nav2_util::declare_parameter_if_not_declared; namespace nav2_bt_navigator @@ -36,62 +39,8 @@ BtNavigator::BtNavigator(rclcpp::NodeOptions options) { RCLCPP_INFO(get_logger(), "Creating"); - const std::vector plugin_libs = { - "nav2_compute_path_to_pose_action_bt_node", - "nav2_compute_path_through_poses_action_bt_node", - "nav2_smooth_path_action_bt_node", - "nav2_follow_path_action_bt_node", - "nav2_spin_action_bt_node", - "nav2_wait_action_bt_node", - "nav2_assisted_teleop_action_bt_node", - "nav2_back_up_action_bt_node", - "nav2_drive_on_heading_bt_node", - "nav2_clear_costmap_service_bt_node", - "nav2_is_stuck_condition_bt_node", - "nav2_goal_reached_condition_bt_node", - "nav2_initial_pose_received_condition_bt_node", - "nav2_goal_updated_condition_bt_node", - "nav2_globally_updated_goal_condition_bt_node", - "nav2_is_path_valid_condition_bt_node", - "nav2_are_error_codes_active_condition_bt_node", - "nav2_would_a_controller_recovery_help_condition_bt_node", - "nav2_would_a_planner_recovery_help_condition_bt_node", - "nav2_would_a_smoother_recovery_help_condition_bt_node", - "nav2_reinitialize_global_localization_service_bt_node", - "nav2_rate_controller_bt_node", - "nav2_distance_controller_bt_node", - "nav2_speed_controller_bt_node", - "nav2_truncate_path_action_bt_node", - "nav2_truncate_path_local_action_bt_node", - "nav2_goal_updater_node_bt_node", - "nav2_recovery_node_bt_node", - "nav2_pipeline_sequence_bt_node", - "nav2_round_robin_node_bt_node", - "nav2_transform_available_condition_bt_node", - "nav2_time_expired_condition_bt_node", - "nav2_path_expiring_timer_condition", - "nav2_distance_traveled_condition_bt_node", - "nav2_single_trigger_bt_node", - "nav2_goal_updated_controller_bt_node", - "nav2_is_battery_low_condition_bt_node", - "nav2_navigate_through_poses_action_bt_node", - "nav2_navigate_to_pose_action_bt_node", - "nav2_remove_passed_goals_action_bt_node", - "nav2_planner_selector_bt_node", - "nav2_controller_selector_bt_node", - "nav2_goal_checker_selector_bt_node", - "nav2_controller_cancel_bt_node", - "nav2_path_longer_on_approach_bt_node", - "nav2_wait_cancel_bt_node", - "nav2_spin_cancel_bt_node", - "nav2_assisted_teleop_cancel_bt_node", - "nav2_back_up_cancel_bt_node", - "nav2_drive_on_heading_cancel_bt_node", - "nav2_is_battery_charging_condition_bt_node" - }; - declare_parameter_if_not_declared( - this, "plugin_lib_names", rclcpp::ParameterValue(plugin_libs)); + this, "plugin_lib_names", rclcpp::ParameterValue(std::vector{})); declare_parameter_if_not_declared( this, "transform_tolerance", rclcpp::ParameterValue(0.1)); declare_parameter_if_not_declared( @@ -124,7 +73,13 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) odom_topic_ = get_parameter("odom_topic").as_string(); // Libraries to pull plugins (BT Nodes) from - auto plugin_lib_names = get_parameter("plugin_lib_names").as_string_array(); + std::vector plugin_lib_names; + boost::split(plugin_lib_names, nav2::details::BT_BUILTIN_PLUGINS, boost::is_any_of(";")); + auto user_defined_plugins = get_parameter("plugin_lib_names").as_string_array(); + // append user_defined_plugins to plugin_lib_names + plugin_lib_names.insert( + plugin_lib_names.end(), user_defined_plugins.begin(), + user_defined_plugins.end()); nav2_core::FeedbackUtils feedback_utils; feedback_utils.tf = tf_; diff --git a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp index 0825f99fbe..c9c718a177 100644 --- a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp +++ b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp @@ -62,7 +62,7 @@ class BehaviorTreeHandler odom_smoother_ = std::make_shared(node_); std::vector plugin_libs; - boost::split(plugin_libs, nav2::details::BT_PLUGIN_LIST, boost::is_any_of(";")); + boost::split(plugin_libs, nav2::details::BT_BUILTIN_PLUGINS, boost::is_any_of(";")); for (const auto & p : plugin_libs) { factory_.registerFromPlugin(BT::SharedLibrary::getOSName(p)); diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml index f5d79153ec..c2e4cb0e0d 100644 --- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -47,51 +47,10 @@ bt_navigator: # files to allow for a commandline change default used is the # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml & # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + plugin_lib_names: [] controller_server: ros__parameters: diff --git a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml index bf65fdf9ba..7ca28f876d 100644 --- a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml @@ -47,52 +47,10 @@ bt_navigator: # files to allow for a commandline change default used is the # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml & # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + plugin_lib_names: [] controller_server: ros__parameters: diff --git a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml index 3799062cf8..dd7b304c64 100644 --- a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml @@ -47,52 +47,10 @@ bt_navigator: # files to allow for a commandline change default used is the # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml & # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + plugin_lib_names: [] controller_server: ros__parameters: diff --git a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml index b8bc078103..07a451e00b 100644 --- a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml +++ b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml @@ -15,58 +15,11 @@ bt_navigator: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_would_a_smoother_recovery_help_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_is_battery_charging_condition_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + plugin_lib_names: [] + error_code_names: - compute_path_error_code - follow_path_error_code From d0e6c33cd7435d90b460d9d0fd64dcdbc8618787 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Tue, 20 Feb 2024 17:04:50 +0100 Subject: [PATCH 3/5] check result of get_parameter(plugin_lib_names) Signed-off-by: Davide Faconti --- nav2_bt_navigator/src/bt_navigator.cpp | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 2b2c116eeb..7ff78ac096 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -75,11 +75,17 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) // Libraries to pull plugins (BT Nodes) from std::vector plugin_lib_names; boost::split(plugin_lib_names, nav2::details::BT_BUILTIN_PLUGINS, boost::is_any_of(";")); - auto user_defined_plugins = get_parameter("plugin_lib_names").as_string_array(); - // append user_defined_plugins to plugin_lib_names - plugin_lib_names.insert( - plugin_lib_names.end(), user_defined_plugins.begin(), - user_defined_plugins.end()); + + rclcpp::Parameter plugins_param; + if(get_parameter("plugin_lib_names", plugins_param) && + plugins_param.get_type() == rclcpp::ParameterType::PARAMETER_STRING_ARRAY) + { + auto user_defined_plugins = plugins_param.as_string_array(); + // append user_defined_plugins to plugin_lib_names + plugin_lib_names.insert( + plugin_lib_names.end(), user_defined_plugins.begin(), + user_defined_plugins.end()); + } nav2_core::FeedbackUtils feedback_utils; feedback_utils.tf = tf_; From 38cdd40eefa036c7d1b09230ea8442f346a9c9dd Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Tue, 20 Feb 2024 18:07:48 +0100 Subject: [PATCH 4/5] fix previous commit (uncrustify) Signed-off-by: Davide Faconti --- nav2_bringup/params/nav2_multirobot_params_1.yaml | 2 +- nav2_bringup/params/nav2_multirobot_params_2.yaml | 2 +- nav2_bringup/params/nav2_multirobot_params_all.yaml | 2 +- nav2_bringup/params/nav2_params.yaml | 2 +- nav2_bt_navigator/src/bt_navigator.cpp | 4 ++-- nav2_system_tests/src/costmap_filters/keepout_params.yaml | 2 +- .../src/costmap_filters/speed_global_params.yaml | 2 +- nav2_system_tests/src/costmap_filters/speed_local_params.yaml | 2 +- nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml | 2 +- 9 files changed, 10 insertions(+), 10 deletions(-) diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index 555f3ff8be..91401fc21f 100644 --- a/nav2_bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml @@ -59,7 +59,7 @@ bt_navigator: # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). # Built-in plugins are added automatically - plugin_lib_names: [] + # plugin_lib_names: [] error_code_names: - compute_path_error_code diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml index c3399bbeb1..732fa24eb1 100644 --- a/nav2_bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml @@ -59,7 +59,7 @@ bt_navigator: # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). # Built-in plugins are added automatically - plugin_lib_names: [] + # plugin_lib_names: [] error_code_names: - compute_path_error_code diff --git a/nav2_bringup/params/nav2_multirobot_params_all.yaml b/nav2_bringup/params/nav2_multirobot_params_all.yaml index 1dac20e5e3..bb812d276b 100644 --- a/nav2_bringup/params/nav2_multirobot_params_all.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_all.yaml @@ -59,7 +59,7 @@ bt_navigator: # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). # Built-in plugins are added automatically - plugin_lib_names: [] + # plugin_lib_names: [] error_code_names: - compute_path_error_code diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index ea5891523b..28e7d273fb 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -59,7 +59,7 @@ bt_navigator: # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). # Built-in plugins are added automatically - plugin_lib_names: [] + # plugin_lib_names: [] error_code_names: - compute_path_error_code diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 7ff78ac096..387a2ccad2 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -77,8 +77,8 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) boost::split(plugin_lib_names, nav2::details::BT_BUILTIN_PLUGINS, boost::is_any_of(";")); rclcpp::Parameter plugins_param; - if(get_parameter("plugin_lib_names", plugins_param) && - plugins_param.get_type() == rclcpp::ParameterType::PARAMETER_STRING_ARRAY) + if (get_parameter("plugin_lib_names", plugins_param) && + plugins_param.get_type() == rclcpp::ParameterType::PARAMETER_STRING_ARRAY) { auto user_defined_plugins = plugins_param.as_string_array(); // append user_defined_plugins to plugin_lib_names diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml index c2e4cb0e0d..9f9bdd33de 100644 --- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -50,7 +50,7 @@ bt_navigator: # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). # Built-in plugins are added automatically - plugin_lib_names: [] + # plugin_lib_names: [] controller_server: ros__parameters: diff --git a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml index 7ca28f876d..10040d8798 100644 --- a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml @@ -50,7 +50,7 @@ bt_navigator: # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). # Built-in plugins are added automatically - plugin_lib_names: [] + # plugin_lib_names: [] controller_server: ros__parameters: diff --git a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml index dd7b304c64..b50108a13f 100644 --- a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml @@ -50,7 +50,7 @@ bt_navigator: # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). # Built-in plugins are added automatically - plugin_lib_names: [] + # plugin_lib_names: [] controller_server: ros__parameters: diff --git a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml index 07a451e00b..6e9f3f490b 100644 --- a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml +++ b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml @@ -18,7 +18,7 @@ bt_navigator: # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). # Built-in plugins are added automatically - plugin_lib_names: [] + # plugin_lib_names: [] error_code_names: - compute_path_error_code From a6325e520a2615923fd580100926b86631b83c13 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Tue, 20 Feb 2024 20:52:18 +0100 Subject: [PATCH 5/5] revert change Signed-off-by: Davide Faconti --- nav2_bt_navigator/src/bt_navigator.cpp | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 387a2ccad2..4c1b4f47bc 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -76,16 +76,11 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) std::vector plugin_lib_names; boost::split(plugin_lib_names, nav2::details::BT_BUILTIN_PLUGINS, boost::is_any_of(";")); - rclcpp::Parameter plugins_param; - if (get_parameter("plugin_lib_names", plugins_param) && - plugins_param.get_type() == rclcpp::ParameterType::PARAMETER_STRING_ARRAY) - { - auto user_defined_plugins = plugins_param.as_string_array(); - // append user_defined_plugins to plugin_lib_names - plugin_lib_names.insert( - plugin_lib_names.end(), user_defined_plugins.begin(), - user_defined_plugins.end()); - } + auto user_defined_plugins = get_parameter("plugin_lib_names").as_string_array(); + // append user_defined_plugins to plugin_lib_names + plugin_lib_names.insert( + plugin_lib_names.end(), user_defined_plugins.begin(), + user_defined_plugins.end()); nav2_core::FeedbackUtils feedback_utils; feedback_utils.tf = tf_;