diff --git a/behaviortree_ros2/bt_executor_parameters.md b/behaviortree_ros2/bt_executor_parameters.md index 877d5ec..9b4cc93 100644 --- a/behaviortree_ros2/bt_executor_parameters.md +++ b/behaviortree_ros2/bt_executor_parameters.md @@ -72,3 +72,11 @@ List of 'package_name/subfolder' containing SubTrees to load into the BehaviorTr *Constraints:* - contains no duplicates + +## tree_on_initialization + +The name of the behavior tree to launch on intialization. Defaults to no behavior tree launched on initialization + +* Type: `string` +* Default Value: "" +* Read only: True diff --git a/behaviortree_ros2/src/bt_executor_parameters.yaml b/behaviortree_ros2/src/bt_executor_parameters.yaml index b6c36c1..5b2813e 100644 --- a/behaviortree_ros2/src/bt_executor_parameters.yaml +++ b/behaviortree_ros2/src/bt_executor_parameters.yaml @@ -47,3 +47,9 @@ bt_server: unique<>: null, } } + tree_on_initialization: { + type: string, + default_value: "", + read_only: true, + description: "The name of the behavior tree to launch on intialization. Defaults to no behavior tree launched on initialization" + } diff --git a/behaviortree_ros2/src/tree_execution_server.cpp b/behaviortree_ros2/src/tree_execution_server.cpp index 80c112a..a58d641 100644 --- a/behaviortree_ros2/src/tree_execution_server.cpp +++ b/behaviortree_ros2/src/tree_execution_server.cpp @@ -33,6 +33,8 @@ struct TreeExecutionServer::Pimpl rclcpp_action::Server::SharedPtr action_server; std::thread action_thread; + rclcpp_action::Client::SharedPtr client_server; + std::shared_ptr param_listener; bt_server::Params params; @@ -71,6 +73,8 @@ TreeExecutionServer::TreeExecutionServer(const rclcpp::Node::SharedPtr& node) handle_accepted(std::move(goal_handle)); }); + p_->client_server = rclcpp_action::create_client(node_, action_name); + // we use a wall timer to run asynchronously executeRegistration(); rclcpp::VoidCallbackType callback = [this]() { if(!p_->factory_initialized_) @@ -101,6 +105,16 @@ void TreeExecutionServer::executeRegistration() // load trees (XML) from multiple directories RegisterBehaviorTrees(p_->params, p_->factory, node_); + // launch initalization behavior tree if set + if(!p_->params.tree_on_initialization.empty()) + { + auto goal_msg = ExecuteTree::Goal(); + goal_msg.target_tree = p_->params.tree_on_initialization; + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + p_->client_server->async_send_goal(goal_msg, send_goal_options); + } + p_->factory_initialized_ = true; }