adding groot separation by BT type (continuation on 2409) (#2627)

* adding groot separation by BT type

* fixing linting

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp

@@ -86,6 +86,17 @@ 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
@@ -232,9 +243,9 @@ class BtActionServer
   std::chrono::milliseconds default_server_timeout_;
 
   // Parameters for Groot monitoring
-  bool enable_groot_monitoring_;
-  int groot_zmq_publisher_port_;
-  int groot_zmq_server_port_;
+  bool enable_groot_monitoring_ = true;
+  int groot_publisher_port_ = 1666;
+  int groot_server_port_ = 1667;
 
   // User-provided callbacks
   OnGoalReceivedCallback on_goal_received_callback_;

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -59,15 +59,6 @@ BtActionServer<ActionT>::BtActionServer(
   if (!node->has_parameter("default_server_timeout")) {
     node->declare_parameter("default_server_timeout", 20);
   }
-  if (!node->has_parameter("enable_groot_monitoring")) {
-    node->declare_parameter("enable_groot_monitoring", false);
-  }
-  if (!node->has_parameter("groot_zmq_publisher_port")) {
-    node->declare_parameter("groot_zmq_publisher_port", 1666);
-  }
-  if (!node->has_parameter("groot_zmq_server_port")) {
-    node->declare_parameter("groot_zmq_server_port", 1667);
-  }
 }
 
 template<class ActionT>
@@ -102,11 +93,6 @@ bool BtActionServer<ActionT>::on_configure()
     node->get_node_waitables_interface(),
     action_name_, std::bind(&BtActionServer<ActionT>::executeCallback, this));
 
-  // Get parameter for monitoring with Groot via ZMQ Publisher
-  node->get_parameter("enable_groot_monitoring", enable_groot_monitoring_);
-  node->get_parameter("groot_zmq_publisher_port", groot_zmq_publisher_port_);
-  node->get_parameter("groot_zmq_server_port", groot_zmq_server_port_);
-
   // Get parameters for BT timeouts
   int timeout;
   node->get_parameter("bt_loop_duration", timeout);
@@ -161,6 +147,17 @@ bool BtActionServer<ActionT>::on_cleanup()
   return true;
 }
 
+template<class ActionT>
+void BtActionServer<ActionT>::setGrootMonitoring(
+  const bool enable,
+  const unsigned publisher_port,
+  const unsigned server_port)
+{
+  enable_groot_monitoring_ = enable;
+  groot_publisher_port_ = publisher_port;
+  groot_server_port_ = server_port;
+}
+
 template<class ActionT>
 bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filename)
 {
@@ -198,7 +195,10 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
   if (enable_groot_monitoring_) {
     // optionally add max_msg_per_second = 25 (default) here
     try {
-      bt_->addGrootMonitoring(&tree_, groot_zmq_publisher_port_, groot_zmq_server_port_);
+      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());
     }

nav2_bringup/params/nav2_multirobot_params_1.yaml

@@ -61,8 +61,10 @@ bt_navigator:
     # 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
-    groot_zmq_publisher_port: 1666
-    groot_zmq_server_port: 1667
+    pose_groot_publisher_port: 1666
+    pose_groot_server_port: 1667
+    poses_groot_publisher_port: 1668
+    poses_groot_server_port: 1669
     plugin_lib_names:
     - nav2_compute_path_to_pose_action_bt_node
     - nav2_compute_path_through_poses_action_bt_node

nav2_bringup/params/nav2_multirobot_params_2.yaml

@@ -61,8 +61,10 @@ bt_navigator:
     # 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
-    groot_zmq_publisher_port: 1789
-    groot_zmq_server_port: 1887
+    pose_groot_publisher_port: 1666
+    pose_groot_server_port: 1667
+    poses_groot_publisher_port: 1668
+    poses_groot_server_port: 1669
     plugin_lib_names:
     - nav2_compute_path_to_pose_action_bt_node
     - nav2_compute_path_through_poses_action_bt_node

nav2_bringup/params/nav2_params.yaml

@@ -56,8 +56,10 @@ bt_navigator:
     bt_loop_duration: 10
     default_server_timeout: 20
     enable_groot_monitoring: True
-    groot_zmq_publisher_port: 1666
-    groot_zmq_server_port: 1667
+    pose_groot_publisher_port: 1666
+    pose_groot_server_port: 1667
+    poses_groot_publisher_port: 1668
+    poses_groot_server_port: 1669
     # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
     # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
     # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml

nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp

@@ -229,6 +229,15 @@ class Navigator
 
   virtual std::string getDefaultBTFilepath(rclcpp_lifecycle::LifecycleNode::WeakPtr node) = 0;
 
+  /**
+   * @brief Get the action server
+   * @return Action server pointer
+   */
+  std::unique_ptr<nav2_behavior_tree::BtActionServer<ActionT>> & getActionServer()
+  {
+    return bt_action_server_;
+  }
+
 protected:
   /**
    * @brief An intermediate goal reception function to mux navigators.

nav2_bt_navigator/src/bt_navigator.cpp

@@ -72,6 +72,11 @@ 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()
@@ -112,12 +117,22 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
     return nav2_util::CallbackReturn::FAILURE;
   }
 
+  pose_navigator_->getActionServer()->setGrootMonitoring(
+    get_parameter("enable_groot_monitoring").as_bool(),
+    get_parameter("pose_groot_publisher_port").as_int(),
+    get_parameter("pose_groot_server_port").as_int());
+
   if (!poses_navigator_->on_configure(
       shared_from_this(), plugin_lib_names, feedback_utils, &plugin_muxer_))
   {
     return nav2_util::CallbackReturn::FAILURE;
   }
 
+  poses_navigator_->getActionServer()->setGrootMonitoring(
+    get_parameter("enable_groot_monitoring").as_bool(),
+    get_parameter("poses_groot_publisher_port").as_int(),
+    get_parameter("poses_groot_server_port").as_int());
+
   // Odometry smoother object for getting current speed
   odom_smoother_ = std::make_unique<nav2_util::OdomSmoother>(shared_from_this(), 0.3);