diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index ace1542b11..5b34469e7e 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -132,9 +132,12 @@ class BtActionNode : public BT::ActionNodeBase /** * @brief Function to perform some user-defined operation after a timeout - * waiting for a result that hasn't been received yet + * waiting for a result that hasn't been received yet. Also provides access to + * the latest feedback message from the action server. Feedback will be nullptr + * in subsequent calls to this function if no new feedback is received while waiting for a result. + * @param feedback shared_ptr to latest feedback message, nullptr if no new feedback was received */ - virtual void on_wait_for_result() + virtual void on_wait_for_result(std::shared_ptr/*feedback*/) { } @@ -206,7 +209,10 @@ class BtActionNode : public BT::ActionNodeBase // The following code corresponds to the "RUNNING" loop if (rclcpp::ok() && !goal_result_available_) { // user defined callback. May modify the value of "goal_updated_" - on_wait_for_result(); + on_wait_for_result(feedback_); + + // reset feedback to avoid stale information + feedback_.reset(); auto goal_status = goal_handle_->get_status(); if (goal_updated_ && (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING || @@ -340,6 +346,11 @@ class BtActionNode : public BT::ActionNodeBase result_ = result; } }; + send_goal_options.feedback_callback = + [this](typename rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr feedback) { + feedback_ = feedback; + }; future_goal_handle_ = std::make_shared< std::shared_future::SharedPtr>>( @@ -406,6 +417,9 @@ class BtActionNode : public BT::ActionNodeBase typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle_; typename rclcpp_action::ClientGoalHandle::WrappedResult result_; + // To handle feedback from action server + std::shared_ptr feedback_; + // The node that will be used for any ROS operations rclcpp::Node::SharedPtr node_; rclcpp::CallbackGroup::SharedPtr callback_group_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp index ef6ccf0d63..a97993c773 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp @@ -16,6 +16,7 @@ #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_PATH_ACTION_HPP_ #include +#include #include "nav2_msgs/action/follow_path.hpp" #include "nav2_behavior_tree/bt_action_node.hpp" @@ -48,8 +49,10 @@ class FollowPathAction : public BtActionNode /** * @brief Function to perform some user-defined operation after a timeout * waiting for a result that hasn't been received yet + * @param feedback shared_ptr to latest feedback message */ - void on_wait_for_result() override; + void on_wait_for_result( + std::shared_ptr feedback) override; /** * @brief Creates list of BT ports diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp index d3353a8981..0fc0dc5b57 100644 --- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp @@ -35,7 +35,8 @@ void FollowPathAction::on_tick() getInput("goal_checker_id", goal_.goal_checker_id); } -void FollowPathAction::on_wait_for_result() +void FollowPathAction::on_wait_for_result( + std::shared_ptr/*feedback*/) { // Grab the new path nav_msgs::msg::Path new_path;