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..a633508d9b0 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,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_; 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 8a0faaa93b8..6f2c3cd06d1 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", 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 @@ -97,11 +88,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); @@ -156,6 +142,14 @@ 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) { @@ -193,7 +187,7 @@ 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_); } catch (const std::logic_error & e) { RCLCPP_ERROR(logger_, "ZMQ already enabled, Error: %s", e.what()); } diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index 97508a0173a..76a68497cdc 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/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 diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp index 68abf0830fc..64bb41fe014 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 086e09189b4..3fd74241c2d 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/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(shared_from_this(), 0.3);