Add groot monitoring behavior tree visualization (#1958)

* include ZMQ publisher for Groot

very plain integration, should be made optionally through a launch parameter

* fix Groot crashing finding custom nodes in monitor mode

straight forward working fix. The manifest was missing, so Groot searched custom node IDs that it did not have. This is implemented correctly directly in BT.CPP V3 and should be used instead of an implementation in nav2_bt_engine

* refactor buildTreeFromText to createTreeFromText as in BT.CPP v3

* forward XML to createTreeFromText from BT.CPP v3 factory function

* Add createTreeFromFile forware to BT-factory function

* fix createTreeFromFile args..

* add personal copyright

I think this is okay for finding a nasty bug.. :)

* move creating ZMQ Publisher from run to dedicated function

this way the ZMQ Publisher ca be added to individual trees within the same factory. Should be important for switching trees (XML files)

* Add parameter for Groot Monitor...

doc/parameters/param_list.md

@@ -5,6 +5,9 @@
 | Parameter | Default | Description |
 | ----------| --------| ------------|
 | default_bt_xml_filename | N/A | path to the default behavior tree XML description |
+| enable_groot_monitoring | True | enable Groot live monitoring of the behavior tree |
+| groot_zmq_publisher_port | 1666 | change port of the zmq publisher needed for groot |
+| groot_zmq_server_port | 1667 | change port of the zmq server needed for groot |
 | plugin_lib_names | ["nav2_compute_path_to_pose_action_bt_node", "nav2_follow_path_action_bt_node", "nav2_back_up_action_bt_node", "nav2_spin_action_bt_node", "nav2_wait_action_bt_node", "nav2_clear_costmap_service_bt_node", "nav2_is_stuck_condition_bt_node", "nav2_goal_reached_condition_bt_node", "nav2_initial_pose_received_condition_bt_node", "nav2_goal_updated_condition_bt_node", "nav2_reinitialize_global_localization_service_bt_node", "nav2_rate_controller_bt_node", "nav2_distance_controller_bt_node", "nav2_recovery_node_bt_node", "nav2_pipeline_sequence_bt_node", "nav2_round_robin_node_bt_node", "nav2_transform_available_condition_bt_node"] | list of behavior tree node shared libraries |
 | transform_tolerance | 0.1 | TF transform tolerance |
 | global_frame | "map" | Reference frame |

nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp

 // Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Florian Gramss
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -22,6 +23,8 @@
 #include "behaviortree_cpp_v3/behavior_tree.h"
 #include "behaviortree_cpp_v3/bt_factory.h"
 #include "behaviortree_cpp_v3/xml_parsing.h"
+#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h"
+
 
 namespace nav2_behavior_tree
 {
@@ -40,28 +43,29 @@ public:
     std::function<bool()> cancelRequested,
     std::chrono::milliseconds loopTimeout = std::chrono::milliseconds(10));
 
