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 f5aab180c59..6f0c9bfb914 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 @@ -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 groot_monitor_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index e913555a998..e423afcb458 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/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_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 0289c2f822d..dfce6d284a8 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -142,22 +142,10 @@ bool BtActionServer::on_cleanup() current_bt_xml_filename_.clear(); blackboard_.reset(); bt_->haltAllActions(tree_.rootNode()); - bt_->resetGrootMonitor(); bt_.reset(); return true; } -template -void BtActionServer::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 bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filename) { @@ -170,9 +158,6 @@ bool BtActionServer::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::loadBehaviorTree(const std::string & bt_xml_filena topic_logger_ = std::make_unique(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; } diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index e689cae927a..ed55b085349 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/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( - *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) diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index 2154779bf72..164773c7db5 100644 --- a/nav2_bringup/params/nav2_multirobot_params_1.yaml +++ b/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 diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml index 138598df29b..ed07eb0c008 100644 --- a/nav2_bringup/params/nav2_multirobot_params_2.yaml +++ b/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 diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index c89d868453b..828ca4efdf2 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/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 diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 0f404122800..7f62c05e12b 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/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(shared_from_this(), 0.3); diff --git a/nav2_system_tests/src/system/CMakeLists.txt b/nav2_system_tests/src/system/CMakeLists.txt index df3f6fe8618..fdd20a8d2d9 100644 --- a/nav2_system_tests/src/system/CMakeLists.txt +++ b/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 ) diff --git a/nav2_system_tests/src/system/nav_to_pose_tester_node.py b/nav2_system_tests/src/system/nav_to_pose_tester_node.py index efc6dffa5eb..6fb119e980e 100755 --- a/nav2_system_tests/src/system/nav_to_pose_tester_node.py +++ b/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 diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index 84a295a1fb1..6b067deea4e 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/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( diff --git a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py index b45031cec71..4db5de783f2 100755 --- a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py +++ b/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(