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 ec977944418..e913555a998 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,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_; 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 fa8fec27ba9..0289c2f822d 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 @@ -59,15 +59,6 @@ BtActionServer::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 @@ -102,11 +93,6 @@ bool BtActionServer::on_configure() node->get_node_waitables_interface(), action_name_, std::bind(&BtActionServer::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::on_cleanup() 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) { @@ -198,7 +195,10 @@ bool BtActionServer::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()); } diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index 8620f2e6389..36904507623 100644 --- a/nav2_bringup/params/nav2_multirobot_params_1.yaml +++ b/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 diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml index 498fb36eda6..9a94226c887 100644 --- a/nav2_bringup/params/nav2_multirobot_params_2.yaml +++ b/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 diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 97508a0173a..31255ada9fd 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/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 diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp index 68abf0830fc..82c31deaeb6 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp +++ b/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> & getActionServer() + { + return bt_action_server_; + } + protected: /** * @brief An intermediate goal reception function to mux navigators. diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 7f62c05e12b..0f404122800 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/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(shared_from_this(), 0.3);