diff --git a/nav2_bringup/launch/bt_navigator.xml b/nav2_bringup/launch/bt_navigator.xml index 2667ed02011560eb9962a0f417cd3110005a3c54..2f5fe1cd6af4dd50b0ac2b469a9ba29065b9a824 100644 --- a/nav2_bringup/launch/bt_navigator.xml +++ b/nav2_bringup/launch/bt_navigator.xml @@ -21,7 +21,9 @@ <RateController hz="1.0"> <ComputePathToPose goal="${goal}"/> </RateController> - <FollowPath path="${path}"/> + <SequenceStar name="RememberFollowPathResult"> + <FollowPath path="${path}"/> + </SequenceStar> </Sequence> </BehaviorTree> </root> diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index f0b2b2feb64fd0c5ea697979a5a1ca82c9d7fc5e..939fec62a7f143d50762261e3b590ba85769315f 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -72,7 +72,6 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) // Put items on the blackboard blackboard_->set<geometry_msgs::msg::PoseStamped::SharedPtr>("goal", goal_); // NOLINT blackboard_->set<nav2_msgs::msg::Path::SharedPtr>("path", path_); // NOLINT - blackboard_->set<rclcpp::Node::SharedPtr>("node", client_node_); // NOLINT blackboard_->set<std::chrono::milliseconds>("node_loop_timeout", std::chrono::milliseconds(10)); // NOLINT blackboard_->set<bool>("path_updated", false); // NOLINT blackboard_->set<bool>("initial_pose_received", false); // NOLINT diff --git a/nav2_tasks/include/nav2_tasks/bt_action_node.hpp b/nav2_tasks/include/nav2_tasks/bt_action_node.hpp index b80363a53da8c4a066352f0ed08fa81bb56d43d5..46000b6a0dc9bf7f4eb7bc95c1aabb875013199c 100644 --- a/nav2_tasks/include/nav2_tasks/bt_action_node.hpp +++ b/nav2_tasks/include/nav2_tasks/bt_action_node.hpp @@ -19,22 +19,23 @@ #include <string> #include "behaviortree_cpp/action_node.h" +#include "nav2_util/node_utils.hpp" #include "rclcpp_action/rclcpp_action.hpp" namespace nav2_tasks { template<class ActionT> -class BtActionNode : public BT::CoroActionNode +class BtActionNode : public BT::AsyncActionNode { public: explicit BtActionNode(const std::string & action_name) - : BT::CoroActionNode(action_name), action_name_(action_name) + : BT::AsyncActionNode(action_name), action_name_(action_name) { } BtActionNode(const std::string & action_name, const BT::NodeParameters & params) - : BT::CoroActionNode(action_name, params), action_name_(action_name) + : BT::AsyncActionNode(action_name, params), action_name_(action_name) { } @@ -50,12 +51,13 @@ public: // but override on_init instead. void onInit() final { + node_ = nav2_util::generate_internal_node(); + // Initialize the input and output messages goal_ = typename ActionT::Goal(); result_ = typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult(); // Get the required items from the blackboard - node_ = blackboard()->template get<rclcpp::Node::SharedPtr>("node"); node_loop_timeout_ = blackboard()->template get<std::chrono::milliseconds>("node_loop_timeout"); @@ -126,13 +128,12 @@ new_goal_received: if (rc == rclcpp::executor::FutureReturnCode::TIMEOUT) { on_loop_timeout(); - if (goal_updated_) { + // We can handle a new goal if we're still executing + auto status = goal_handle_->get_status(); + if (goal_updated_ && (status == action_msgs::msg::GoalStatus::STATUS_EXECUTING)) { goal_updated_ = false; goto new_goal_received; } - - // Yield to any other CoroActionNodes (couroutines) - setStatusRunningAndYield(); } } while (rc != rclcpp::executor::FutureReturnCode::SUCCESS); @@ -168,7 +169,6 @@ new_goal_received: } setStatus(BT::NodeStatus::IDLE); - CoroActionNode::halt(); } protected: