Added Groot2 monitor

plansys2_bt_actions/CMakeLists.txt

@@ -11,6 +11,7 @@ find_package(rclcpp_lifecycle REQUIRED)
 find_package(plansys2_executor REQUIRED)
 find_package(behaviortree_cpp REQUIRED)
 find_package(lifecycle_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
 
 set(BT_ACTIONS_SOURCES
   src/plansys2_bt_actions/BTAction.cpp
@@ -28,6 +29,8 @@ target_link_libraries(${PROJECT_NAME}
   rclcpp::rclcpp
   rclcpp_action::rclcpp_action
   rclcpp_lifecycle::rclcpp_lifecycle
+  PRIVATE
+  ${std_msgs_TARGETS}
 )
 
 add_executable(bt_action_node

plansys2_bt_actions/README.md

@@ -8,14 +8,12 @@ These parameters are
 1. `action_name`: The name of the `plansys2` action to implement. Note that this name must match what is in your pddl domain file.
 2. `bt_xml_file`: An absolute path to the BT `.xml` file to execute.
 3. `plugins`: a list of BehaviorTree.CPP shared libraries to load. Any BT node which is in the `.xml` but is not provided by the BehaviorTree.CPP library itself must be in one of the libraries specified
-4. `enable_groot_monitoring`: a boolean which specifies if ZMQ publisher should be created, for use with [Groot](https://github.com/BehaviorTree/Groot) (default is `false`)
-5. `publisher_port`: the ZMQ publisher port to use (if `enable_groot_monitoring` is enabled)
-6. `server_port`: the ZMQ server port to use (if `enable_groot_monitoring` is enabled)
-7. `max_msgs_per_second`: max ZMQ messages per second (if `enable_groot_monitoring` is enabled)
-8. `bt_file_logging`: a boolean which [enables logging of BT state changes in `.fbl` files](https://www.behaviortree.dev/tutorial_05_subtrees/), useful for playing back behavior tree execution using `Groot` (default is `false`)
-9. `bt_minitrace_logging`: a boolean which enables logging of `.json` files for recording the execution time of each node (default is `false`)
+4. `enable_groot_monitoring`: a boolean which specifies if a Groot2 publisher should be created, for use with [Groot](https://www.behaviortree.dev/groot/) (default is `false`)
+5. `server_port`: the Groot2 server port to use (if `enable_groot_monitoring` is enabled)
+6. `bt_file_logging`: a boolean which [enables logging of BT state changes in `.btlog` files](https://www.behaviortree.dev/docs/tutorial-basics/tutorial_11_groot2), useful for playing back behavior tree execution using `Groot2` (default is `false`)
+7. `bt_minitrace_logging`: a boolean which enables logging of `.json` files for recording the execution time of each node (default is `false`)
 
-Files created by the `.fbl` and minitrace loggers are stored in `/tmp/<node_name>/`, with names containing a timestamp.
+Files created by the `.btlog` and minitrace loggers are stored in `/tmp/<node_name>/`, with names containing a timestamp.
 
 ### BT node for calling ROS2 action servers
 The `BtActionNode` template class provides a convenient means of calling ROS2 action servers from within a BT.  It takes care of the details of setting up and handling a ROS action client, reducing code duplication and providing a simple API.

plansys2_bt_actions/include/plansys2_bt_actions/BTAction.hpp

@@ -24,6 +24,7 @@
 #include "behaviortree_cpp/xml_parsing.h"
 #include "behaviortree_cpp/loggers/bt_file_logger_v2.h"
 #include "behaviortree_cpp/loggers/bt_minitrace_logger.h"
+#include "behaviortree_cpp/loggers/groot2_publisher.h"
 
 #include "plansys2_executor/ActionExecutorClient.hpp"
 #include "rclcpp/rclcpp.hpp"
@@ -34,9 +35,7 @@ namespace plansys2
 class BTAction : public plansys2::ActionExecutorClient
 {
 public:
-  explicit BTAction(
-    const std::string & action,
-    const std::chrono::nanoseconds & rate);
+  explicit BTAction(const std::string & action, const std::chrono::nanoseconds & rate);
 
   const std::string & getActionName() const {return action_;}
   const std::string & getBTFile() const {return bt_xml_file_;}
@@ -56,6 +55,18 @@ class BTAction : public plansys2::ActionExecutorClient
 
   void do_work();
 
+  /**
+   * @brief Add Groot2 monitor to publish BT status changes
+   * @param tree BT to monitor
+   * @param server_port Groot2 Server port, first of the pair (server_port, publisher_port)
+   */
+  void addGrootMonitoring(BT::Tree * tree, uint16_t server_port);
+
+  /**
+   * @brief Reset Groot2 monitor
+   */
+  void resetGrootMonitor();
+
   BT::BehaviorTreeFactory factory_;
 
 private:
@@ -67,6 +78,8 @@ class BTAction : public plansys2::ActionExecutorClient
   bool finished_;
   std::unique_ptr<BT::FileLogger2> bt_file_logger_;
   std::unique_ptr<BT::MinitraceLogger> bt_minitrace_logger_;
+  // Groot2 monitor
+  std::unique_ptr<BT::Groot2Publisher> groot_monitor_;
 };
 
 }  // namespace plansys2

plansys2_bt_actions/include/plansys2_bt_actions/JSONUtils.hpp

@@ -0,0 +1,49 @@
+// Copyright (c) 2025 Alberto J. Tudela Roldán
+// Copyright (c) 2025 Grupo Avispa, DTE, Universidad de Málaga
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef PLANSYS2_BT_ACTIONS__JSONUTILS_HPP_
+#define PLANSYS2_BT_ACTIONS__JSONUTILS_HPP_
+
+#include <string>
+
+#include "behaviortree_cpp/json_export.h"
+#include "builtin_interfaces/msg/time.hpp"
+#include "std_msgs/msg/header.hpp"
+
+// The follow templates are required when using Groot 2 to visualize the BT. They
+// convert the data types into JSON format easy for visualization.
+
+namespace builtin_interfaces::msg
+{
+BT_JSON_CONVERTER(builtin_interfaces::msg::Time, msg)
+{
+  add_field("sec", &msg.sec);
+  add_field("nanosec", &msg.nanosec);
+}
+
+}  // namespace builtin_interfaces::msg
+
+namespace std_msgs::msg
+{
+
+BT_JSON_CONVERTER(std_msgs::msg::Header, msg)
+{
+  add_field("stamp", &msg.stamp);
+  add_field("frame_id", &msg.frame_id);
+}
+
+}  // namespace std_msgs::msg
+
+#endif  // PLANSYS2_BT_ACTIONS__JSONUTILS_HPP_

plansys2_bt_actions/package.xml

@@ -17,6 +17,8 @@
   <depend>rclcpp_lifecycle</depend>
   <depend>plansys2_executor</depend>
   <depend>behaviortree_cpp</depend>
+  <depend>lifecycle_msgs</depend>
+  <depend>std_msgs</depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>

plansys2_bt_actions/src/plansys2_bt_actions/BTAction.cpp

@@ -22,22 +22,24 @@
 #include <memory>
 #include <chrono>
 
+#include "behaviortree_cpp/json_export.h"
 #include "behaviortree_cpp/utils/shared_library.h"
+#include "std_msgs/msg/header.hpp"
 #include "plansys2_bt_actions/BTAction.hpp"
+#include "plansys2_bt_actions/JSONUtils.hpp"
 
 namespace plansys2
 {
 
-BTAction::BTAction(
-  const std::string & action,
-  const std::chrono::nanoseconds & rate)
+BTAction::BTAction(const std::string & action, const std::chrono::nanoseconds & rate)
 : ActionExecutorClient(action, rate)
 {
   declare_parameter<std::string>("bt_xml_file", "");
-  declare_parameter<std::vector<std::string>>(
-    "plugins", std::vector<std::string>({}));
+  declare_parameter<std::vector<std::string>>("plugins", std::vector<std::string>({}));
   declare_parameter<bool>("bt_file_logging", false);
   declare_parameter<bool>("bt_minitrace_logging", false);
+  declare_parameter<bool>("enable_groot_monitoring", false);
+  declare_parameter<int>("server_port", -1);
 }
 
 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
@@ -75,6 +77,9 @@ BTAction::on_cleanup(const rclcpp_lifecycle::State & previous_state)
 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
 BTAction::on_activate(const rclcpp_lifecycle::State & previous_state)
 {
+  // If a new tree is created, than the Groot2 Publisher must be destroyed
+  resetGrootMonitor();
+
   try {
     tree_ = factory_.createTreeFromFile(bt_xml_file_, blackboard_);
   } catch (const std::exception & ex) {
@@ -108,7 +113,7 @@ BTAction::on_activate(const rclcpp_lifecycle::State & previous_state)
     filename << std::put_time(std::localtime(&now_time_t), "%Y_%m_%d__%H_%M_%S");
 
     if (get_parameter("bt_file_logging").as_bool()) {
-      std::string filename_extension = filename.str() + ".fbl";
+      std::string filename_extension = filename.str() + ".btlog";
       RCLCPP_INFO_STREAM(
         get_logger(),
         "Logging to file: " << filename_extension);
@@ -126,6 +131,17 @@ BTAction::on_activate(const rclcpp_lifecycle::State & previous_state)
     }
   }
 
+  bool enable_groot_monitoring = get_parameter("enable_groot_monitoring").as_bool();
+  int server_port = get_parameter("server_port").as_int();
+  if (enable_groot_monitoring) {
+    if (server_port <= 0) {
+      RCLCPP_WARN(get_logger(), "Groot2 monitoring port not provided, disabling it");
+    } else {
+      RCLCPP_INFO(get_logger(), "Enabling Groot2 monitoring on port: %d", server_port);
+      addGrootMonitoring(&tree_, server_port);
+    }
+  }
+
   finished_ = false;
   return ActionExecutorClient::on_activate(previous_state);
 }
@@ -136,12 +152,12 @@ BTAction::on_deactivate(const rclcpp_lifecycle::State & previous_state)
   bt_minitrace_logger_.reset();
   bt_file_logger_.reset();
   tree_.haltTree();
+  resetGrootMonitor();
 
   return ActionExecutorClient::on_deactivate(previous_state);
 }
 
-void
-BTAction::do_work()
+void BTAction::do_work()
 {
   if (!finished_) {
     BT::NodeStatus result;
@@ -173,4 +189,21 @@ BTAction::do_work()
   }
 }
 
+void BTAction::addGrootMonitoring(BT::Tree * tree, uint16_t server_port)
+{
+  // This logger publish status changes using Groot2
+  groot_monitor_ = std::make_unique<BT::Groot2Publisher>(*tree, server_port);
+
+  // Register common types JSON definitions
+  BT::RegisterJsonDefinition<builtin_interfaces::msg::Time>();
+  BT::RegisterJsonDefinition<std_msgs::msg::Header>();
+}
+
+void BTAction::resetGrootMonitor()
+{
+  if (groot_monitor_) {
+    groot_monitor_.reset();
+  }
+}
+
 }  // namespace plansys2

plansys2_executor/include/plansys2_executor/ComputeBT.hpp

@@ -18,6 +18,8 @@
 #include <memory>
 #include <string>
 
+#include "behaviortree_cpp/loggers/groot2_publisher.h"
+
 #include "plansys2_domain_expert/DomainExpertClient.hpp"
 #include "plansys2_domain_expert/DomainExpertNode.hpp"
 #include "plansys2_executor/BTBuilder.hpp"
@@ -82,6 +84,21 @@ class ComputeBT : public rclcpp_lifecycle::LifecycleNode
   void savePlan(const plansys2_msgs::msg::Plan & plan, const std::string & filename) const;
   void saveBT(const std::string & bt_xml, const std::string & filename) const;
   void saveDotGraph(const std::string & dotgraph, const std::string & filename) const;
+
+  /**
+   * @brief Add Groot2 monitor to publish BT status changes
+   * @param tree BT to monitor
+   * @param server_port Groot2 Server port, first of the pair (server_port, publisher_port)
+   */
+  void addGrootMonitoring(BT::Tree * tree, uint16_t server_port);
+
+  /**
+   * @brief Reset Groot2 monitor
+   */
+  void resetGrootMonitor();
+
+  // Groot2 monitor
+  std::unique_ptr<BT::Groot2Publisher> groot_monitor_;
 };
 
 }  // namespace plansys2

plansys2_executor/include/plansys2_executor/ExecutorNode.hpp

@@ -33,6 +33,7 @@
 
 #include "behaviortree_cpp/bt_factory.h"
 #include "behaviortree_cpp/blackboard.h"
+#include "behaviortree_cpp/loggers/groot2_publisher.h"
 
 #include "plansys2_msgs/action/execute_plan.hpp"
 #include "plansys2_msgs/msg/action_execution_info.hpp"
@@ -175,6 +176,21 @@ class ExecutorNode : public rclcpp_lifecycle::LifecycleNode
   int executor_state_;
 
   PlanRuntineInfo runtime_info_;
+
+  /**
+   * @brief Add Groot2 monitor to publish BT status changes
+   * @param tree BT to monitor
+   * @param server_port Groot2 Server port, first of the pair (server_port, publisher_port)
+   */
+  void addGrootMonitoring(BT::Tree * tree, uint16_t server_port);
+
+  /**
+   * @brief Reset Groot2 monitor
+   */
+  void resetGrootMonitor();
+
+  // Groot2 monitor
+  std::unique_ptr<BT::Groot2Publisher> groot_monitor_;
 };
 
 }  // namespace plansys2

plansys2_executor/src/plansys2_executor/ComputeBT.cpp

@@ -69,6 +69,9 @@ ComputeBT::ComputeBT()
       0.0);
   }
 
