Skip to content
Snippets Groups Projects
Unverified Commit 43bace3b authored by Carlos A. Orduno's avatar Carlos A. Orduno Committed by GitHub
Browse files

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.
parent 054a6871
No related branches found
No related tags found
No related merge requests found
Showing
with 397 additions and 57 deletions
......@@ -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()
......@@ -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_;
};
......
// 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_
......@@ -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_
// 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");
}
// 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");
}
// 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");
}
// 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");
}
// 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");
}
// 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
// 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");
}
// 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");
}
......@@ -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(
......
// 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");
}
// 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");
}
// 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");
}
......@@ -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
......@@ -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:
......
......@@ -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:
......
......@@ -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:
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment