Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Set groot monitoring parameters individually for bt_navigator's pose … #2409

Closed
wants to merge 1 commit into from
Closed

Set groot monitoring parameters individually for bt_navigator's pose … #2409

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 11 additions & 3 deletions nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -86,6 +86,14 @@ class BtActionServer
*/
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
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
Expand Down Expand Up @@ -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_;
Expand Down
26 changes: 11 additions & 15 deletions nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -193,7 +187,9 @@ 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());
}
Expand Down
10 changes: 6 additions & 4 deletions nav2_bringup/bringup/params/nav2_params.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
9 changes: 9 additions & 0 deletions nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down
13 changes: 13 additions & 0 deletions nav2_bt_navigator/src/bt_navigator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down Expand Up @@ -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<nav2_util::OdomSmoother>(shared_from_this(), 0.3);
Expand Down