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 6d2220db4f..ca627bbe7e 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,11 @@ 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 (node->status() == BT::NodeStatus::RUNNING) { node->halt(); - node->setStatus(BT::NodeStatus::IDLE); } }; BT::applyRecursiveVisitor(root_node, visitor); diff --git a/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp b/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp index 7c7dcf7059..75333b4a8f 100644 --- a/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp +++ b/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp @@ -82,7 +82,7 @@ BT::NodeStatus PipelineSequence::tick() auto status = children_nodes_[i]->executeTick(); switch (status) { case BT::NodeStatus::FAILURE: - haltChildren(0); + ControlNode::haltChildren(); last_child_ticked_ = 0; // reset return status; break; @@ -106,7 +106,7 @@ BT::NodeStatus PipelineSequence::tick() } } // Wrap up. - haltChildren(0); + ControlNode::haltChildren(); last_child_ticked_ = 0; // reset return BT::NodeStatus::SUCCESS; } diff --git a/nav2_behavior_tree/plugins/control/recovery_node.cpp b/nav2_behavior_tree/plugins/control/recovery_node.cpp index 945f8f8a35..2a213cfa4a 100644 --- a/nav2_behavior_tree/plugins/control/recovery_node.cpp +++ b/nav2_behavior_tree/plugins/control/recovery_node.cpp @@ -88,7 +88,7 @@ class RecoveryNode : public BT::ControlNode current_child_idx_++; break; } else { - haltChildren(0); + ControlNode::haltChildren(); return BT::NodeStatus::FAILURE; } } @@ -111,7 +111,7 @@ class RecoveryNode : public BT::ControlNode { retry_count_++; current_child_idx_--; - haltChildren(1); + ControlNode::haltChildren(); } break; diff --git a/nav2_behavior_tree/plugins/control/round_robin_node.cpp b/nav2_behavior_tree/plugins/control/round_robin_node.cpp index 95b1c96e92..be85e75ec7 100644 --- a/nav2_behavior_tree/plugins/control/round_robin_node.cpp +++ b/nav2_behavior_tree/plugins/control/round_robin_node.cpp @@ -46,11 +46,11 @@ class RoundRobinNode : public BT::ControlNode current_child_idx_ = 0; } - haltChildren(0); + ControlNode::haltChildren(); return BT::NodeStatus::SUCCESS; case BT::NodeStatus::FAILURE: - haltChildren(0); + ControlNode::haltChildren(); return BT::NodeStatus::FAILURE; case BT::NodeStatus::RUNNING: diff --git a/nav2_behavior_tree/plugins/decorator/rate_controller.cpp b/nav2_behavior_tree/plugins/decorator/rate_controller.cpp index a80e1d078c..2c972eacd7 100644 --- a/nav2_behavior_tree/plugins/decorator/rate_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/rate_controller.cpp @@ -86,13 +86,11 @@ inline BT::NodeStatus RateController::tick() return BT::NodeStatus::RUNNING; case BT::NodeStatus::SUCCESS: - child_node_->setStatus(BT::NodeStatus::IDLE); start_ = std::chrono::high_resolution_clock::now(); // Reset the timer return BT::NodeStatus::SUCCESS; case BT::NodeStatus::FAILURE: default: - child_node_->setStatus(BT::NodeStatus::IDLE); return BT::NodeStatus::FAILURE; } } diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index acf962d356..36b852e61c 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -47,11 +47,11 @@ BehaviorTreeEngine::run( // Loop until something happens with ROS or the node completes while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { if (cancelRequested()) { - tree->root_node->halt(); + tree->rootNode()->halt(); return BtStatus::CANCELED; } - result = tree->root_node->executeTick(); + result = tree->tickRoot(); onLoop(); diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 0d9f03dd49..71617aa969 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -174,7 +174,7 @@ BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/) plugin_lib_names_.clear(); xml_string_.clear(); blackboard_.reset(); - bt_->haltAllActions(tree_.root_node); + bt_->haltAllActions(tree_.rootNode()); bt_.reset(); RCLCPP_INFO(get_logger(), "Completed Cleaning up"); @@ -229,7 +229,7 @@ BtNavigator::navigateToPose() 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); + bt_->haltAllActions(tree_.rootNode()); switch (rc) { case nav2_behavior_tree::BtStatus::SUCCEEDED: diff --git a/nav2_bt_navigator/src/ros_topic_logger.cpp b/nav2_bt_navigator/src/ros_topic_logger.cpp index 3c1348f38d..192318d2f7 100644 --- a/nav2_bt_navigator/src/ros_topic_logger.cpp +++ b/nav2_bt_navigator/src/ros_topic_logger.cpp @@ -22,7 +22,7 @@ namespace nav2_bt_navigator RosTopicLogger::RosTopicLogger( const rclcpp::Node::SharedPtr & ros_node, const BT::Tree & tree) -: StatusChangeLogger(tree.root_node), ros_node_(ros_node) +: StatusChangeLogger(tree.rootNode()), ros_node_(ros_node) { log_pub_ = ros_node_->create_publisher( "behavior_tree_log", diff --git a/tools/ros2_dependencies.repos b/tools/ros2_dependencies.repos index d1df3d43b8..b10c8af63a 100644 --- a/tools/ros2_dependencies.repos +++ b/tools/ros2_dependencies.repos @@ -2,7 +2,7 @@ repositories: BehaviorTree.CPP: type: git url: https://github.com/BehaviorTree/BehaviorTree.CPP.git - version: ros2-3.1.1 + version: master angles: type: git url: https://github.com/ros/angles.git