-  BT::Tree buildTreeFromText(
+  BT::Tree createTreeFromText(
     const std::string & xml_string,
     BT::Blackboard::Ptr blackboard);
 
+  BT::Tree createTreeFromFile(
+    const std::string & file_path,
+    BT::Blackboard::Ptr blackboard);
+
+  void addGrootMonitoring(
+    BT::Tree * tree,
+    uint16_t publisher_port,
+    uint16_t server_port,
+    uint16_t max_msg_per_second = 25);
+
+  void resetGrootMonitor();
+
   // In order to re-run a Behavior Tree, we must be able to reset all nodes to the initial state
-  void haltAllActions(BT::TreeNode * root_node)
-  {
-    // this halt signal should propagate through the entire tree.
-    root_node->halt();
-
-    // but, just in case...
-    auto visitor = [](BT::TreeNode * node) {
-        if (node->status() == BT::NodeStatus::RUNNING) {
-          node->halt();
-        }
-      };
-    BT::applyRecursiveVisitor(root_node, visitor);
-  }
+  void haltAllActions(BT::TreeNode * root_node);
 
 protected:
   // The factory that will be used to dynamically construct the behavior tree
   BT::BehaviorTreeFactory factory_;
+  std::unique_ptr<BT::PublisherZMQ> groot_monitor_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -53,6 +53,8 @@
       <input_port name="parent">Parent frame for transform</input_port>
     </Condition>
 
+    <Condition ID="GoalUpdated"/>
+
     <!-- ############################### CONTROL NODES ################################ -->
     <Control ID="PipelineSequence"/>
 

nav2_behavior_tree/src/behavior_tree_engine.cpp

 // Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Florian Gramss
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -21,8 +22,6 @@
 #include "rclcpp/rclcpp.hpp"
 #include "behaviortree_cpp_v3/utils/shared_library.h"
 
-using namespace std::chrono_literals;
-
 namespace nav2_behavior_tree
 {
 
@@ -62,13 +61,54 @@ BehaviorTreeEngine::run(
 }
 
 BT::Tree
-BehaviorTreeEngine::buildTreeFromText(
+BehaviorTreeEngine::createTreeFromText(
   const std::string & xml_string,
   BT::Blackboard::Ptr blackboard)
 {
-  BT::XMLParser p(factory_);
-  p.loadFromText(xml_string);
-  return p.instantiateTree(blackboard);
+  return factory_.createTreeFromText(xml_string, blackboard);
+}
+
+BT::Tree
+BehaviorTreeEngine::createTreeFromFile(
+  const std::string & file_path,
+  BT::Blackboard::Ptr blackboard)
+{
+  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()
+{
+  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)
+{
+  // this halt signal should propagate through the entire tree.
+  root_node->halt();
+
+  // but, just in case...
+  auto visitor = [](BT::TreeNode * node) {
+      if (node->status() == BT::NodeStatus::RUNNING) {
+        node->halt();
+      }
+    };
+  BT::applyRecursiveVisitor(root_node, visitor);
 }
 
 }  // namespace nav2_behavior_tree

nav2_bringup/bringup/params/nav2_params.yaml

@@ -53,6 +53,9 @@ bt_navigator:
     global_frame: map
     robot_base_frame: base_link
     odom_topic: /odom
+    enable_groot_monitoring: True
+    groot_zmq_publisher_port: 1666
+    groot_zmq_server_port: 1667
     default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
     plugin_lib_names:
     - nav2_compute_path_to_pose_action_bt_node

nav2_bt_navigator/src/bt_navigator.cpp

@@ -20,6 +20,7 @@
 #include <utility>
 #include <vector>
 #include <set>
+#include <exception>
 
 #include "nav2_util/geometry_utils.hpp"
 #include "nav2_util/robot_utils.hpp"
@@ -67,6 +68,9 @@ BtNavigator::BtNavigator()
   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", true);
+  declare_parameter("groot_zmq_publisher_port", 1666);
+  declare_parameter("groot_zmq_server_port", 1667);
 }
 
 BtNavigator::~BtNavigator()
@@ -138,9 +142,13 @@ BtNavigator::loadBehaviorTree(const std::string & bt_xml_filename)
 {
   // Use previous BT if it is the existing one
   if (current_bt_xml_filename_ == bt_xml_filename) {
+    RCLCPP_DEBUG(get_logger(), "BT will not be reloaded as the given xml is already loaded");
     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(bt_xml_filename);
 
@@ -153,13 +161,21 @@ BtNavigator::loadBehaviorTree(const std::string & bt_xml_filename)
     std::istreambuf_iterator<char>(xml_file),
     std::istreambuf_iterator<char>());
 
-  RCLCPP_DEBUG(get_logger(), "Behavior Tree file: '%s'", bt_xml_filename.c_str());
-  RCLCPP_DEBUG(get_logger(), "Behavior Tree XML: %s", xml_string.c_str());
-
   // Create the Behavior Tree from the XML input
-  tree_ = bt_->buildTreeFromText(xml_string, blackboard_);
+  tree_ = bt_->createTreeFromText(xml_string, blackboard_);
   current_bt_xml_filename_ = bt_xml_filename;
 
+  // get parameter for monitoring with Groot via ZMQ Publisher
+  if (get_parameter("enable_groot_monitoring").as_bool()) {
+    uint16_t zmq_publisher_port = get_parameter("groot_zmq_publisher_port").as_int();
+    uint16_t zmq_server_port = get_parameter("groot_zmq_server_port").as_int();
+    // optionally add max_msg_per_second = 25 (default) here
+    try {
+      bt_->addGrootMonitoring(&tree_, zmq_publisher_port, zmq_server_port);
+    } catch (const std::logic_error & e) {
+      RCLCPP_ERROR(get_logger(), "ZMQ already enabled, Error: %s", e.what());
+    }
+  }
   return true;
 }
 
@@ -214,6 +230,7 @@ BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
   current_bt_xml_filename_.clear();
   blackboard_.reset();
   bt_->haltAllActions(tree_.rootNode());
+  bt_->resetGrootMonitor();
   bt_.reset();
 
   RCLCPP_INFO(get_logger(), "Completed Cleaning up");
@@ -246,15 +263,15 @@ BtNavigator::navigateToPose()
       return action_server_->is_cancel_requested();
     };
 
-  auto bt_xml_filename = action_server_->get_current_goal()->behavior_tree;
+  std::string bt_xml_filename = action_server_->get_current_goal()->behavior_tree;
 
   // Empty id in request is default for backward compatibility
   bt_xml_filename = bt_xml_filename.empty() ? default_bt_xml_filename_ : bt_xml_filename;
 
   if (!loadBehaviorTree(bt_xml_filename)) {
     RCLCPP_ERROR(
-      get_logger(), "BT file not found: %s. Navigation canceled",
-      bt_xml_filename.c_str(), current_bt_xml_filename_.c_str());
+      get_logger(), "BT file not found: %s. Navigation canceled.",
+      bt_xml_filename.c_str());
     action_server_->terminate_current();
     return;
   }

