diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md
index f49d537576d641f6c32e7304c9d7d2fdf7ffad7d..5b67b12d0d39e5dac885729aadb920017faf18f3 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 ca627bbe7e420786cdb6634f7f5405aa116d2cb3..61816a995048d556ded6be7b278a52b3cbfd3f13 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 3a96da5269fdd6b34a0aa50acdbf24643e8d82dc..9a36ccdc6e02f45217b9fc77f5ff90ed1d366394 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 36b852e61c9cd99f7a52ed3fa0957c5611143918..93841615d9ec75d8d5a73cbfef851f067edfa764 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 4b1b8434562ec0465445cba05257b78cfaff797a..f10edb772923cc72c9c4db5efd9e481f229d6273 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 545cf6164799c73ea20505a6f3d5f329ae8c94d6..9d224b0d5a5b085187b45d0c7261b825f3a901b0 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 4d9b86373f44c083988e66ca45679413ec3cf21a..462f204e8dff9f17864cf045abceac868f9c65d1 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 b3ce62b13afc58c49d2ef0bda6fb398d3197d21a..51ed254b9c479f0f0ce99f25a4d69f9528b90658 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 a7ae2aa43fa633b3306ea8e66ab2360e6f4f6a37..3c38e2fb42861aa338c075917b364657e1589d94 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 92ea252eba04ffbbf29b0a7c2fe0ce444891a745..c56d23c41c72859fd92e030b076be6c8d61502b5 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 83e9df9dc9f0110563d513967b9c0ba5d60a2dcc..d84724ea5c843f56ef3a659102d181e0e274c2f9 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