diff --git a/behaviortree_ros2/src/tree_execution_server.cpp b/behaviortree_ros2/src/tree_execution_server.cpp index f712a52..f63d22a 100644 --- a/behaviortree_ros2/src/tree_execution_server.cpp +++ b/behaviortree_ros2/src/tree_execution_server.cpp @@ -25,6 +25,8 @@ #include "behaviortree_cpp/loggers/groot2_publisher.h" +#include "btcpp_ros2_interfaces/srv/get_trees.hpp" + // generated file #include "bt_executor_parameters.hpp" namespace @@ -35,6 +37,8 @@ static const auto kLogger = rclcpp::get_logger("bt_action_server"); namespace BT { +using GetTrees = btcpp_ros2_interfaces::srv::GetTrees; + struct TreeExecutionServer::Pimpl { rclcpp_action::Server::SharedPtr action_server; @@ -43,6 +47,8 @@ struct TreeExecutionServer::Pimpl std::shared_ptr param_listener; bt_server::Params params; + rclcpp::Service::SharedPtr get_trees_service; + BT::BehaviorTreeFactory factory; std::shared_ptr groot_publisher; @@ -88,6 +94,12 @@ TreeExecutionServer::TreeExecutionServer(const rclcpp::Node::SharedPtr& node) p_->single_shot_timer->cancel(); }; + p_->get_trees_service = node_->create_service( + "get_loaded_trees", [this](const std::shared_ptr _, + std::shared_ptr response) { + response->tree_ids = p_->factory.registeredBehaviorTrees(); + }); + p_->single_shot_timer = node_->create_wall_timer(std::chrono::milliseconds(1), callback); } diff --git a/btcpp_ros2_interfaces/CMakeLists.txt b/btcpp_ros2_interfaces/CMakeLists.txt index 23ca9aa..764f3cd 100644 --- a/btcpp_ros2_interfaces/CMakeLists.txt +++ b/btcpp_ros2_interfaces/CMakeLists.txt @@ -9,6 +9,7 @@ find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(btcpp_ros2_interfaces "msg/NodeStatus.msg" + "srv/GetTrees.srv" "action/ExecuteTree.action" "action/Sleep.action") diff --git a/btcpp_ros2_interfaces/srv/GetTrees.srv b/btcpp_ros2_interfaces/srv/GetTrees.srv new file mode 100644 index 0000000..e895280 --- /dev/null +++ b/btcpp_ros2_interfaces/srv/GetTrees.srv @@ -0,0 +1,8 @@ +#### Request #### +# Empty + +--- +#### Result #### + +# Ids of the available trees +string[] tree_ids