From 82aea49fb64bc27dc16957979a6bb5ccbc902d3c Mon Sep 17 00:00:00 2001 From: "marj3220@usherbrooke.ca" Date: Fri, 10 May 2024 16:33:02 -0400 Subject: [PATCH 1/2] Adding behavior tree on launch (initial commit) --- behaviortree_ros2/bt_executor_parameters.md | 8 ++++++++ behaviortree_ros2/src/bt_executor_parameters.yaml | 6 ++++++ behaviortree_ros2/src/tree_execution_server.cpp | 14 ++++++++++++++ 3 files changed, 28 insertions(+) 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..3fec195 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_) @@ -102,6 +106,16 @@ void TreeExecutionServer::executeRegistration() RegisterBehaviorTrees(p_->params, p_->factory, node_); p_->factory_initialized_ = true; + + // 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); + } } rclcpp::node_interfaces::NodeBaseInterface::SharedPtr From 1ce5216c66b53aad61599e932242a1bcfaf4967e Mon Sep 17 00:00:00 2001 From: "marj3220@usherbrooke.ca" Date: Sun, 12 May 2024 09:23:52 -0400 Subject: [PATCH 2/2] Using node_ attribute instead of passed node parameter --- behaviortree_ros2/src/tree_execution_server.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/behaviortree_ros2/src/tree_execution_server.cpp b/behaviortree_ros2/src/tree_execution_server.cpp index 3fec195..a58d641 100644 --- a/behaviortree_ros2/src/tree_execution_server.cpp +++ b/behaviortree_ros2/src/tree_execution_server.cpp @@ -73,7 +73,7 @@ TreeExecutionServer::TreeExecutionServer(const rclcpp::Node::SharedPtr& node) handle_accepted(std::move(goal_handle)); }); - p_->client_server = rclcpp_action::create_client(node, action_name); + p_->client_server = rclcpp_action::create_client(node_, action_name); // we use a wall timer to run asynchronously executeRegistration(); rclcpp::VoidCallbackType callback = [this]() { @@ -105,8 +105,6 @@ void TreeExecutionServer::executeRegistration() // load trees (XML) from multiple directories RegisterBehaviorTrees(p_->params, p_->factory, node_); - p_->factory_initialized_ = true; - // launch initalization behavior tree if set if(!p_->params.tree_on_initialization.empty()) { @@ -116,6 +114,8 @@ void TreeExecutionServer::executeRegistration() auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); p_->client_server->async_send_goal(goal_msg, send_goal_options); } + + p_->factory_initialized_ = true; } rclcpp::node_interfaces::NodeBaseInterface::SharedPtr