From 0e617b3e848e26f620c609c35b8e247df8550911 Mon Sep 17 00:00:00 2001 From: Jonatan Olofsson Date: Wed, 16 Jun 2021 01:15:46 +0200 Subject: [PATCH] Set groot monitoring parameters individually for bt_navigator's pose and poses navigators --- .../nav2_behavior_tree/bt_action_server.hpp | 14 +++++++--- .../bt_action_server_impl.hpp | 26 ++++++++----------- nav2_bringup/bringup/params/nav2_params.yaml | 10 ++++--- .../include/nav2_bt_navigator/navigator.hpp | 9 +++++++ nav2_bt_navigator/src/bt_navigator.cpp | 13 ++++++++++ 5 files changed, 50 insertions(+), 22 deletions(-) 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 ec97794441..b8b152a204 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 @@ -232,9 +240,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 c6a1479cda..b209cfe8b8 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 @@ -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,9 @@ 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/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index 97508a0173..afb041731a 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 + 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 @@ -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 68abf0830f..64bb41fe01 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 086e09189b..f7c05a59fb 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("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() @@ -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("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);