Skip to content

Commit 7e727ba

Browse files
facontidavidefacontidavide
committed
fix issue #61: postpone registerNodesIntoFactory
1 parent d5d869e commit 7e727ba

File tree

4 files changed

+99
-51
lines changed

4 files changed

+99
-51
lines changed

‎behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp

Lines changed: 14 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -56,14 +56,23 @@ void LoadBehaviorTrees(BT::BehaviorTreeFactory& factory,
5656
const std::string& directory_path);
5757

5858
/**
59-
* @brief Function to load BehaviorTree ROS plugins (or standard BT.CPP plugins) from a specific directory
59+
* @brief Function to load a BehaviorTree ROS plugin (or standard BT.CPP plugins)
6060
*
61+
* @param factory BehaviorTreeFactory to register the plugin into
62+
* @param file_path Full path to the directory to search for BehaviorTree plugin
63+
* @param params parameters passed to the ROS plugin
64+
*/
65+
void LoadPlugin(BT::BehaviorTreeFactory& factory, const std::filesystem::path& file_path,
66+
BT::RosNodeParams params);
67+
68+
/** @brief Function to load all plugins from the specified package
69+
*
70+
* @param params ROS parameters that contain the package name to load plugins from
6171
* @param factory BehaviorTreeFactory to register the plugins into
62-
* @param directory_path Full path to the directory to search for BehaviorTree plugins
63-
* @param params parameters passed to the ROS plugins
72+
* @param node node pointer that is shared with the ROS based Behavior plugins
6473
*/
65-
void LoadRosPlugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path,
66-
BT::RosNodeParams params);
74+
void RegisterPlugins(bt_server::Params& params, BT::BehaviorTreeFactory& factory,
75+
rclcpp::Node::SharedPtr node);
6776

6877
/**
6978
* @brief Function to register all Behaviors and BehaviorTrees from user specified packages

‎behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -123,6 +123,18 @@ class TreeExecutionServer
123123
return std::nullopt;
124124
}
125125

126+
protected:
127+
/**
128+
* @brief Method to register the trees and BT custom nodes.
129+
* It will call registerNodesIntoFactory().
130+
*
131+
* This callback will invoked asynchronously when this rclcpp Node is attached
132+
* to a rclcpp Executor.
133+
* Alternatively, it can be called synchronously in the constructor of a
134+
* __derived__ class of TreeExecutionServer.
135+
*/
136+
void executeRegistration();
137+
126138
private:
127139
struct Pimpl;
128140
std::unique_ptr<Pimpl> p_;

‎behaviortree_ros2/src/bt_utils.cpp

Lines changed: 41 additions & 40 deletions
Original file line numberDiff line numberDiff line change
@@ -93,53 +93,42 @@ void LoadBehaviorTrees(BT::BehaviorTreeFactory& factory,
9393
}
9494
}
9595

96-
void LoadRosPlugins(BT::BehaviorTreeFactory& factory, const std::string& directory_path,
97-
BT::RosNodeParams params)
96+
void LoadPlugin(BT::BehaviorTreeFactory& factory, const std::filesystem::path& file_path,
97+
BT::RosNodeParams params)
9898
{
99-
using std::filesystem::directory_iterator;
100-
101-
for(const auto& entry : directory_iterator(directory_path))
99+
const auto filename = file_path.filename();
100+
try
102101
{
103-
const auto filename = entry.path().filename();
104-
if(entry.path().extension() == ".so")
102+
BT::SharedLibrary loader(file_path.string());
103+
if(loader.hasSymbol(BT::PLUGIN_SYMBOL))
105104
{
106-
try
107-
{
108-
BT::SharedLibrary loader(entry.path().string());
109-
if(loader.hasSymbol(BT::PLUGIN_SYMBOL))
110-
{
111-
typedef void (*Func)(BehaviorTreeFactory&);
112-
auto func = (Func)loader.getSymbol(BT::PLUGIN_SYMBOL);
113-
func(factory);
114-
}
115-
else if(loader.hasSymbol(BT::ROS_PLUGIN_SYMBOL))
116-
{
117-
typedef void (*Func)(BT::BehaviorTreeFactory&, const BT::RosNodeParams&);
118-
auto func = (Func)loader.getSymbol(BT::ROS_PLUGIN_SYMBOL);
119-
func(factory, params);
120-
}
121-
else
122-
{
123-
RCLCPP_ERROR(kLogger, "Failed to load Plugin from file: %s.", filename.c_str());
124-
continue;
125-
}
126-
RCLCPP_INFO(kLogger, "Loaded ROS Plugin: %s", filename.c_str());
127-
}
128-
catch(const std::exception& e)
129-
{
130-
RCLCPP_ERROR(kLogger, "Failed to load ROS Plugin: %s \n %s", filename.c_str(),
131-
e.what());
132-
}
105+
typedef void (*Func)(BehaviorTreeFactory&);
106+
auto func = (Func)loader.getSymbol(BT::PLUGIN_SYMBOL);
107+
func(factory);
108+
}
109+
else if(loader.hasSymbol(BT::ROS_PLUGIN_SYMBOL))
110+
{
111+
typedef void (*Func)(BT::BehaviorTreeFactory&, const BT::RosNodeParams&);
112+
auto func = (Func)loader.getSymbol(BT::ROS_PLUGIN_SYMBOL);
113+
func(factory, params);
114+
}
115+
else
116+
{
117+
RCLCPP_ERROR(kLogger, "Failed to load Plugin from file: %s.", filename.c_str());
118+
return;
133119
}
120+
RCLCPP_INFO(kLogger, "Loaded ROS Plugin: %s", filename.c_str());
121+
}
122+
catch(const std::exception& ex)
123+
{
124+
RCLCPP_ERROR(kLogger, "Failed to load ROS Plugin: %s \n %s", filename.c_str(),
125+
ex.what());
134126
}
135127
}
136128

137-
void RegisterBehaviorTrees(bt_server::Params& params, BT::BehaviorTreeFactory& factory,
138-
rclcpp::Node::SharedPtr node)
129+
void RegisterPlugins(bt_server::Params& params, BT::BehaviorTreeFactory& factory,
130+
rclcpp::Node::SharedPtr node)
139131
{
140-
// clear the factory and load/reload it with the Behaviors and Trees specified by the user in their [bt_action_server] config yaml
141-
factory.clearRegisteredBehaviorTrees();
142-
143132
BT::RosNodeParams ros_params;
144133
ros_params.nh = node;
145134
ros_params.server_timeout = std::chrono::milliseconds(params.ros_plugins_timeout);
@@ -153,9 +142,21 @@ void RegisterBehaviorTrees(bt_server::Params& params, BT::BehaviorTreeFactory& f
153142
{
154143
continue;
155144
}
156-
LoadRosPlugins(factory, plugin_directory, ros_params);
145+
using std::filesystem::directory_iterator;
146+
147+
for(const auto& entry : directory_iterator(plugin_directory))
148+
{
149+
if(entry.path().extension() == ".so")
150+
{
151+
LoadPlugin(factory, entry.path(), ros_params);
152+
}
153+
}
157154
}
155+
}
158156

157+
void RegisterBehaviorTrees(bt_server::Params& params, BT::BehaviorTreeFactory& factory,
158+
rclcpp::Node::SharedPtr node)
159+
{
159160
for(const auto& tree_dir : params.behavior_trees)
160161
{
161162
const auto tree_directory = GetDirectoryPath(tree_dir);

‎behaviortree_ros2/src/tree_execution_server.cpp

Lines changed: 32 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -47,6 +47,9 @@ struct TreeExecutionServer::Pimpl
4747
std::string current_tree_name;
4848
std::shared_ptr<BT::Tree> tree;
4949
BT::Blackboard::Ptr global_blackboard;
50+
bool factory_initialized_ = false;
51+
52+
rclcpp::WallTimer<rclcpp::VoidCallbackType>::SharedPtr single_shot_timer;
5053

5154
rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID& uuid,
5255
std::shared_ptr<const ExecuteTree::Goal> goal);
@@ -70,6 +73,22 @@ TreeExecutionServer::Pimpl::Pimpl(const rclcpp::NodeOptions& node_options)
7073
TreeExecutionServer::~TreeExecutionServer()
7174
{}
7275

76+
void TreeExecutionServer::executeRegistration()
77+
{
78+
// Before executing check if we have new Behaviors or Subtrees to reload
79+
p_->factory.clearRegisteredBehaviorTrees();
80+
81+
p_->params = p_->param_listener->get_params();
82+
// user defined method
83+
registerNodesIntoFactory(p_->factory);
84+
// load plugins from multiple directories
85+
RegisterPlugins(p_->params, p_->factory, p_->node);
86+
// load trees (XML) from multiple directories
87+
RegisterBehaviorTrees(p_->params, p_->factory, p_->node);
88+
89+
p_->factory_initialized_ = true;
90+
}
91+
7392
TreeExecutionServer::TreeExecutionServer(const rclcpp::NodeOptions& options)
7493
: p_(new Pimpl(options))
7594
{
@@ -89,9 +108,18 @@ TreeExecutionServer::TreeExecutionServer(const rclcpp::NodeOptions& options)
89108
handle_accepted(std::move(goal_handle));
90109
});
91110

92-
// register the users Plugins and BehaviorTree.xml files into the factory
93-
RegisterBehaviorTrees(p_->params, p_->factory, p_->node);
94-
registerNodesIntoFactory(p_->factory);
111+
// we use a wall timer to run asynchronously executeRegistration();
112+
rclcpp::VoidCallbackType callback = [this]() {
113+
if(!p_->factory_initialized_)
114+
{
115+
executeRegistration();
116+
}
117+
// we must cancel the timer after the first execution
118+
p_->single_shot_timer->cancel();
119+
};
120+
121+
p_->single_shot_timer =
122+
p_->node->create_wall_timer(std::chrono::milliseconds(1), callback);
95123
}
96124

97125
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
@@ -151,9 +179,7 @@ void TreeExecutionServer::execute(
151179
// Before executing check if we have new Behaviors or Subtrees to reload
152180
if(p_->param_listener->is_old(p_->params))
153181
{
154-
p_->params = p_->param_listener->get_params();
155-
RegisterBehaviorTrees(p_->params, p_->factory, p_->node);
156-
registerNodesIntoFactory(p_->factory);
182+
executeRegistration();
157183
}
158184

159185
// Loop until something happens with ROS or the node completes

0 commit comments

Comments
 (0)