Enable Groot2 monitoring

nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp

@@ -22,6 +22,7 @@
 
 #include "behaviortree_cpp/behavior_tree.h"
 #include "behaviortree_cpp/bt_factory.h"
+#include "behaviortree_cpp/loggers/groot2_publisher.h"
 #include "behaviortree_cpp/xml_parsing.h"
 
 #include "rclcpp/rclcpp.hpp"
@@ -85,6 +86,18 @@ class BehaviorTreeEngine
     const std::string & file_path,
     BT::Blackboard::Ptr blackboard);
 
+  /**
+   * @brief Add Groot2 monitor to publish BT status changes
+   * @param tree BT to monitor
+   * @param publisher_port Publisher port for the Groot2 monitor
+   */
+  void addGrootMonitoring(BT::Tree * tree, uint16_t publisher_port);
+
+  /**
+   * @brief Reset groot monitor
+   */
+  void resetGrootMonitor();
+
   /**
    * @brief Function to explicitly reset all BT nodes to initial state
    * @param tree Tree to halt
@@ -97,6 +110,9 @@ class BehaviorTreeEngine
 
   // Clock
   rclcpp::Clock::SharedPtr clock_;
+
+  // Groot2 monitor
+  std::unique_ptr<BT::Groot2Publisher> groot_monitor_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

@@ -20,9 +20,11 @@
 #include <chrono>
 
 #include "behaviortree_cpp/action_node.h"
+#include "behaviortree_cpp/json_export.h"
 #include "nav2_util/node_utils.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
 #include "nav2_behavior_tree/bt_utils.hpp"
+#include "nav2_behavior_tree/json_utils.hpp"
 
 namespace nav2_behavior_tree
 {

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp

@@ -91,6 +91,13 @@ class BtActionServer
    */
   bool on_cleanup();
 
