Add service to get the list of loaded trees

Author: agonzat

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<ExecuteTree>::SharedPtr action_server;
@@ -43,6 +47,8 @@ struct TreeExecutionServer::Pimpl
   std::shared_ptr<bt_server::ParamListener> param_listener;
   bt_server::Params params;
 
+  rclcpp::Service<GetTrees>::SharedPtr get_trees_service;
+
   BT::BehaviorTreeFactory factory;
   std::shared_ptr<BT::Groot2Publisher> groot_publisher;
 
@@ -88,6 +94,13 @@ TreeExecutionServer::TreeExecutionServer(const rclcpp::Node::SharedPtr& node)
     p_->single_shot_timer->cancel();
   };
 
+  p_->get_trees_service = node_->create_service<GetTrees>(
+      "get_loaded_trees",
+      [this](const std::shared_ptr<GetTrees::Request> _,
+             std::shared_ptr<GetTrees::Response> response) {
+        response->tree_ids = p_->factory.registeredBehaviorTrees();
+      });
+
   p_->single_shot_timer =
       node_->create_wall_timer(std::chrono::milliseconds(1), callback);
 }

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")
 

btcpp_ros2_interfaces/srv/GetTrees.srv

@@ -0,0 +1,8 @@
+#### Request ####
+# Empty
+
+---
+#### Result ####
+
+# Ids of the available trees
+string[] tree_ids