Behavior tree execution on launch

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

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"
+ }

behaviortree_ros2/src/tree_execution_server.cpp

@@ -33,6 +33,8 @@ struct TreeExecutionServer::Pimpl
  rclcpp_action::Server<ExecuteTree>::SharedPtr action_server;
  std::thread action_thread;
 
+ rclcpp_action::Client<ExecuteTree>::SharedPtr client_server;
+
  std::shared_ptr<bt_server::ParamListener> 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<ExecuteTree>(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<ExecuteTree>::SendGoalOptions();
+ p_->client_server->async_send_goal(goal_msg, send_goal_options);
+ }
+
  p_->factory_initialized_ = true;
 }