Set groot monitoring parameters individually for bt_navigator's pose …

…and poses navigators

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp

@@ -86,6 +86,14 @@ 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
@@ -233,8 +241,8 @@ class BtActionServer
 
   // Parameters for Groot monitoring
   bool enable_groot_monitoring_;
-  int groot_zmq_publisher_port_;
-  int groot_zmq_server_port_;
+  int groot_publisher_port_;
+  int groot_server_port_;
 
   // 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", true);
-  }
-  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>
@@ -97,11 +88,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);
@@ -156,6 +142,14 @@ 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)
 {
@@ -193,7 +187,7 @@ 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_);
     } catch (const std::logic_error & e) {
       RCLCPP_ERROR(logger_, "ZMQ already enabled, Error: %s", e.what());
     }

nav2_bringup/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
+    nt_pose_groot_publisher_port: 1666
+    nt_pose_groot_server_port: 1667
+    nt_poses_groot_publisher_port: 1668
+    nt_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
@@ -169,7 +171,7 @@ controller_server:
       RotateToGoal.scale: 32.0
       RotateToGoal.slowing_factor: 5.0
       RotateToGoal.lookahead_time: -1.0
-      
+
 controller_server_rclcpp_node:
   ros__parameters:
     use_sim_time: True
@@ -320,7 +322,7 @@ waypoint_follower:
   ros__parameters:
     loop_rate: 20
     stop_on_failure: false
-    waypoint_task_executor_plugin: "wait_at_waypoint"   
+    waypoint_task_executor_plugin: "wait_at_waypoint"
     wait_at_waypoint:
       plugin: "nav2_waypoint_follower::WaitAtWaypoint"
       enabled: True

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()
   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("nt_pose_groot_publisher_port", 1666);
+  declare_parameter("nt_pose_groot_server_port", 1667);
+  declare_parameter("nt_poses_groot_publisher_port", 1668);
+  declare_parameter("nt_poses_groot_server_port", 1669);
 }
 
 BtNavigator::~BtNavigator()
@@ -111,12 +116,20 @@ 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("nt_pose_groot_publisher_port").as_int(),
+      get_parameter("nt_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("nt_poses_groot_publisher_port").as_int(),
+      get_parameter("nt_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);