removing live groot monitoring from Nav2

nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp

@@ -83,24 +83,6 @@ class BehaviorTreeEngine
     const std::string & file_path,
     BT::Blackboard::Ptr blackboard);
 
-  /**
-   * @brief Add groot monitor to publish BT status changes
-   * @param tree BT to monitor
-   * @param publisher_port ZMQ publisher port for the Groot monitor
-   * @param server_port ZMQ server port for the Groot monitor
-   * @param max_msg_per_second Maximum number of messages that can be sent per second
-   */
-  void addGrootMonitoring(
-    BT::Tree * tree,
-    uint16_t publisher_port,
-    uint16_t server_port,
-    uint16_t max_msg_per_second = 25);
-
-  /**
-   * @brief Reset groot monitor
-   */
-  void resetGrootMonitor();
-
   /**
    * @brief Function to explicitly reset all BT nodes to initial state
    * @param root_node Pointer to BT root node
@@ -110,8 +92,6 @@ class BehaviorTreeEngine
 protected:
   // The factory that will be used to dynamically construct the behavior tree
   BT::BehaviorTreeFactory factory_;
-
-  static inline std::unique_ptr<BT::PublisherZMQ> groot_monitor_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp

@@ -86,17 +86,6 @@ class BtActionServer
    */
   bool on_cleanup();
 
-  /**
-   * @brief Enable (or disable) Groot monitoring of BT
-   * @param Enable Groot monitoring
-   * @param Publisher port
-   * @param Server port
-   */
-  void setGrootMonitoring(
-    const bool enable,
-    const unsigned publisher_port,
-    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
@@ -242,11 +231,6 @@ class BtActionServer
   // Default timeout value while waiting for response from a server
   std::chrono::milliseconds default_server_timeout_;
 
-  // Parameters for Groot monitoring
-  bool enable_groot_monitoring_ = true;
-  int groot_publisher_port_ = 1666;
-  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

@@ -142,22 +142,10 @@ bool BtActionServer<ActionT>::on_cleanup()
   current_bt_xml_filename_.clear();
   blackboard_.reset();
   bt_->haltAllActions(tree_.rootNode());
-  bt_->resetGrootMonitor();
   bt_.reset();
   return true;
 }
 
-template<class ActionT>
-void BtActionServer<ActionT>::setGrootMonitoring(
-  const bool enable,
-  const unsigned publisher_port,
-  const unsigned server_port)
-{
-  enable_groot_monitoring_ = enable;
-  groot_publisher_port_ = publisher_port;
-  groot_server_port_ = server_port;
-}
-
 template<class ActionT>
 bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filename)
 {
@@ -170,9 +158,6 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
     return true;
   }
 
-  // if a new tree is created, than the ZMQ Publisher must be destroyed
-  bt_->resetGrootMonitor();
-
   // Read the input BT XML from the specified file into a string
   std::ifstream xml_file(filename);
 
@@ -190,20 +175,6 @@ 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 Groot
-  if (enable_groot_monitoring_) {
-    // optionally add max_msg_per_second = 25 (default) here
-    try {
-      bt_->addGrootMonitoring(&tree_, groot_publisher_port_, groot_server_port_);
-      RCLCPP_INFO(
-        logger_, "Enabling Groot monitoring for %s: %d, %d",
-        action_name_.c_str(), groot_publisher_port_, groot_server_port_);
-    } catch (const std::logic_error & e) {
-      RCLCPP_ERROR(logger_, "ZMQ already enabled, Error: %s", e.what());
-    }
-  }
-
   return true;
 }
 

nav2_behavior_tree/src/behavior_tree_engine.cpp

@@ -83,27 +83,6 @@ BehaviorTreeEngine::createTreeFromFile(
   return factory_.createTreeFromFile(file_path, blackboard);
 }
 
-void
-BehaviorTreeEngine::addGrootMonitoring(
-  BT::Tree * tree,
-  uint16_t publisher_port,
-  uint16_t server_port,
-  uint16_t max_msg_per_second)
-{
-  // This logger publish status changes using ZeroMQ. Used by Groot
-  groot_monitor_ = std::make_unique<BT::PublisherZMQ>(
-    *tree, max_msg_per_second, publisher_port,
-    server_port);
-}
-
-void
-BehaviorTreeEngine::resetGrootMonitor()
-{
-  if (groot_monitor_) {
-    groot_monitor_.reset();
-  }
-}
-
 // In order to re-run a Behavior Tree, we must be able to reset all nodes to the initial state
 void
 BehaviorTreeEngine::haltAllActions(BT::TreeNode * root_node)

nav2_bringup/params/nav2_multirobot_params_1.yaml

@@ -59,12 +59,6 @@ bt_navigator:
     # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
     # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
     # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