+  /**
+   * @brief Enable (or disable) Groot2 monitoring of BT
+   * @param enable Groot2 monitoring
+   * @param server_port Groot2 Server port, first of the pair (server_port, publisher_port)
+   */
+  void setGrootMonitoring(const bool enable, const unsigned server_port);
+
   /**
    * @brief Replace current BT with another one
    * @param bt_xml_filename The file containing the new BT, uses default filename if empty
@@ -277,6 +284,10 @@ class BtActionServer
   // should the BT be reloaded even if the same xml filename is requested?
   bool always_reload_bt_xml_ = false;
 
+  // Parameters for Groot2 monitoring
+  bool enable_groot_monitoring_ = true;
+  int groot_server_port_ = 1667;
+
   // User-provided callbacks
   OnGoalReceivedCallback on_goal_received_callback_;
   OnLoopCallback on_loop_callback_;

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -239,10 +239,18 @@ bool BtActionServer<ActionT>::on_cleanup()
   current_bt_xml_filename_.clear();
   blackboard_.reset();
   bt_->haltAllActions(tree_);
+  bt_->resetGrootMonitor();
   bt_.reset();
   return true;
 }
 
+template<class ActionT>
+void BtActionServer<ActionT>::setGrootMonitoring(const bool enable, const unsigned server_port)
+{
+  enable_groot_monitoring_ = enable;
+  groot_server_port_ = server_port;
+}
+
 template<class ActionT>
 bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filename)
 {
@@ -255,6 +263,9 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
     return true;
   }
 
+  // if a new tree is created, than the Groot2 Publisher must be destroyed
+  bt_->resetGrootMonitor();
+
   // Read the input BT XML from the specified file into a string
   std::ifstream xml_file(filename);
 
@@ -285,6 +296,15 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
   topic_logger_ = std::make_unique<RosTopicLogger>(client_node_, tree_);
 
   current_bt_xml_filename_ = filename;
+
+  // Enable monitoring with Groot2
+  if (enable_groot_monitoring_) {
+    bt_->addGrootMonitoring(&tree_, groot_server_port_);
+    RCLCPP_DEBUG(
+      logger_, "Enabling Groot2 monitoring for %s: %d",
+      action_name_.c_str(), groot_server_port_);
+  }
+
   return true;
 }
 

nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp

@@ -20,9 +20,11 @@
 #include <chrono>
 
 #include "behaviortree_cpp/action_node.h"
+#include "behaviortree_cpp/json_export.h"
 #include "nav2_util/node_utils.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_behavior_tree/bt_utils.hpp"
+#include "nav2_behavior_tree/json_utils.hpp"
 #include "nav2_util/service_client.hpp"
 
 namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp

@@ -43,6 +43,13 @@
 template<>
 inline geometry_msgs::msg::Point convertFromString(const StringView key)
 {
+  // if string starts with "json:{", try to parse it as json
+  if (StartWith(key, "json:")) {
+    auto new_key = key;
+    new_key.remove_prefix(5);
+    return convertFromJSON<geometry_msgs::msg::Point>(new_key);
+  }
+
   // three real numbers separated by semicolons
   auto parts = BT::splitString(key, ';');
   if (parts.size() != 3) {
@@ -64,6 +71,13 @@
 template<>
 inline geometry_msgs::msg::Quaternion convertFromString(const StringView key)
 {
+  // if string starts with "json:{", try to parse it as json
+  if (StartWith(key, "json:")) {
+    auto new_key = key;
+    new_key.remove_prefix(5);
+    return convertFromJSON<geometry_msgs::msg::Quaternion>(new_key);
+  }
+
   // four real numbers separated by semicolons
   auto parts = BT::splitString(key, ';');
   if (parts.size() != 4) {
@@ -86,6 +100,13 @@
 template<>
 inline geometry_msgs::msg::PoseStamped convertFromString(const StringView key)
 {
+  // if string starts with "json:{", try to parse it as json
+  if (StartWith(key, "json:")) {
+    auto new_key = key;
+    new_key.remove_prefix(5);
+    return convertFromJSON<geometry_msgs::msg::PoseStamped>(new_key);
+  }
+
   // 7 real numbers separated by semicolons
   auto parts = BT::splitString(key, ';');
   if (parts.size() != 9) {
@@ -113,6 +134,13 @@
 template<>
 inline std::vector<geometry_msgs::msg::PoseStamped> convertFromString(const StringView key)
 {
+  // if string starts with "json:{", try to parse it as json
+  if (StartWith(key, "json:")) {
+    auto new_key = key;
+    new_key.remove_prefix(5);
+    return convertFromJSON<std::vector<geometry_msgs::msg::PoseStamped>>(new_key);
+  }
+
   auto parts = BT::splitString(key, ';');
   if (parts.size() % 9 != 0) {
     throw std::runtime_error("invalid number of fields for std::vector<PoseStamped> attribute)");
@@ -143,6 +171,13 @@
 template<>
 inline nav_msgs::msg::Goals convertFromString(const StringView key)
 {
+  // if string starts with "json:{", try to parse it as json
+  if (StartWith(key, "json:")) {
+    auto new_key = key;
+    new_key.remove_prefix(5);
+    return convertFromJSON<nav_msgs::msg::Goals>(new_key);
+  }
+
   auto parts = BT::splitString(key, ';');
   if ((parts.size() - 2) % 9 != 0) {
     throw std::runtime_error("invalid number of fields for Goals attribute)");
@@ -175,6 +210,13 @@
 template<>
 inline nav_msgs::msg::Path convertFromString(const StringView key)
 {
+  // if string starts with "json:{", try to parse it as json
+  if (StartWith(key, "json:")) {
+    auto new_key = key;
+    new_key.remove_prefix(5);
+    return convertFromJSON<nav_msgs::msg::Path>(new_key);
+  }
+
   auto parts = BT::splitString(key, ';');
   if ((parts.size() - 2) % 9 != 0) {
     throw std::runtime_error("invalid number of fields for Path attribute)");
@@ -207,6 +249,13 @@
 template<>
 inline std::chrono::milliseconds convertFromString<std::chrono::milliseconds>(const StringView key)
 {
+  // if string starts with "json:{", try to parse it as json
+  if (StartWith(key, "json:")) {
+    auto new_key = key;
+    new_key.remove_prefix(5);
+    return convertFromJSON<std::chrono::milliseconds>(new_key);
+  }
+
   return std::chrono::milliseconds(std::stoul(key.data()));
 }
 

nav2_behavior_tree/include/nav2_behavior_tree/json_utils.hpp

@@ -0,0 +1,141 @@
+// 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 NAV2_BEHAVIOR_TREE__JSON_UTILS_HPP_
+#define NAV2_BEHAVIOR_TREE__JSON_UTILS_HPP_
+
+#include <string>
+#include <set>
+#include <vector>
+
+#include "rclcpp/time.hpp"
+#include "rclcpp/node.hpp"
+#include "behaviortree_cpp/json_export.h"
+#include "geometry_msgs/msg/point.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/quaternion.hpp"
+#include "nav_msgs/msg/goals.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "nav2_msgs/msg/waypoint_status.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
+
+namespace geometry_msgs::msg
+{
+
+BT_JSON_CONVERTER(geometry_msgs::msg::Point, msg)
+{
+  add_field("x", &msg.x);
+  add_field("y", &msg.y);
+  add_field("z", &msg.z);
+}
+
+BT_JSON_CONVERTER(geometry_msgs::msg::Quaternion, msg)
+{
+  add_field("x", &msg.x);
+  add_field("y", &msg.y);
+  add_field("z", &msg.z);
+  add_field("w", &msg.w);
+}
+
+BT_JSON_CONVERTER(geometry_msgs::msg::Pose, msg)
+{
+  add_field("position", &msg.position);
+  add_field("orientation", &msg.orientation);
+}
+
+BT_JSON_CONVERTER(geometry_msgs::msg::PoseStamped, msg)
+{
+  add_field("header", &msg.header);
+  add_field("pose", &msg.pose);
+}
+
+}  // namespace geometry_msgs::msg
+
+namespace nav_msgs::msg
+{
+
+BT_JSON_CONVERTER(nav_msgs::msg::Goals, msg)
+{
+  add_field("header", &msg.header);
+  add_field("goals", &msg.goals);
+}
+
+BT_JSON_CONVERTER(nav_msgs::msg::Path, msg)
+{
+  add_field("header", &msg.header);
+  add_field("poses", &msg.poses);
+}
+
+}  // namespace nav_msgs::msg
+
+namespace nav2_msgs::msg
+{
+
+BT_JSON_CONVERTER(nav2_msgs::msg::WaypointStatus, msg)
+{
+  add_field("waypoint_status", &msg.waypoint_status);
+  add_field("waypoint_index", &msg.waypoint_index);
+  add_field("waypoint_pose", &msg.waypoint_pose);
+  add_field("error_code", &msg.error_code);
+  add_field("error_msg", &msg.error_msg);
+}
+
+}  // namespace nav2_msgs::msg
+
+namespace std
+{
+
+inline void from_json(const nlohmann::json & js, std::chrono::milliseconds & dest)
+{
+  if (js.contains("ms")) {
+    dest = std::chrono::milliseconds(js.at("ms").get<int>());
+  } else {
+    throw std::runtime_error("Invalid JSON for std::chrono::milliseconds");
+  }
+}
+
+inline void to_json(nlohmann::json & js, const std::chrono::milliseconds & src)
+{
+  js["__type"] = "std::chrono::milliseconds";
+  js["ms"] = src.count();
+}
+
+}  // namespace std
+
+#endif  // NAV2_BEHAVIOR_TREE__JSON_UTILS_HPP_

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp

@@ -78,6 +78,10 @@ class ComputePathThroughPosesAction
    */
   static BT::PortsList providedPorts()
   {
+    // Register JSON definitions for the types used in the ports
+    BT::RegisterJsonDefinition<nav_msgs::msg::Path>();
+    BT::RegisterJsonDefinition<geometry_msgs::msg::PoseStamped>();
+
     return providedBasicPorts(
       {
         BT::InputPort<nav_msgs::msg::Goals>(

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp

@@ -75,6 +75,10 @@ class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePa
    */
   static BT::PortsList providedPorts()
   {
+    // Register JSON definitions for the types used in the ports
+    BT::RegisterJsonDefinition<nav_msgs::msg::Path>();
+    BT::RegisterJsonDefinition<geometry_msgs::msg::PoseStamped>();
+
     return providedBasicPorts(
       {
         BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination to plan to"),