nav2_common/nav2_common/launch/rewritten_yaml.py

@@ -21,131 +21,127 @@ import yaml
 import tempfile
 import launch
 
+
 class DictItemReference:
-  def __init__(self, dictionary, key):
-    self.dictionary = dictionary
-    self.dictKey = key
+    def __init__(self, dictionary, key):
+        self.dictionary = dictionary
+        self.dictKey = key
+
+    def key(self):
+        return self.dictKey
 
-  def key(self):
-      return self.dictKey
+    def setValue(self, value):
+        self.dictionary[self.dictKey] = value
 
-  def setValue(self, value):
-      self.dictionary[self.dictKey] = value
 
 class RewrittenYaml(launch.Substitution):
-  """
-  Substitution that modifies the given YAML file.
-
-  Used in launch system
-  """
-
-  def __init__(self,
-    source_file: launch.SomeSubstitutionsType,
-    param_rewrites: Dict,
-    root_key: Optional[launch.SomeSubstitutionsType] = None,
-    key_rewrites: Optional[Dict] = None,
-    convert_types = False) -> None:
-    super().__init__()
     """
-    Construct the substitution
+    Substitution that modifies the given YAML file.
 
-    :param: source_file the original YAML file to modify
-    :param: param_rewrites mappings to replace
-    :param: root_key if provided, the contents are placed under this key
-    :param: key_rewrites keys of mappings to replace
-    :param: convert_types whether to attempt converting the string to a number or boolean
+    Used in launch system
     """
 