+  this->declare_parameter<bool>("enable_groot_monitoring", false);
+  this->declare_parameter<int>("server_port", 1800);
+
   compute_bt_srv_ = create_service<std_srvs::srv::Trigger>(
     "compute_bt",
     std::bind(
@@ -354,6 +357,13 @@ ComputeBT::computeBTCallback(
 
   auto tree = factory.createTreeFromText(bt_xml_tree, blackboard);
 
+  bool enable_groot_monitoring = get_parameter("enable_groot_monitoring").as_bool();
+  int server_port = get_parameter("server_port").as_int();
+  if (enable_groot_monitoring) {
+    RCLCPP_INFO(get_logger(), "Enabling Groot2 monitoring on port: %d", get_name(), server_port);
+    addGrootMonitoring(&tree, server_port);
+  }
+
   finish = true;
   t.join();
 
@@ -412,4 +422,17 @@ ComputeBT::saveDotGraph(const std::string & dotgraph, const std::string & filena
   }
 }
 
+void ComputeBT::addGrootMonitoring(BT::Tree * tree, uint16_t server_port)
+{
+  // This logger publish status changes using Groot2
+  groot_monitor_ = std::make_unique<BT::Groot2Publisher>(*tree, server_port);
+}
+
+void ComputeBT::resetGrootMonitor()
+{
+  if (groot_monitor_) {
+    groot_monitor_.reset();
+  }
+}
+
 }  // namespace plansys2

plansys2_executor/src/plansys2_executor/ExecutorNode.cpp

@@ -80,6 +80,9 @@ ExecutorNode::ExecutorNode()
       0.0);
   }
 
