From 43bace3bbb9ddec4d2b362b56af05957194233b7 Mon Sep 17 00:00:00 2001 From: "Carlos A. Orduno" <39749557+orduno@users.noreply.github.com> Date: Mon, 11 Nov 2019 11:41:01 -0800 Subject: [PATCH] Enable dynamically loading BT nodes (#1323) * Making bt nodes separate libraries. * Using params file to specify which plugins to use. * Add plugin names to multi-robot params files. --- nav2_behavior_tree/CMakeLists.txt | 47 ++++++++++++++--- .../behavior_tree_engine.hpp | 6 +-- .../initial_pose_received_condition.hpp | 28 +++++++++++ ...ontroller_node.hpp => rate_controller.hpp} | 6 +-- nav2_behavior_tree/plugins/back_up_action.cpp | 23 +++++++++ .../plugins/clear_costmap_service.cpp | 23 +++++++++ .../plugins/compute_path_to_pose_action.cpp | 23 +++++++++ .../plugins/follow_path_action.cpp | 23 +++++++++ .../plugins/goal_reached_condition.cpp | 23 +++++++++ .../initial_pose_received_condition.cpp | 36 +++++++++++++ .../plugins/is_stuck_condition.cpp | 23 +++++++++ .../plugins/rate_controller.cpp | 23 +++++++++ .../{src => plugins}/recovery_node.cpp | 11 +++- ...initialize_global_localization_service.cpp | 24 +++++++++ nav2_behavior_tree/plugins/spin_action.cpp | 23 +++++++++ nav2_behavior_tree/plugins/wait_action.cpp | 23 +++++++++ .../src/behavior_tree_engine.cpp | 50 +++---------------- .../params/nav2_multirobot_params_1.yaml | 13 +++++ .../params/nav2_multirobot_params_2.yaml | 13 +++++ nav2_bringup/bringup/params/nav2_params.yaml | 13 +++++ .../nav2_bt_navigator/bt_navigator.hpp | 4 ++ nav2_bt_navigator/src/bt_navigator.cpp | 10 +++- 22 files changed, 410 insertions(+), 58 deletions(-) create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/initial_pose_received_condition.hpp rename nav2_behavior_tree/include/nav2_behavior_tree/{rate_controller_node.hpp => rate_controller.hpp} (94%) create mode 100644 nav2_behavior_tree/plugins/back_up_action.cpp create mode 100644 nav2_behavior_tree/plugins/clear_costmap_service.cpp create mode 100644 nav2_behavior_tree/plugins/compute_path_to_pose_action.cpp create mode 100644 nav2_behavior_tree/plugins/follow_path_action.cpp create mode 100644 nav2_behavior_tree/plugins/goal_reached_condition.cpp create mode 100644 nav2_behavior_tree/plugins/initial_pose_received_condition.cpp create mode 100644 nav2_behavior_tree/plugins/is_stuck_condition.cpp create mode 100644 nav2_behavior_tree/plugins/rate_controller.cpp rename nav2_behavior_tree/{src => plugins}/recovery_node.cpp (95%) create mode 100644 nav2_behavior_tree/plugins/reinitialize_global_localization_service.cpp create mode 100644 nav2_behavior_tree/plugins/spin_action.cpp create mode 100644 nav2_behavior_tree/plugins/wait_action.cpp diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 86da24c7..b6e0eca3 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -26,11 +26,6 @@ include_directories( set(library_name ${PROJECT_NAME}) -add_library(${library_name} SHARED - src/behavior_tree_engine.cpp - src/recovery_node.cpp -) - set(dependencies rclcpp rclcpp_action @@ -47,11 +42,46 @@ set(dependencies nav2_util ) +add_library(${library_name} SHARED + src/behavior_tree_engine.cpp +) + ament_target_dependencies(${library_name} ${dependencies} ) +# The behavior tree (bt) node plugins +set(bt_plugins + compute_path_to_pose_action + follow_path_action + back_up_action + spin_action + wait_action + clear_costmap_service + is_stuck_condition + goal_reached_condition + initial_pose_received_condition + reinitialize_global_localization_service + rate_controller + recovery_node +) + +foreach(bt_plugin ${bt_plugins}) + + set(lib_name "nav2_${bt_plugin}_bt_node") + + add_library(${lib_name} SHARED plugins/${bt_plugin}.cpp) + + ament_target_dependencies(${lib_name} ${dependencies}) + + target_compile_definitions(${lib_name} PRIVATE BT_PLUGIN_EXPORT) + + list(APPEND plugin_libs ${lib_name}) + +endforeach() + install(TARGETS ${library_name} + ${plugin_libs} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME} @@ -67,7 +97,12 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) -ament_export_libraries(${library_name}) + +ament_export_libraries( + ${library_name} + ${plugin_libs} +) + ament_export_dependencies(${dependencies}) ament_package() 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 6d222c30..7403fdeb 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 @@ -17,6 +17,7 @@ #include <memory> #include <string> +#include <vector> #include "behaviortree_cpp/behavior_tree.h" #include "behaviortree_cpp/bt_factory.h" @@ -30,7 +31,7 @@ enum class BtStatus { SUCCEEDED, FAILED, CANCELED }; class BehaviorTreeEngine { public: - BehaviorTreeEngine(); + explicit BehaviorTreeEngine(const std::vector<std::string> & plugin_libraries); virtual ~BehaviorTreeEngine() {} BtStatus run( @@ -63,9 +64,6 @@ public: } protected: - // Methods used to register as (simple action) BT nodes - BT::NodeStatus initialPoseReceived(BT::TreeNode & tree_node); - // The factory that will be used to dynamically construct the behavior tree BT::BehaviorTreeFactory factory_; }; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/initial_pose_received_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/initial_pose_received_condition.hpp new file mode 100644 index 00000000..69b721bc --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/initial_pose_received_condition.hpp @@ -0,0 +1,28 @@ +// Copyright (c) 2019 Intel Corporation +// +// 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. + +#ifndef NAV2_BEHAVIOR_TREE__INITIAL_POSE_RECEIVED_CONDITION_HPP_ +#define NAV2_BEHAVIOR_TREE__INITIAL_POSE_RECEIVED_CONDITION_HPP_ + +#include "behaviortree_cpp/behavior_tree.h" + + +namespace nav2_behavior_tree +{ + +BT::NodeStatus initialPoseReceived(BT::TreeNode & tree_node); + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__INITIAL_POSE_RECEIVED_CONDITION_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/rate_controller_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/rate_controller.hpp similarity index 94% rename from nav2_behavior_tree/include/nav2_behavior_tree/rate_controller_node.hpp rename to nav2_behavior_tree/include/nav2_behavior_tree/rate_controller.hpp index f66e68af..a2d1ad54 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/rate_controller_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/rate_controller.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_BEHAVIOR_TREE__RATE_CONTROLLER_NODE_HPP_ -#define NAV2_BEHAVIOR_TREE__RATE_CONTROLLER_NODE_HPP_ +#ifndef NAV2_BEHAVIOR_TREE__RATE_CONTROLLER_HPP_ +#define NAV2_BEHAVIOR_TREE__RATE_CONTROLLER_HPP_ #include <chrono> #include <string> @@ -98,4 +98,4 @@ inline BT::NodeStatus RateController::tick() } // namespace nav2_behavior_tree -#endif // NAV2_BEHAVIOR_TREE__RATE_CONTROLLER_NODE_HPP_ +#endif // NAV2_BEHAVIOR_TREE__RATE_CONTROLLER_HPP_ diff --git a/nav2_behavior_tree/plugins/back_up_action.cpp b/nav2_behavior_tree/plugins/back_up_action.cpp new file mode 100644 index 00000000..2767e629 --- /dev/null +++ b/nav2_behavior_tree/plugins/back_up_action.cpp @@ -0,0 +1,23 @@ +// Copyright (c) 2019 Intel Corporation +// +// 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. + +#include "nav2_behavior_tree/back_up_action.hpp" + +#include "behaviortree_cpp/bt_factory.h" + + +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType<nav2_behavior_tree::BackUpAction>("BackUp"); +} diff --git a/nav2_behavior_tree/plugins/clear_costmap_service.cpp b/nav2_behavior_tree/plugins/clear_costmap_service.cpp new file mode 100644 index 00000000..a70ec950 --- /dev/null +++ b/nav2_behavior_tree/plugins/clear_costmap_service.cpp @@ -0,0 +1,23 @@ +// Copyright (c) 2019 Intel Corporation +// +// 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. + +#include "nav2_behavior_tree/clear_costmap_service.hpp" + +#include "behaviortree_cpp/bt_factory.h" + + +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType<nav2_behavior_tree::ClearEntireCostmapService>("ClearEntireCostmap"); +} diff --git a/nav2_behavior_tree/plugins/compute_path_to_pose_action.cpp b/nav2_behavior_tree/plugins/compute_path_to_pose_action.cpp new file mode 100644 index 00000000..5026206b --- /dev/null +++ b/nav2_behavior_tree/plugins/compute_path_to_pose_action.cpp @@ -0,0 +1,23 @@ +// Copyright (c) 2019 Intel Corporation +// +// 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. + +#include "nav2_behavior_tree/compute_path_to_pose_action.hpp" + +#include "behaviortree_cpp/bt_factory.h" + + +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType<nav2_behavior_tree::ComputePathToPoseAction>("ComputePathToPose"); +} diff --git a/nav2_behavior_tree/plugins/follow_path_action.cpp b/nav2_behavior_tree/plugins/follow_path_action.cpp new file mode 100644 index 00000000..fa8a77b5 --- /dev/null +++ b/nav2_behavior_tree/plugins/follow_path_action.cpp @@ -0,0 +1,23 @@ +// Copyright (c) 2019 Intel Corporation +// +// 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. + +#include "nav2_behavior_tree/follow_path_action.hpp" + +#include "behaviortree_cpp/bt_factory.h" + + +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType<nav2_behavior_tree::FollowPathAction>("FollowPath"); +} diff --git a/nav2_behavior_tree/plugins/goal_reached_condition.cpp b/nav2_behavior_tree/plugins/goal_reached_condition.cpp new file mode 100644 index 00000000..365588cd --- /dev/null +++ b/nav2_behavior_tree/plugins/goal_reached_condition.cpp @@ -0,0 +1,23 @@ +// Copyright (c) 2019 Intel Corporation +// +// 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. + +#include "nav2_behavior_tree/goal_reached_condition.hpp" + +#include "behaviortree_cpp/bt_factory.h" + + +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType<nav2_behavior_tree::GoalReachedCondition>("GoalReached"); +} diff --git a/nav2_behavior_tree/plugins/initial_pose_received_condition.cpp b/nav2_behavior_tree/plugins/initial_pose_received_condition.cpp new file mode 100644 index 00000000..052c88bd --- /dev/null +++ b/nav2_behavior_tree/plugins/initial_pose_received_condition.cpp @@ -0,0 +1,36 @@ +// Copyright (c) 2019 Intel Corporation +// +// 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. + +#include "nav2_behavior_tree/initial_pose_received_condition.hpp" + +#include "behaviortree_cpp/bt_factory.h" + + +BT_REGISTER_NODES(factory) +{ + factory.registerSimpleCondition("InitialPoseReceived", + std::bind(&nav2_behavior_tree::initialPoseReceived, std::placeholders::_1)); +} + +namespace nav2_behavior_tree +{ + +BT::NodeStatus +initialPoseReceived(BT::TreeNode & tree_node) +{ + auto initPoseReceived = tree_node.config().blackboard->get<bool>("initial_pose_received"); + return initPoseReceived ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; +} + +} // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/is_stuck_condition.cpp b/nav2_behavior_tree/plugins/is_stuck_condition.cpp new file mode 100644 index 00000000..c84accb9 --- /dev/null +++ b/nav2_behavior_tree/plugins/is_stuck_condition.cpp @@ -0,0 +1,23 @@ +// Copyright (c) 2019 Intel Corporation +// +// 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. + +#include "nav2_behavior_tree/is_stuck_condition.hpp" + +#include "behaviortree_cpp/bt_factory.h" + + +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType<nav2_behavior_tree::IsStuckCondition>("IsStuck"); +} diff --git a/nav2_behavior_tree/plugins/rate_controller.cpp b/nav2_behavior_tree/plugins/rate_controller.cpp new file mode 100644 index 00000000..a04a0426 --- /dev/null +++ b/nav2_behavior_tree/plugins/rate_controller.cpp @@ -0,0 +1,23 @@ +// Copyright (c) 2019 Intel Corporation +// +// 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. + +#include "nav2_behavior_tree/rate_controller.hpp" + +#include "behaviortree_cpp/bt_factory.h" + + +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType<nav2_behavior_tree::RateController>("RateController"); +} diff --git a/nav2_behavior_tree/src/recovery_node.cpp b/nav2_behavior_tree/plugins/recovery_node.cpp similarity index 95% rename from nav2_behavior_tree/src/recovery_node.cpp rename to nav2_behavior_tree/plugins/recovery_node.cpp index c05193e1..ca4865cf 100644 --- a/nav2_behavior_tree/src/recovery_node.cpp +++ b/nav2_behavior_tree/plugins/recovery_node.cpp @@ -12,9 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include <string> #include "nav2_behavior_tree/recovery_node.hpp" +#include <string> + +#include "behaviortree_cpp/bt_factory.h" + + +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType<nav2_behavior_tree::RecoveryNode>("RecoveryNode"); +} + namespace nav2_behavior_tree { RecoveryNode::RecoveryNode( diff --git a/nav2_behavior_tree/plugins/reinitialize_global_localization_service.cpp b/nav2_behavior_tree/plugins/reinitialize_global_localization_service.cpp new file mode 100644 index 00000000..cf9fda59 --- /dev/null +++ b/nav2_behavior_tree/plugins/reinitialize_global_localization_service.cpp @@ -0,0 +1,24 @@ +// Copyright (c) 2019 Intel Corporation +// +// 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. + +#include "nav2_behavior_tree/reinitialize_global_localization_service.hpp" + +#include "behaviortree_cpp/bt_factory.h" + + +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType<nav2_behavior_tree::ReinitializeGlobalLocalizationService>( + "ReinitializeGlobalLocalization"); +} diff --git a/nav2_behavior_tree/plugins/spin_action.cpp b/nav2_behavior_tree/plugins/spin_action.cpp new file mode 100644 index 00000000..4440bdf4 --- /dev/null +++ b/nav2_behavior_tree/plugins/spin_action.cpp @@ -0,0 +1,23 @@ +// Copyright (c) 2019 Intel Corporation +// +// 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. + +#include "nav2_behavior_tree/spin_action.hpp" + +#include "behaviortree_cpp/bt_factory.h" + + +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType<nav2_behavior_tree::SpinAction>("Spin"); +} diff --git a/nav2_behavior_tree/plugins/wait_action.cpp b/nav2_behavior_tree/plugins/wait_action.cpp new file mode 100644 index 00000000..ad8459d9 --- /dev/null +++ b/nav2_behavior_tree/plugins/wait_action.cpp @@ -0,0 +1,23 @@ +// Copyright (c) 2019 Intel Corporation +// +// 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. + +#include "nav2_behavior_tree/wait_action.hpp" + +#include "behaviortree_cpp/bt_factory.h" + + +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType<nav2_behavior_tree::WaitAction>("Wait"); +} diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index 9e905a65..13c6ebc5 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -16,51 +16,22 @@ #include <memory> #include <string> +#include <vector> -#include "nav2_behavior_tree/back_up_action.hpp" -#include "nav2_behavior_tree/bt_conversions.hpp" -#include "nav2_behavior_tree/compute_path_to_pose_action.hpp" -#include "nav2_behavior_tree/follow_path_action.hpp" -#include "nav2_behavior_tree/goal_reached_condition.hpp" -#include "nav2_behavior_tree/is_stuck_condition.hpp" -#include "nav2_behavior_tree/rate_controller_node.hpp" -#include "nav2_behavior_tree/recovery_node.hpp" -#include "nav2_behavior_tree/spin_action.hpp" -#include "nav2_behavior_tree/wait_action.hpp" -#include "nav2_behavior_tree/clear_costmap_service.hpp" -#include "nav2_behavior_tree/reinitialize_global_localization_service.hpp" #include "rclcpp/rclcpp.hpp" +#include "behaviortree_cpp/utils/shared_library.h" using namespace std::chrono_literals; namespace nav2_behavior_tree { -BehaviorTreeEngine::BehaviorTreeEngine() +BehaviorTreeEngine::BehaviorTreeEngine(const std::vector<std::string> & plugin_libraries) { - // Register our custom action nodes so that they can be included in XML description - factory_.registerNodeType<nav2_behavior_tree::ComputePathToPoseAction>("ComputePathToPose"); - factory_.registerNodeType<nav2_behavior_tree::FollowPathAction>("FollowPath"); - factory_.registerNodeType<nav2_behavior_tree::BackUpAction>("BackUp"); - factory_.registerNodeType<nav2_behavior_tree::SpinAction>("Spin"); - factory_.registerNodeType<nav2_behavior_tree::WaitAction>("Wait"); - factory_.registerNodeType<nav2_behavior_tree::ClearEntireCostmapService>("ClearEntireCostmap"); - factory_.registerNodeType<nav2_behavior_tree::ReinitializeGlobalLocalizationService>( - "ReinitializeGlobalLocalization"); - - // Register our custom condition nodes - factory_.registerNodeType<nav2_behavior_tree::IsStuckCondition>("IsStuck"); - factory_.registerNodeType<nav2_behavior_tree::GoalReachedCondition>("GoalReached"); - - // Register our simple condition nodes - factory_.registerSimpleCondition("initialPoseReceived", - std::bind(&BehaviorTreeEngine::initialPoseReceived, this, std::placeholders::_1)); - - // Register our custom decorator nodes - factory_.registerNodeType<nav2_behavior_tree::RateController>("RateController"); - - // Register our custom control nodes - factory_.registerNodeType<nav2_behavior_tree::RecoveryNode>("RecoveryNode"); + BT::SharedLibrary loader; + for (const auto & p : plugin_libraries) { + factory_.registerFromPlugin(loader.getOSName(p)); + } } BtStatus @@ -100,11 +71,4 @@ BehaviorTreeEngine::buildTreeFromText( return p.instantiateTree(blackboard); } -BT::NodeStatus -BehaviorTreeEngine::initialPoseReceived(BT::TreeNode & tree_node) -{ - auto initPoseReceived = tree_node.config().blackboard->get<bool>("initial_pose_received"); - return initPoseReceived ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; -} - } // namespace nav2_behavior_tree diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml index 6c138d09..458b1aa1 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml @@ -50,6 +50,19 @@ bt_navigator: ros__parameters: use_sim_time: True bt_xml_filename: "navigate_w_replanning_and_recovery.xml" + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_back_up_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_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_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_recovery_node_bt_node bt_navigator_rclcpp_node: ros__parameters: diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml index d1f52ba2..9a45958d 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml @@ -50,6 +50,19 @@ bt_navigator: ros__parameters: use_sim_time: True bt_xml_filename: "navigate_w_replanning_and_recovery.xml" + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_back_up_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_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_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_recovery_node_bt_node bt_navigator_rclcpp_node: ros__parameters: diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index 634fced8..f3a336b9 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -50,6 +50,19 @@ bt_navigator: ros__parameters: use_sim_time: True bt_xml_filename: "navigate_w_replanning_and_recovery.xml" + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_back_up_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_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_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_recovery_node_bt_node bt_navigator_rclcpp_node: ros__parameters: diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp index f016df80..8b4bcf9a 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp @@ -17,6 +17,7 @@ #include <memory> #include <string> +#include <vector> #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_behavior_tree/behavior_tree_engine.hpp" @@ -121,6 +122,9 @@ protected: // The complete behavior tree that results from parsing the incoming XML std::unique_ptr<BT::Tree> tree_; + // Libraries to pull plugins (BT Nodes) from + std::vector<std::string> plugin_lib_names_; + // A client that we'll use to send a command message to our own task server rclcpp_action::Client<nav2_msgs::action::NavigateToPose>::SharedPtr self_client_; diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index ee63854d..7298b874 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -19,6 +19,7 @@ #include <streambuf> #include <string> #include <utility> +#include <vector> #include "nav2_behavior_tree/bt_conversions.hpp" @@ -32,6 +33,9 @@ BtNavigator::BtNavigator() // Declare this node's parameters declare_parameter("bt_xml_filename"); + + declare_parameter("plugin_lib_names", + rclcpp::ParameterValue(std::vector<std::string>({"nav2_behavior_tree_nodes"}))); } BtNavigator::~BtNavigator() @@ -73,8 +77,11 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) get_node_waitables_interface(), "NavigateToPose", std::bind(&BtNavigator::navigateToPose, this), false); + // Get the libraries to pull plugins from + get_parameter("plugin_lib_names", plugin_lib_names_); + // Create the class that registers our custom nodes and executes the BT - bt_ = std::make_unique<nav2_behavior_tree::BehaviorTreeEngine>(); + bt_ = std::make_unique<nav2_behavior_tree::BehaviorTreeEngine>(plugin_lib_names_); // Create the blackboard that will be shared by all of the nodes in the tree blackboard_ = BT::Blackboard::create(); @@ -148,6 +155,7 @@ BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/) tf_.reset(); tf_listener_.reset(); action_server_.reset(); + plugin_lib_names_.clear(); xml_string_.clear(); tree_.reset(); blackboard_.reset(); -- GitLab