-    from launch.utilities import normalize_to_list_of_substitutions  # import here to avoid loop
-    self.__source_file = normalize_to_list_of_substitutions(source_file)
-    self.__param_rewrites = {}
-    self.__key_rewrites = {}
-    self.__convert_types = convert_types
-    self.__root_key = None
-    for key in param_rewrites:
-        self.__param_rewrites[key] = normalize_to_list_of_substitutions(param_rewrites[key])
-    if key_rewrites is not None:
-        for key in key_rewrites:
-          self.__key_rewrites[key] = normalize_to_list_of_substitutions(key_rewrites[key])
-    if root_key is not None:
-        self.__root_key = normalize_to_list_of_substitutions(root_key)
-
-  @property
-  def name(self) -> List[launch.Substitution]:
-    """Getter for name."""
-    return self.__source_file
-
-  def describe(self) -> Text:
-    """Return a description of this substitution as a string."""
-    return ''
-
-  def perform(self, context: launch.LaunchContext) -> Text:
-    yaml_filename = launch.utilities.perform_substitutions(context, self.name)
-    rewritten_yaml = tempfile.NamedTemporaryFile(mode='w', delete=False)
-    param_rewrites, keys_rewrites = self.resolve_rewrites(context)
-    data = yaml.safe_load(open(yaml_filename, 'r'))
-    self.substitute_params(data, param_rewrites)
-    self.substitute_keys(data, keys_rewrites)
-    if self.__root_key is not None:
-        root_key = launch.utilities.perform_substitutions(context, self.__root_key)
-        if root_key:
-            data = {root_key: data}
-    yaml.dump(data, rewritten_yaml)
-    rewritten_yaml.close()
-    return rewritten_yaml.name
-
-  def resolve_rewrites(self, context):
-    resolved_params = {}
-    for key in self.__param_rewrites:
-      resolved_params[key] = launch.utilities.perform_substitutions(context, self.__param_rewrites[key])
-    resolved_keys = {}
-    for key in self.__key_rewrites:
-      resolved_keys[key] = launch.utilities.perform_substitutions(context, self.__key_rewrites[key])
-    return resolved_params, resolved_keys
-
-  def substitute_params(self, yaml, param_rewrites):
-    for key in self.getYamlLeafKeys(yaml):
-      if key.key() in param_rewrites:
-        raw_value = param_rewrites[key.key()]
-        key.setValue(self.convert(raw_value))
-
-  def substitute_keys(self, yaml, key_rewrites):
-    if len(key_rewrites) != 0:
-      for key, val in yaml.items():
-        if isinstance(val, dict) and key in key_rewrites:
-          new_key = key_rewrites[key]
-          yaml[new_key] = yaml[key]
-          del yaml[key]
-          self.substitute_keys(val, key_rewrites)
-
-  def getYamlLeafKeys(self, yamlData):
-    try:
-      for key in yamlData.keys():
-        for k in self.getYamlLeafKeys(yamlData[key]):
-          yield k
-        yield DictItemReference(yamlData, key)
-    except AttributeError:
-      return
-
-  def convert(self, text_value):
-    if self.__convert_types:
-      # try converting to float
-      try:
-        return float(text_value)
-      except ValueError:
-        pass
-
-      # try converting to int
-      try:
-        return int(text_value)
-      except ValueError:
-        pass
-
-      # try converting to bool
-      if text_value.lower() == "true":
-        return True
-      if text_value.lower() == "false":
-        return False
-
-      #nothing else worked so fall through and return text
-    return text_value
+    def __init__(self,
+        source_file: launch.SomeSubstitutionsType,
+        param_rewrites: Dict,
+        root_key: Optional[launch.SomeSubstitutionsType] = None,
+        key_rewrites: Optional[Dict] = None,
+        convert_types = False) -> None:
+        super().__init__()
+        """
+        Construct the substitution
+
+        :param: source_file the original YAML file to modify
+        :param: param_rewrites mappings to replace
+        :param: root_key if provided, the contents are placed under this key
+        :param: key_rewrites keys of mappings to replace
+        :param: convert_types whether to attempt converting the string to a number or boolean
+        """
+
+        from launch.utilities import normalize_to_list_of_substitutions  # import here to avoid loop
+        self.__source_file = normalize_to_list_of_substitutions(source_file)
+        self.__param_rewrites = {}
+        self.__key_rewrites = {}
+        self.__convert_types = convert_types
+        self.__root_key = None
+        for key in param_rewrites:
+            self.__param_rewrites[key] = normalize_to_list_of_substitutions(param_rewrites[key])
+        if key_rewrites is not None:
+            for key in key_rewrites:
+                self.__key_rewrites[key] = normalize_to_list_of_substitutions(key_rewrites[key])
+        if root_key is not None:
+            self.__root_key = normalize_to_list_of_substitutions(root_key)
+
+    @property
+    def name(self) -> List[launch.Substitution]:
+        """Getter for name."""
+        return self.__source_file
+
+    def describe(self) -> Text:
+        """Return a description of this substitution as a string."""
+        return ''
+
+    def perform(self, context: launch.LaunchContext) -> Text:
+        yaml_filename = launch.utilities.perform_substitutions(context, self.name)
+        rewritten_yaml = tempfile.NamedTemporaryFile(mode='w', delete=False)
+        param_rewrites, keys_rewrites = self.resolve_rewrites(context)
+        data = yaml.safe_load(open(yaml_filename, 'r'))
+        self.substitute_params(data, param_rewrites)
+        self.substitute_keys(data, keys_rewrites)
+        if self.__root_key is not None:
+            root_key = launch.utilities.perform_substitutions(context, self.__root_key)
+            if root_key:
+                data = {root_key: data}
+        yaml.dump(data, rewritten_yaml)
+        rewritten_yaml.close()
+        return rewritten_yaml.name
+
+    def resolve_rewrites(self, context):
+        resolved_params = {}
+        for key in self.__param_rewrites:
+            resolved_params[key] = launch.utilities.perform_substitutions(context, self.__param_rewrites[key])
+        resolved_keys = {}
+        for key in self.__key_rewrites:
+            resolved_keys[key] = launch.utilities.perform_substitutions(context, self.__key_rewrites[key])
+        return resolved_params, resolved_keys
+
+    def substitute_params(self, yaml, param_rewrites):
+        for key in self.getYamlLeafKeys(yaml):
+            if key.key() in param_rewrites:
+                raw_value = param_rewrites[key.key()]
+                key.setValue(self.convert(raw_value))
+
+    def substitute_keys(self, yaml, key_rewrites):
+        if len(key_rewrites) != 0:
+            for key, val in yaml.items():
+                if isinstance(val, dict) and key in key_rewrites:
+                    new_key = key_rewrites[key]
+                    yaml[new_key] = yaml[key]
+                    del yaml[key]
+                    self.substitute_keys(val, key_rewrites)
+
+    def getYamlLeafKeys(self, yamlData):
+        try:
+            for key in yamlData.keys():
+                for k in self.getYamlLeafKeys(yamlData[key]):
+                    yield k
+                yield DictItemReference(yamlData, key)
+        except AttributeError:
+            return
+
+    def convert(self, text_value):
+        if self.__convert_types:
+            # try converting to int or float
+            try:
+                return float(text_value) if '.' in text_value else int(text_value)
+            except ValueError:
+                pass
+
+        # try converting to bool
+        if text_value.lower() == "true":
+            return True
+        if text_value.lower() == "false":
+            return False
+
+        # nothing else worked so fall through and return text
+        return text_value