-    # if enable_groot_monitoring is set to True, ports need to be different for each robot !!
-    enable_groot_monitoring: False
-    pose_groot_publisher_port: 1666
-    pose_groot_server_port: 1667
-    poses_groot_publisher_port: 1668
-    poses_groot_server_port: 1669
     plugin_lib_names:
     - nav2_compute_path_to_pose_action_bt_node
     - nav2_compute_path_through_poses_action_bt_node

nav2_bringup/params/nav2_multirobot_params_2.yaml

@@ -59,12 +59,6 @@ bt_navigator:
     # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
     # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
     # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
-    # if enable_groot_monitoring is set to True, ports need to be different for each robot !!
-    enable_groot_monitoring: False
-    pose_groot_publisher_port: 1666
-    pose_groot_server_port: 1667
-    poses_groot_publisher_port: 1668
-    poses_groot_server_port: 1669
     plugin_lib_names:
     - nav2_compute_path_to_pose_action_bt_node
     - nav2_compute_path_through_poses_action_bt_node

nav2_bringup/params/nav2_params.yaml

@@ -55,11 +55,6 @@ bt_navigator:
     odom_topic: /odom
     bt_loop_duration: 10
     default_server_timeout: 20
-    enable_groot_monitoring: True
-    pose_groot_publisher_port: 1666
-    pose_groot_server_port: 1667
-    poses_groot_publisher_port: 1668
-    poses_groot_server_port: 1669
     # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
     # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
     # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml

nav2_bt_navigator/src/bt_navigator.cpp

@@ -72,11 +72,6 @@ BtNavigator::BtNavigator(const rclcpp::NodeOptions & options)
   declare_parameter("global_frame", std::string("map"));
   declare_parameter("robot_base_frame", std::string("base_link"));
   declare_parameter("odom_topic", std::string("odom"));
-  declare_parameter("enable_groot_monitoring", false);
-  declare_parameter("pose_groot_publisher_port", 1666);
-  declare_parameter("pose_groot_server_port", 1667);
-  declare_parameter("poses_groot_publisher_port", 1668);
-  declare_parameter("poses_groot_server_port", 1669);
 }
 
 BtNavigator::~BtNavigator()
@@ -117,22 +112,12 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
     return nav2_util::CallbackReturn::FAILURE;
   }
 
-  pose_navigator_->getActionServer()->setGrootMonitoring(
-    get_parameter("enable_groot_monitoring").as_bool(),
-    get_parameter("pose_groot_publisher_port").as_int(),
-    get_parameter("pose_groot_server_port").as_int());
-
   if (!poses_navigator_->on_configure(
       shared_from_this(), plugin_lib_names, feedback_utils, &plugin_muxer_))
   {
     return nav2_util::CallbackReturn::FAILURE;
   }
 
-  poses_navigator_->getActionServer()->setGrootMonitoring(
-    get_parameter("enable_groot_monitoring").as_bool(),
-    get_parameter("poses_groot_publisher_port").as_int(),
-    get_parameter("poses_groot_server_port").as_int());
-
   // Odometry smoother object for getting current speed
   odom_smoother_ = std::make_unique<nav2_util::OdomSmoother>(shared_from_this(), 0.3);
 

nav2_system_tests/src/system/CMakeLists.txt

@@ -49,7 +49,7 @@ ament_add_test(test_bt_navigator_with_dijkstra
     PLANNER=nav2_navfn_planner/NavfnPlanner
 )
 
-ament_add_test(test_bt_navigator_with_groot_monitoring
+ament_add_test(test_bt_navigator_2
   GENERATE_RESULT_FOR_RETURN_CODE_ZERO
   COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py"
   WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
@@ -62,7 +62,6 @@ ament_add_test(test_bt_navigator_with_groot_monitoring
     BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
     TESTER=nav_to_pose_tester_node.py
     ASTAR=False
-    GROOT_MONITORING=True
     CONTROLLER=dwb_core::DWBLocalPlanner
     PLANNER=nav2_navfn_planner/NavfnPlanner
 )

nav2_system_tests/src/system/nav_to_pose_tester_node.py

@@ -16,7 +16,6 @@
 
 import argparse
 import math
-import os
 import sys
 import time
 
@@ -37,8 +36,6 @@
 from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
 from rclpy.qos import QoSProfile
 
-import zmq
-
 
 class NavTester(Node):
     def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ""):
@@ -96,13 +93,6 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None):
         while not self.action_client.wait_for_server(timeout_sec=1.0):
             self.info_msg("'NavigateToPose' action server not available, waiting...")
 
