From 199ef58f04bce9da4fcf8800630d171665349015 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Florian=20Gram=C3=9F?=
<6034322+gramss@users.noreply.github.com>
Date: Wed, 9 Dec 2020 03:53:17 +0100
Subject: [PATCH] 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 Monitoring - default true. Also cleanup ZMQ
* Move haltAllActions() Implementation from .hpp to .cpp
* update Copyright in hpp of BT-engine
* make linters happy.. :)
* Update Groot parameter naming and chg default=0
* rename resetZMQGrootMonitor -> resetGrootMonitor
* add parameter to nav2_params.yaml - default = false
* add ZMQ params and logic for server/pub ports
* Fix RewrittenYaml ignoring Integers
Integers where converted as floats before which crashes get_parameter.. fun thing....
* add launch based tests for params and ZMQ
* Activate Dijkstra and A* switching tests, thanks to RewrittenYaml
* add pyzmq==19.0.2 via pip3 to CI test_workspace
* make flake8 linter happy
* make cpp linters happy
* add personal copyright
* add GoalUpdated BT node description in order to view the full default BT
only affects editor mode of Groot and not live monitoring
* make linter happy (unused import)
* remove unused groot-port replacement functions in test_system_launch.py
* add groot parameters to params.md
* get reloading BTs to work nicely with Groot
* pretty space for smac :)
* switch from unsinged to uint16_t
* fix converting string into float or int
* Revert "add pyzmq==19.0.2 via pip3 to CI test_workspace"
This reverts commit 7bca08121c88db3763771911e3c6b4c6f4f8ddeb.
* Switch to 4 spaces indent and other linter stuff for RewrittenYaml
* removed prints in test_system_launch.py
* linter stuff
* add python-zmq as test_depend in package.xml (instead of .CI_conf)
* enable groot monitoring by default
* remove ZMQ from naming (function / variable)
* remove variable zmq ports from testing scripts
* remove default ports in BT_engine, as they are set through (def-)params
* Remove complete test for "dynamic" ZMQ ports testing
* fix python-zmq depend location
* fix style
* swap missing Groot to default True
* fix rosdep zmq + flake8 fixes in system_tests
* remove debug logs + c_str()
* remove final debug_log
---
doc/parameters/param_list.md | 3 +
.../behavior_tree_engine.hpp | 32 +--
nav2_behavior_tree/nav2_tree_nodes.xml | 2 +
.../src/behavior_tree_engine.cpp | 52 +++-
nav2_bringup/bringup/params/nav2_params.yaml | 3 +
nav2_bt_navigator/src/bt_navigator.cpp | 31 ++-
.../nav2_common/launch/rewritten_yaml.py | 234 +++++++++---------
nav2_system_tests/package.xml | 1 +
nav2_system_tests/src/system/CMakeLists.txt | 17 +-
.../src/system/test_system_launch.py | 19 +-
nav2_system_tests/src/system/tester_node.py | 97 ++++++++
11 files changed, 340 insertions(+), 151 deletions(-)
diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md
index f49d5375..5b67b12d 100644
--- a/doc/parameters/param_list.md
+++ b/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 |
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp
index ca627bbe..61816a99 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp
@@ -1,4 +1,5 @@
// 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
diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml
index 3a96da52..9a36ccdc 100644
--- a/nav2_behavior_tree/nav2_tree_nodes.xml
+++ b/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"/>
diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp
index 36b852e6..93841615 100644
--- a/nav2_behavior_tree/src/behavior_tree_engine.cpp
+++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp
@@ -1,4 +1,5 @@
// 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
diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml
index 4b1b8434..f10edb77 100644
--- a/nav2_bringup/bringup/params/nav2_params.yaml
+++ b/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
diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp
index 545cf616..9d224b0d 100644
--- a/nav2_bt_navigator/src/bt_navigator.cpp
+++ b/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;
}
diff --git a/nav2_common/nav2_common/launch/rewritten_yaml.py b/nav2_common/nav2_common/launch/rewritten_yaml.py
index 4d9b8637..462f204e 100644
--- a/nav2_common/nav2_common/launch/rewritten_yaml.py
+++ b/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
diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml
index b3ce62b1..51ed254b 100644
--- a/nav2_system_tests/package.xml
+++ b/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>
diff --git a/nav2_system_tests/src/system/CMakeLists.txt b/nav2_system_tests/src/system/CMakeLists.txt
index a7ae2aa4..3c38e2fb 100644
--- a/nav2_system_tests/src/system/CMakeLists.txt
+++ b/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"
diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py
index 92ea252e..c56d23c4 100755
--- a/nav2_system_tests/src/system/test_system_launch.py
+++ b/nav2_system_tests/src/system/test_system_launch.py
@@ -1,6 +1,7 @@
#!/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()),
])
diff --git a/nav2_system_tests/src/system/tester_node.py b/nav2_system_tests/src/system/tester_node.py
index 83e9df9d..d84724ea 100755
--- a/nav2_system_tests/src/system/tester_node.py
+++ b/nav2_system_tests/src/system/tester_node.py
@@ -1,5 +1,6 @@
#! /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
--
GitLab