nav2_system_tests/package.xml

@@ -59,6 +59,7 @@
   <test_depend>launch</test_depend>
   <test_depend>launch_ros</test_depend>
   <test_depend>launch_testing</test_depend>
+  <test_depend>python3-zmq</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

nav2_system_tests/src/system/CMakeLists.txt

@@ -12,7 +12,7 @@ ament_add_test(test_bt_navigator
     TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world
     GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
     BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml
-    ASTAR=False
+    ASTAR=True
 )
 
 ament_add_test(test_bt_navigator_with_dijkstra
@@ -29,6 +29,21 @@ ament_add_test(test_bt_navigator_with_dijkstra
     ASTAR=False
 )
 
+ament_add_test(test_bt_navigator_with_groot_monitoring
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  TIMEOUT 180
+  ENV
+    TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
+    TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
+    TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world
+    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
+    BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml
+    ASTAR=False
+    GROOT_MONITORING=True
+)
+
 ament_add_test(test_dynamic_obstacle
   GENERATE_RESULT_FOR_RETURN_CODE_ZERO
   COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py"

nav2_system_tests/src/system/test_system_launch.py

 #!/usr/bin/env python3
 
 # Copyright (c) 2018 Intel Corporation
+# Copyright (c) 2020 Florian Gramss
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
@@ -22,6 +23,7 @@ from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch import LaunchService
 from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable
+from launch.launch_context import LaunchContext
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
 from launch_testing.legacy import LaunchTestService
@@ -40,15 +42,24 @@ def generate_launch_description():
     bringup_dir = get_package_share_directory('nav2_bringup')
     params_file = os.path.join(bringup_dir, 'params', 'nav2_params.yaml')
 
-    # Replace the `use_astar` setting on the params file
-    param_substitutions = {
-        'planner_server.ros__parameters.GridBased.use_astar': os.getenv('ASTAR')}
+    # Replace the default parameter values for testing special features
+    # without having multiple params_files inside the nav2 stack
+    context = LaunchContext()
+    param_substitutions = {}
+
+    if (os.getenv('ASTAR') == 'True'):
+        param_substitutions.update({'use_astar': 'True'})
+
+    if (os.getenv('GROOT_MONITORING') == 'True'):
+        param_substitutions.update({'enable_groot_monitoring': 'True'})
+
     configured_params = RewrittenYaml(
         source_file=params_file,
         root_key='',
         param_rewrites=param_substitutions,
         convert_types=True)
 
+    new_yaml = configured_params.perform(context)
     return LaunchDescription([
         SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'),
 
@@ -79,7 +90,7 @@ def generate_launch_description():
                               'use_namespace': 'False',
                               'map': map_yaml_file,
                               'use_sim_time': 'True',
-                              'params_file': configured_params,
+                              'params_file': new_yaml,
                               'bt_xml_file': bt_navigator_xml,
                               'autostart': 'True'}.items()),
     ])

