From 8c1c996571f7b28e37d2d07f206b9958e9428c57 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Sat, 21 Mar 2020 16:22:25 +0100 Subject: [PATCH 1/9] Proposed refactoring to avoid issues with CoroAction --- .../nav2_behavior_tree/bt_action_node.hpp | 101 ++++++++++++------ .../nav2_behavior_tree/bt_service_node.hpp | 4 +- 2 files changed, 70 insertions(+), 35 deletions(-) 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 2be52da7982..83f9822dc79 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 @@ -26,14 +26,14 @@ namespace nav2_behavior_tree { template -class BtActionNode : public BT::CoroActionNode +class BtActionNode : public BT::ActionNodeBase { public: BtActionNode( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) - : BT::CoroActionNode(xml_tag_name, conf), action_name_(action_name) + : BT::ActionNodeBase(xml_tag_name, conf), action_name_(action_name) { node_ = config().blackboard->get("node"); @@ -101,49 +101,58 @@ class BtActionNode : public BT::CoroActionNode { } + // Called when a the action is aborted. By default, the node will return FAILURE. + // The user may override it to return another value, instead. + virtual BT::NodeStatus on_aborted() + { + return BT::NodeStatus::FAILURE; + } + + // Called when a the action is cancelled. By default, the node will return SUCCESS. + // The user may override it to return another value, instead. + virtual BT::NodeStatus on_cancelled() + { + return BT::NodeStatus::SUCCESS; + } + // The main override required by a BT action BT::NodeStatus tick() override { - on_tick(); - -new_goal_received: - goal_result_available_ = false; - auto send_goal_options = typename rclcpp_action::Client::SendGoalOptions(); - send_goal_options.result_callback = - [this](const typename rclcpp_action::ClientGoalHandle::WrappedResult & result) { - if (result.code != rclcpp_action::ResultCode::ABORTED) { - goal_result_available_ = true; - result_ = result; - } - }; + // first step to be done only at the beginning of the Action + if(status() == BT::NodeStatus::IDLE) + { + // setting the status to RUNNING to notify the BT Loggers (if any) + setStatus(BT::NodeStatus::RUNNING); - auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options); + // user defined callback + on_tick(); - if (rclcpp::spin_until_future_complete(node_, future_goal_handle) != - rclcpp::executor::FutureReturnCode::SUCCESS) - { - throw std::runtime_error("send_goal failed"); + on_new_goal_received(); } - goal_handle_ = future_goal_handle.get(); - if (!goal_handle_) { - throw std::runtime_error("Goal was rejected by the action server"); - } + // The following code corresponds to the "RUNNING" loop - while (rclcpp::ok() && !goal_result_available_) { + if (rclcpp::ok() && !goal_result_available_) { + + // user defined callback. May modify the value of "goal_updated_" on_wait_for_result(); - auto status = goal_handle_->get_status(); - if (goal_updated_ && (status == action_msgs::msg::GoalStatus::STATUS_EXECUTING || - status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED)) + auto goal_status = goal_handle_->get_status(); + + if (goal_updated_ && (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING || + goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED)) { goal_updated_ = false; - goto new_goal_received; + on_new_goal_received(); } - // Yield to any other nodes - setStatusRunningAndYield(); rclcpp::spin_some(node_); + + // check if, after invoking spin_some(), we finally received the result + if(!goal_result_available_){ + // Yield this Action, returning RUNNING + return BT::NodeStatus::RUNNING; + } } switch (result_.code) { @@ -152,10 +161,10 @@ class BtActionNode : public BT::CoroActionNode return BT::NodeStatus::SUCCESS; case rclcpp_action::ResultCode::ABORTED: - return BT::NodeStatus::FAILURE; + return on_aborted(); case rclcpp_action::ResultCode::CANCELED: - return BT::NodeStatus::SUCCESS; + return on_cancelled(); default: throw std::logic_error("BtActionNode::Tick: invalid status value"); @@ -178,7 +187,6 @@ class BtActionNode : public BT::CoroActionNode } setStatus(BT::NodeStatus::IDLE); - CoroActionNode::halt(); } protected: @@ -202,6 +210,33 @@ class BtActionNode : public BT::CoroActionNode return false; } + + void on_new_goal_received() + { + goal_result_available_ = false; + auto send_goal_options = typename rclcpp_action::Client::SendGoalOptions(); + send_goal_options.result_callback = + [this](const typename rclcpp_action::ClientGoalHandle::WrappedResult & result) { + if (result.code != rclcpp_action::ResultCode::ABORTED) { + goal_result_available_ = true; + result_ = result; + } + }; + + auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options); + + if (rclcpp::spin_until_future_complete(node_, future_goal_handle) != + rclcpp::executor::FutureReturnCode::SUCCESS) + { + throw std::runtime_error("send_goal failed"); + } + + goal_handle_ = future_goal_handle.get(); + if (!goal_handle_) { + throw std::runtime_error("Goal was rejected by the action server"); + } + } + const std::string action_name_; typename std::shared_ptr> action_client_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index 39ef9c191ba..8fc3ed55011 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -26,13 +26,13 @@ namespace nav2_behavior_tree { template -class BtServiceNode : public BT::CoroActionNode +class BtServiceNode : public BT::SyncActionNode { public: BtServiceNode( const std::string & service_node_name, const BT::NodeConfiguration & conf) - : BT::CoroActionNode(service_node_name, conf), service_node_name_(service_node_name) + : BT::SyncActionNode(service_node_name, conf), service_node_name_(service_node_name) { node_ = config().blackboard->get("node"); From 0f6353d43966c16c851055718927a0265e82b920 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Sat, 21 Mar 2020 17:49:07 +0100 Subject: [PATCH 2/9] correctly haltAllActions (related to #1600) --- .../behavior_tree_engine.hpp | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp index dccf466e8cc..0621e1bc9c0 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp @@ -44,25 +44,25 @@ class BehaviorTreeEngine const std::string & xml_string, BT::Blackboard::Ptr blackboard); + // In order to re-run a Behavior Tree, we must be able to reset all nodes to the initial state void haltAllActions(BT::TreeNode * root_node) { + // this halt signal should propagate through the entire tree. + root_node->halt(); + root_node->setStatus(BT::NodeStatus::IDLE); + + // but, just in case... auto visitor = [](BT::TreeNode * node) { if (auto action = dynamic_cast(node)) { - action->halt(); + if( action->status()==BT::NodeStatus::RUNNING) { + action->halt(); + } + action->setStatus(BT::NodeStatus::IDLE); } }; BT::applyRecursiveVisitor(root_node, visitor); } - // In order to re-run a Behavior Tree, we must be able to reset all nodes to the initial state - void resetTree(BT::TreeNode * root_node) - { - auto visitor = [](BT::TreeNode * node) { - node->setStatus(BT::NodeStatus::IDLE); - }; - BT::applyRecursiveVisitor(root_node, visitor); - } - protected: // The factory that will be used to dynamically construct the behavior tree BT::BehaviorTreeFactory factory_; From 5fb753aed0eeb027c60bbe5089c71a76402bf58f Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Sun, 22 Mar 2020 16:01:24 +0100 Subject: [PATCH 3/9] not really needed and will be deprecated soon --- .../include/nav2_behavior_tree/behavior_tree_engine.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp index 0621e1bc9c0..63e779b4bb6 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp @@ -49,7 +49,6 @@ class BehaviorTreeEngine { // this halt signal should propagate through the entire tree. root_node->halt(); - root_node->setStatus(BT::NodeStatus::IDLE); // but, just in case... auto visitor = [](BT::TreeNode * node) { @@ -57,7 +56,6 @@ class BehaviorTreeEngine if( action->status()==BT::NodeStatus::RUNNING) { action->halt(); } - action->setStatus(BT::NodeStatus::IDLE); } }; BT::applyRecursiveVisitor(root_node, visitor); From ba82d258acd40db6cb51e6bd7ae576b2fc26a3e9 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Mon, 23 Mar 2020 21:47:09 +0100 Subject: [PATCH 4/9] Applying changes suggested in the comments of #1606 - fix haltAllActions - changes method signature on_success() - reverts the changes made here: https://github.com/ros-planning/navigation2/pull/1515/files --- .../nav2_behavior_tree/behavior_tree_engine.hpp | 8 ++++---- .../include/nav2_behavior_tree/bt_action_node.hpp | 8 ++++---- .../plugins/action/compute_path_to_pose_action.cpp | 3 ++- .../include/nav2_bt_navigator/bt_navigator.hpp | 2 ++ nav2_bt_navigator/src/bt_navigator.cpp | 13 +++++++------ 5 files changed, 19 insertions(+), 15 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp index 63e779b4bb6..70e999078a9 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp @@ -49,13 +49,13 @@ class BehaviorTreeEngine { // this halt signal should propagate through the entire tree. root_node->halt(); + root_node->setStatus(BT::NodeStatus::IDLE); // but, just in case... auto visitor = [](BT::TreeNode * node) { - if (auto action = dynamic_cast(node)) { - if( action->status()==BT::NodeStatus::RUNNING) { - action->halt(); - } + if( node->status()==BT::NodeStatus::RUNNING) { + node->halt(); + node->setStatus(BT::NodeStatus::IDLE); } }; BT::applyRecursiveVisitor(root_node, visitor); 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 83f9822dc79..5432e2e485d 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 @@ -96,9 +96,10 @@ class BtActionNode : public BT::ActionNodeBase } // Called upon successful completion of the action. A derived class can override this - // method to put a value on the blackboard, for example - virtual void on_success() + // method to put a value on the blackboard, for example. + virtual BT::NodeStatus on_success() { + return BT::NodeStatus::SUCCESS; } // Called when a the action is aborted. By default, the node will return FAILURE. @@ -157,8 +158,7 @@ class BtActionNode : public BT::ActionNodeBase switch (result_.code) { case rclcpp_action::ResultCode::SUCCEEDED: - on_success(); - return BT::NodeStatus::SUCCESS; + return on_success(); case rclcpp_action::ResultCode::ABORTED: return on_aborted(); diff --git a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp index b8028892cbf..1071e5d99ff 100644 --- a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp @@ -47,7 +47,7 @@ class ComputePathToPoseAction : public BtActionNodepath); @@ -56,6 +56,7 @@ class ComputePathToPoseAction : public BtActionNodeset("path_updated", true); } + return BT::NodeStatus::SUCCESS; } static BT::PortsList providedPorts() diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp index 0ac93a922b5..fe542f5f63b 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp @@ -110,6 +110,8 @@ class BtNavigator : public nav2_util::LifecycleNode void onGoalPoseReceived(const geometry_msgs::msg::PoseStamped::SharedPtr pose); rclcpp::Subscription::SharedPtr goal_sub_; + BT::Tree tree_; + // The blackboard shared by all of the nodes in the tree BT::Blackboard::Ptr blackboard_; diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index dc478cb61a1..f9574d62a0a 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -129,6 +129,9 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_DEBUG(get_logger(), "Behavior Tree file: '%s'", bt_xml_filename.c_str()); RCLCPP_DEBUG(get_logger(), "Behavior Tree XML: %s", xml_string_.c_str()); + // Create the Behavior Tree from the XML input + tree_ = bt_->buildTreeFromText(xml_string_, blackboard_); + return nav2_util::CallbackReturn::SUCCESS; } @@ -172,6 +175,7 @@ BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/) plugin_lib_names_.clear(); xml_string_.clear(); blackboard_.reset(); + bt_->haltAllActions(tree_.root_node); bt_.reset(); RCLCPP_INFO(get_logger(), "Completed Cleaning up"); @@ -211,11 +215,7 @@ BtNavigator::navigateToPose() return action_server_->is_cancel_requested(); }; - - // Create the Behavior Tree from the XML input - BT::Tree tree = bt_->buildTreeFromText(xml_string_, blackboard_); - - RosTopicLogger topic_logger(client_node_, tree); + RosTopicLogger topic_logger(client_node_, tree_); auto on_loop = [&]() { if (action_server_->is_preempt_requested()) { @@ -227,7 +227,7 @@ BtNavigator::navigateToPose() }; // Execute the BT that was previously created in the configure step - nav2_behavior_tree::BtStatus rc = bt_->run(&tree, on_loop, is_canceling); + nav2_behavior_tree::BtStatus rc = bt_->run(&tree_, on_loop, is_canceling); switch (rc) { case nav2_behavior_tree::BtStatus::SUCCEEDED: @@ -243,6 +243,7 @@ BtNavigator::navigateToPose() case nav2_behavior_tree::BtStatus::CANCELED: RCLCPP_INFO(get_logger(), "Navigation canceled"); action_server_->terminate_all(); + bt_->haltAllActions(tree_.root_node); break; default: From 5a00966a847353545cec3a1c86e5b9d2c6ee91f6 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Mon, 23 Mar 2020 22:29:14 +0100 Subject: [PATCH 5/9] fix warnings and errors --- .../include/nav2_behavior_tree/behavior_tree_engine.hpp | 6 +++--- .../include/nav2_behavior_tree/bt_action_node.hpp | 7 ++----- nav2_bt_navigator/src/bt_navigator.cpp | 3 ++- 3 files changed, 7 insertions(+), 9 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp index 70e999078a9..217534b679c 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp @@ -53,9 +53,9 @@ class BehaviorTreeEngine // but, just in case... auto visitor = [](BT::TreeNode * node) { - if( node->status()==BT::NodeStatus::RUNNING) { - node->halt(); - node->setStatus(BT::NodeStatus::IDLE); + if( node->status() == BT::NodeStatus::RUNNING) { + node->halt(); + node->setStatus(BT::NodeStatus::IDLE); } }; BT::applyRecursiveVisitor(root_node, visitor); 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 5432e2e485d..b69a754cab1 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 @@ -120,7 +120,7 @@ class BtActionNode : public BT::ActionNodeBase BT::NodeStatus tick() override { // first step to be done only at the beginning of the Action - if(status() == BT::NodeStatus::IDLE) + if (status() == BT::NodeStatus::IDLE) { // setting the status to RUNNING to notify the BT Loggers (if any) setStatus(BT::NodeStatus::RUNNING); @@ -132,16 +132,13 @@ 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(); auto goal_status = goal_handle_->get_status(); - if (goal_updated_ && (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING || - goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED)) + goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED)) { goal_updated_ = false; on_new_goal_received(); diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index f9574d62a0a..69b8cd9237b 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -226,6 +226,8 @@ BtNavigator::navigateToPose() topic_logger.flush(); }; + // Make sure that the Bt is not in a running state from a previous execution + bt_->haltAllActions(tree_.root_node); // Execute the BT that was previously created in the configure step nav2_behavior_tree::BtStatus rc = bt_->run(&tree_, on_loop, is_canceling); @@ -243,7 +245,6 @@ BtNavigator::navigateToPose() case nav2_behavior_tree::BtStatus::CANCELED: RCLCPP_INFO(get_logger(), "Navigation canceled"); action_server_->terminate_all(); - bt_->haltAllActions(tree_.root_node); break; default: From 35b197f3f6e26b7a1c046255c652a608e093afc9 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Mon, 23 Mar 2020 23:21:35 +0100 Subject: [PATCH 6/9] make uncrustify happy? --- .../include/nav2_behavior_tree/behavior_tree_engine.hpp | 2 +- .../include/nav2_behavior_tree/bt_action_node.hpp | 7 +++---- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp index 217534b679c..6d2220db4f0 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp @@ -53,7 +53,7 @@ class BehaviorTreeEngine // but, just in case... auto visitor = [](BT::TreeNode * node) { - if( node->status() == BT::NodeStatus::RUNNING) { + if (node->status() == BT::NodeStatus::RUNNING) { node->halt(); node->setStatus(BT::NodeStatus::IDLE); } 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 b69a754cab1..b08ff173343 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 @@ -120,8 +120,7 @@ class BtActionNode : public BT::ActionNodeBase BT::NodeStatus tick() override { // first step to be done only at the beginning of the Action - if (status() == BT::NodeStatus::IDLE) - { + if (status() == BT::NodeStatus::IDLE) { // setting the status to RUNNING to notify the BT Loggers (if any) setStatus(BT::NodeStatus::RUNNING); @@ -138,7 +137,7 @@ class BtActionNode : public BT::ActionNodeBase auto goal_status = goal_handle_->get_status(); if (goal_updated_ && (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING || - goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED)) + goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED)) { goal_updated_ = false; on_new_goal_received(); @@ -147,7 +146,7 @@ class BtActionNode : public BT::ActionNodeBase rclcpp::spin_some(node_); // check if, after invoking spin_some(), we finally received the result - if(!goal_result_available_){ + if (!goal_result_available_) { // Yield this Action, returning RUNNING return BT::NodeStatus::RUNNING; } From 3b4a2ee68fd50d0b01576c145b3cc8b0160ac313 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Tue, 24 Mar 2020 21:30:03 +0100 Subject: [PATCH 7/9] Update bt_navigator.cpp --- nav2_bt_navigator/src/bt_navigator.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 69b8cd9237b..54a9c2a4c05 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -226,10 +226,12 @@ BtNavigator::navigateToPose() topic_logger.flush(); }; - // Make sure that the Bt is not in a running state from a previous execution - bt_->haltAllActions(tree_.root_node); + // Execute the BT that was previously created in the configure step nav2_behavior_tree::BtStatus rc = bt_->run(&tree_, on_loop, is_canceling); + // Make sure that the Bt is not in a running state from a previous execution + // note: if all the ControlNodes are implemented correctly, this is not needed. + bt_->haltAllActions(tree_.root_node); switch (rc) { case nav2_behavior_tree::BtStatus::SUCCEEDED: From 6d2da703c887b29e010eeec556b08db0802d96c9 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Wed, 25 Mar 2020 20:20:04 +0100 Subject: [PATCH 8/9] Update bt_navigator.cpp --- nav2_bt_navigator/src/bt_navigator.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 54a9c2a4c05..9f86e3a024c 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -162,7 +162,6 @@ BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/) // TODO(orduno) Fix the race condition between the worker thread ticking the tree // and the main thread resetting the resources, see #1344 - goal_sub_.reset(); client_node_.reset(); self_client_.reset(); @@ -225,7 +224,6 @@ BtNavigator::navigateToPose() } topic_logger.flush(); }; - // Execute the BT that was previously created in the configure step nav2_behavior_tree::BtStatus rc = bt_->run(&tree_, on_loop, is_canceling); From 95afad6dbb25171824e6fc2e66fa04472a3fd060 Mon Sep 17 00:00:00 2001 From: "daf@blue-ocean-robotics.com" Date: Thu, 26 Mar 2020 21:17:41 +0100 Subject: [PATCH 9/9] uncrustify fix --- nav2_bt_navigator/src/bt_navigator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 9f86e3a024c..0d9f03dd490 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -224,7 +224,7 @@ BtNavigator::navigateToPose() } topic_logger.flush(); }; - + // Execute the BT that was previously created in the configure step nav2_behavior_tree::BtStatus rc = bt_->run(&tree_, on_loop, is_canceling); // Make sure that the Bt is not in a running state from a previous execution