+  this->declare_parameter<bool>("enable_groot_monitoring", false);
+  this->declare_parameter<int>("server_port", 1800);
+
   execute_plan_action_server_ = rclcpp_action::create_server<ExecutePlan>(
     this->get_node_base_interface(),
     this->get_node_clock_interface(),
@@ -463,6 +466,13 @@ ExecutorNode::get_tree_from_plan(PlanRuntineInfo & runtime_info)
   *runtime_info.current_tree = {
     factory.createTreeFromText(bt_xml_tree, blackboard), blackboard, bt_builder};
 
+  bool enable_groot_monitoring = get_parameter("enable_groot_monitoring").as_bool();
+  int server_port = get_parameter("server_port").as_int();
+  if (enable_groot_monitoring) {
+    RCLCPP_INFO(get_logger(), "Enabling Groot2 monitoring on port: %d", get_name(), server_port);
+    addGrootMonitoring(&runtime_info.current_tree->tree, server_port);
+  }
+
   return runtime_info.current_tree != nullptr;
 }
 
@@ -857,4 +867,17 @@ ExecutorNode::execution_cycle()
   }
 }
 
+void ExecutorNode::addGrootMonitoring(BT::Tree * tree, uint16_t server_port)
+{
+  // This logger publish status changes using Groot2
+  groot_monitor_ = std::make_unique<BT::Groot2Publisher>(*tree, server_port);
+}
+
+void ExecutorNode::resetGrootMonitor()
+{
+  if (groot_monitor_) {
+    groot_monitor_.reset();
+  }
+}
+
 }  // namespace plansys2