nav2_system_tests/src/system/tester_node.py

 #! /usr/bin/env python3
 # Copyright 2018 Intel Corporation.
+# Copyright 2020 Florian Gramss
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
@@ -15,8 +16,10 @@
 
 import argparse
 import math
+import os
 import sys
 import time
+
 from typing import Optional
 
 from action_msgs.msg import GoalStatus
@@ -34,6 +37,8 @@ from rclpy.node import Node
 from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
 from rclpy.qos import QoSProfile
 
+import zmq
+
 
 class NavTester(Node):
 
@@ -94,6 +99,13 @@ class NavTester(Node):
         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)
@@ -111,6 +123,19 @@ class NavTester(Node):
         self.info_msg('Goal accepted')
         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:
+                self.error_msg('Failed GROOT_BT - ZMQ Tests: ' + e.__doc__ + e.message)
+                future_return = False
+
         self.info_msg("Waiting for 'NavigateToPose' action to complete")
         rclpy.spin_until_future_complete(self, get_result_future)
         status = get_result_future.result().status
@@ -118,9 +143,81 @@ class NavTester(Node):
             self.info_msg('Goal failed with status code: {0}'.format(status))
             return False
 
+        if not future_return:
+            return False
+
         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('tcp://localhost:' + str(port))
+        self.info_msg('ZMQ Server Port: ' + str(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('tcp://127.0.0.1:' + str(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