-        if os.getenv("GROOT_MONITORING") == "True":
-            if self.grootMonitoringGetStatus():
-                self.error_msg("Behavior Tree must not be running already!")
-                self.error_msg("Are you running multiple goals/bts..?")
-                return False
-            self.info_msg("This Error above MUST Fail and is o.k.!")
-
         self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose
         goal_msg = NavigateToPose.Goal()
         goal_msg.pose = self.getStampedPoseMsg(self.goal_pose)
@@ -121,17 +111,6 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None):
         get_result_future = goal_handle.get_result_async()
 
         future_return = True
-        if os.getenv("GROOT_MONITORING") == "True":
-            try:
-                if not self.grootMonitoringReloadTree():
-                    self.error_msg("Failed GROOT_BT - Reload Tree from ZMQ Server")
-                    future_return = False
-                if not self.grootMonitoringGetStatus():
-                    self.error_msg("Failed GROOT_BT - Get Status from ZMQ Publisher")
-                    future_return = False
-            except Exception as e:  # noqa: B902
-                self.error_msg(f"Failed GROOT_BT - ZMQ Tests: {e}")
-                future_return = False
 
         self.info_msg("Waiting for 'NavigateToPose' action to complete")
         rclpy.spin_until_future_complete(self, get_result_future)
@@ -146,75 +125,6 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None):
         self.info_msg("Goal succeeded!")
         return True
 
-    def grootMonitoringReloadTree(self):
-        # ZeroMQ Context
-        context = zmq.Context()
-
-        sock = context.socket(zmq.REQ)
-        port = 1667  # default server port for groot monitoring
-        # # Set a Timeout so we do not spin till infinity
-        sock.setsockopt(zmq.RCVTIMEO, 1000)
-        # sock.setsockopt(zmq.LINGER, 0)
-
-        sock.connect(f"tcp://localhost:{port}")
-        self.info_msg(f"ZMQ Server Port:{port}")
-
-        # this should fail
-        try:
-            sock.recv()
-            self.error_msg("ZMQ Reload Tree Test 1/3 - This should have failed!")
-            # Only works when ZMQ server receives a request first
-            sock.close()
-            return False
-        except zmq.error.ZMQError:
-            self.info_msg("ZMQ Reload Tree Test 1/3: Check")
-        try:
-            # request tree from server
-            sock.send_string("")
-            # receive tree from server as flat_buffer
-            sock.recv()
-            self.info_msg("ZMQ Reload Tree Test 2/3: Check")
-        except zmq.error.Again:
-            self.info_msg("ZMQ Reload Tree Test 2/3 - Failed to load tree")
-            sock.close()
-            return False
-
-        # this should fail
-        try:
-            sock.recv()
-            self.error_msg("ZMQ Reload Tree Test 3/3 - This should have failed!")
-            # Tree should only be loadable ONCE after ZMQ server received a request
-            sock.close()
-            return False
-        except zmq.error.ZMQError:
-            self.info_msg("ZMQ Reload Tree Test 3/3: Check")
-
-        return True
-
-    def grootMonitoringGetStatus(self):
-        # ZeroMQ Context
-        context = zmq.Context()
-        # Define the socket using the 'Context'
-        sock = context.socket(zmq.SUB)
-        # Set a Timeout so we do not spin till infinity
-        sock.setsockopt(zmq.RCVTIMEO, 2000)
-        # sock.setsockopt(zmq.LINGER, 0)
-
-        # Define subscription and messages with prefix to accept.
-        sock.setsockopt_string(zmq.SUBSCRIBE, "")
-        port = 1666  # default publishing port for groot monitoring
-        sock.connect(f"tcp://127.0.0.1:{port}")
-
-        for request in range(3):
-            try:
-                sock.recv()
-            except zmq.error.Again:
-                self.error_msg("ZMQ - Did not receive any status")
-                sock.close()
-                return False
-        self.info_msg("ZMQ - Did receive status")
-        return True
-
     def poseCallback(self, msg):
         self.info_msg("Received amcl_pose")
         self.current_pose = msg.pose.pose

nav2_system_tests/src/system/test_system_launch.py

@@ -50,9 +50,6 @@ def generate_launch_description():
     if (os.getenv('ASTAR') == 'True'):
         param_substitutions.update({'use_astar': 'True'})
 
-    if (os.getenv('GROOT_MONITORING') == 'True'):
-        param_substitutions.update({'enable_groot_monitoring': 'True'})
-
     param_substitutions.update(
         {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER')})
     param_substitutions.update(

nav2_system_tests/src/system/test_wrong_init_pose_launch.py

@@ -50,9 +50,6 @@ def generate_launch_description():
     if (os.getenv('ASTAR') == 'True'):
         param_substitutions.update({'use_astar': 'True'})
 
-    if (os.getenv('GROOT_MONITORING') == 'True'):
-        param_substitutions.update({'enable_groot_monitoring': 'True'})
-
     param_substitutions.update(
         {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER')})
     param_substitutions.update(