From ec7f550ced0bcba19dedc013a55c5ee5a02b76c9 Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Thu, 28 May 2020 00:00:44 +0530 Subject: [PATCH] Add condition nodes for time and distance replanning (#1705) * Add condition nodes and behavior tree to enable replan on new goal Signed-off-by: Sarthak Mittal * Fix time expired and distance traveled conditions * Remove new_goal_received from blackboard * Fix IDLE check condition in new condition nodes * Fix lint errors * Fix lint errors * Address reviewer's comments Signed-off-by: Sarthak Mittal * Add tests Signed-off-by: Sarthak Mittal --- nav2_behavior_tree/CMakeLists.txt | 6 + .../plugins}/distance_controller.hpp | 6 +- .../plugins/distance_traveled_condition.hpp | 65 +++++++++ .../plugins/time_expired_condition.hpp | 54 +++++++ .../condition/distance_traveled_condition.cpp | 91 ++++++++++++ .../condition/time_expired_condition.cpp | 69 +++++++++ .../plugins/decorator/distance_controller.cpp | 2 +- nav2_behavior_tree/test/CMakeLists.txt | 22 ++- .../condition/test_distance_traveled.cpp | 134 ++++++++++++++++++ .../plugins/condition/test_time_expired.cpp | 116 +++++++++++++++ .../decorator/test_distance_controller.cpp | 2 +- .../params/nav2_multirobot_params_1.yaml | 2 + .../params/nav2_multirobot_params_2.yaml | 2 + nav2_bringup/bringup/params/nav2_params.yaml | 2 + nav2_bt_navigator/src/bt_navigator.cpp | 5 +- 15 files changed, 570 insertions(+), 8 deletions(-) rename nav2_behavior_tree/{plugins/decorator => include/nav2_behavior_tree/plugins}/distance_controller.hpp (88%) create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/distance_traveled_condition.hpp create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/time_expired_condition.hpp create mode 100644 nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp create mode 100644 nav2_behavior_tree/plugins/condition/time_expired_condition.cpp create mode 100644 nav2_behavior_tree/test/plugins/condition/test_distance_traveled.cpp create mode 100644 nav2_behavior_tree/test/plugins/condition/test_time_expired.cpp diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 81750481e4..816fa62d2f 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -80,6 +80,12 @@ list(APPEND plugin_libs nav2_goal_reached_condition_bt_node) add_library(nav2_goal_updated_condition_bt_node SHARED plugins/condition/goal_updated_condition.cpp) list(APPEND plugin_libs nav2_goal_updated_condition_bt_node) +add_library(nav2_time_expired_condition_bt_node SHARED plugins/condition/time_expired_condition.cpp) +list(APPEND plugin_libs nav2_time_expired_condition_bt_node) + +add_library(nav2_distance_traveled_condition_bt_node SHARED plugins/condition/distance_traveled_condition.cpp) +list(APPEND plugin_libs nav2_distance_traveled_condition_bt_node) + add_library(nav2_initial_pose_received_condition_bt_node SHARED plugins/condition/initial_pose_received_condition.cpp) list(APPEND plugin_libs nav2_initial_pose_received_condition_bt_node) diff --git a/nav2_behavior_tree/plugins/decorator/distance_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/distance_controller.hpp similarity index 88% rename from nav2_behavior_tree/plugins/decorator/distance_controller.hpp rename to nav2_behavior_tree/include/nav2_behavior_tree/plugins/distance_controller.hpp index 299833f555..b7ad07cc7f 100644 --- a/nav2_behavior_tree/plugins/decorator/distance_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/distance_controller.hpp @@ -13,8 +13,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__DISTANCE_CONTROLLER_HPP_ -#define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__DISTANCE_CONTROLLER_HPP_ +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DISTANCE_CONTROLLER_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__DISTANCE_CONTROLLER_HPP_ #include #include @@ -63,4 +63,4 @@ class DistanceController : public BT::DecoratorNode } // namespace nav2_behavior_tree -#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__DISTANCE_CONTROLLER_HPP_ +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DISTANCE_CONTROLLER_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/distance_traveled_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/distance_traveled_condition.hpp new file mode 100644 index 0000000000..e9a4b6bde5 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/distance_traveled_condition.hpp @@ -0,0 +1,65 @@ +// Copyright (c) 2020 Sarthak Mittal +// Copyright (c) 2019 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DISTANCE_TRAVELED_CONDITION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__DISTANCE_TRAVELED_CONDITION_HPP_ + +#include +#include + +#include "behaviortree_cpp_v3/condition_node.h" + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "tf2_ros/buffer.h" + +namespace nav2_behavior_tree +{ + +class DistanceTraveledCondition : public BT::ConditionNode +{ +public: + DistanceTraveledCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf); + + DistanceTraveledCondition() = delete; + + BT::NodeStatus tick() override; + + static BT::PortsList providedPorts() + { + return { + BT::InputPort("distance", 1.0, "Distance"), + BT::InputPort("global_frame", std::string("map"), "Global frame"), + BT::InputPort("robot_base_frame", std::string("base_link"), "Robot base frame") + }; + } + +private: + rclcpp::Node::SharedPtr node_; + std::shared_ptr tf_; + + geometry_msgs::msg::PoseStamped start_pose_; + + double distance_; + double transform_tolerance_; + std::string global_frame_; + std::string robot_base_frame_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DISTANCE_TRAVELED_CONDITION_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/time_expired_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/time_expired_condition.hpp new file mode 100644 index 0000000000..6fb99d60ca --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/time_expired_condition.hpp @@ -0,0 +1,54 @@ +// Copyright (c) 2020 Sarthak Mittal +// Copyright (c) 2019 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__TIME_EXPIRED_CONDITION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__TIME_EXPIRED_CONDITION_HPP_ + +#include + +#include "rclcpp/rclcpp.hpp" +#include "behaviortree_cpp_v3/condition_node.h" + +namespace nav2_behavior_tree +{ + +class TimeExpiredCondition : public BT::ConditionNode +{ +public: + TimeExpiredCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf); + + TimeExpiredCondition() = delete; + + BT::NodeStatus tick() override; + + // Any BT node that accepts parameters must provide a requiredNodeParameters method + static BT::PortsList providedPorts() + { + return { + BT::InputPort("seconds", 1.0, "Seconds") + }; + } + +private: + rclcpp::Node::SharedPtr node_; + rclcpp::Time start_; + double period_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__TIME_EXPIRED_CONDITION_HPP_ diff --git a/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp b/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp new file mode 100644 index 0000000000..cb700e7a41 --- /dev/null +++ b/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp @@ -0,0 +1,91 @@ +// Copyright (c) 2019 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__DISTANCE_TRAVELED_CONDITION_HPP_ +#define NAV2_BEHAVIOR_TREE__DISTANCE_TRAVELED_CONDITION_HPP_ + +#include +#include + +#include "nav2_util/robot_utils.hpp" +#include "nav2_util/geometry_utils.hpp" + +#include "nav2_behavior_tree/plugins/distance_traveled_condition.hpp" + +namespace nav2_behavior_tree +{ + +DistanceTraveledCondition::DistanceTraveledCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf) +: BT::ConditionNode(condition_name, conf), + distance_(1.0), + transform_tolerance_(0.1), + global_frame_("map"), + robot_base_frame_("base_link") +{ + getInput("distance", distance_); + getInput("global_frame", global_frame_); + getInput("robot_base_frame", robot_base_frame_); + node_ = config().blackboard->get("node"); + tf_ = config().blackboard->get>("tf_buffer"); + node_->get_parameter("transform_tolerance", transform_tolerance_); +} + +BT::NodeStatus DistanceTraveledCondition::tick() +{ + if (status() == BT::NodeStatus::IDLE) { + if (!nav2_util::getCurrentPose( + start_pose_, *tf_, global_frame_, robot_base_frame_, + transform_tolerance_)) + { + RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available."); + } + return BT::NodeStatus::FAILURE; + } + + // Determine distance travelled since we've started this iteration + geometry_msgs::msg::PoseStamped current_pose; + if (!nav2_util::getCurrentPose( + current_pose, *tf_, global_frame_, robot_base_frame_, + transform_tolerance_)) + { + RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available."); + return BT::NodeStatus::FAILURE; + } + + // Get euclidean distance + auto travelled = nav2_util::geometry_utils::euclidean_distance( + start_pose_.pose, current_pose.pose); + + if (travelled < distance_) { + return BT::NodeStatus::FAILURE; + } + + // Update start pose + start_pose_ = current_pose; + + return BT::NodeStatus::SUCCESS; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("DistanceTraveled"); +} + +#endif // NAV2_BEHAVIOR_TREE__DISTANCE_TRAVELED_CONDITION_HPP_ diff --git a/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp b/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp new file mode 100644 index 0000000000..e075c420d3 --- /dev/null +++ b/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp @@ -0,0 +1,69 @@ +// Copyright (c) 2019 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__TIME_EXPIRED_CONDITION_HPP_ +#define NAV2_BEHAVIOR_TREE__TIME_EXPIRED_CONDITION_HPP_ + +#include +#include + +#include "behaviortree_cpp_v3/condition_node.h" + +#include "nav2_behavior_tree/plugins/time_expired_condition.hpp" + +namespace nav2_behavior_tree +{ + +TimeExpiredCondition::TimeExpiredCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf) +: BT::ConditionNode(condition_name, conf), + period_(1.0) +{ + getInput("seconds", period_); + node_ = config().blackboard->get("node"); + start_ = node_->now(); +} + +BT::NodeStatus TimeExpiredCondition::tick() +{ + if (status() == BT::NodeStatus::IDLE) { + start_ = node_->now(); + return BT::NodeStatus::FAILURE; + } + + // Determine how long its been since we've started this iteration + auto elapsed = node_->now() - start_; + + // Now, get that in seconds + auto seconds = elapsed.seconds(); + + if (seconds < period_) { + return BT::NodeStatus::FAILURE; + } + + start_ = node_->now(); // Reset the timer + return BT::NodeStatus::SUCCESS; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("TimeExpired"); +} + +#endif // NAV2_BEHAVIOR_TREE__TIME_EXPIRED_CONDITION_HPP_ diff --git a/nav2_behavior_tree/plugins/decorator/distance_controller.cpp b/nav2_behavior_tree/plugins/decorator/distance_controller.cpp index ce3a61b450..a7d9c1f5fc 100644 --- a/nav2_behavior_tree/plugins/decorator/distance_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/distance_controller.cpp @@ -25,7 +25,7 @@ #include "behaviortree_cpp_v3/decorator_node.h" -#include "distance_controller.hpp" +#include "nav2_behavior_tree/plugins/distance_controller.hpp" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/test/CMakeLists.txt b/nav2_behavior_tree/test/CMakeLists.txt index a6dbd489ca..243305dd1e 100644 --- a/nav2_behavior_tree/test/CMakeLists.txt +++ b/nav2_behavior_tree/test/CMakeLists.txt @@ -1,11 +1,29 @@ ament_add_gtest(test_decorator_distance_controller plugins/decorator/test_distance_controller.cpp ) - target_link_libraries(test_decorator_distance_controller nav2_distance_controller_bt_node ) - ament_target_dependencies(test_decorator_distance_controller ${dependencies} ) + +ament_add_gtest(test_condition_distance_traveled + plugins/condition/test_distance_traveled.cpp +) +target_link_libraries(test_condition_distance_traveled + nav2_distance_traveled_condition_bt_node +) +ament_target_dependencies(test_condition_distance_traveled + ${dependencies} +) + +ament_add_gtest(test_condition_time_expired + plugins/condition/test_time_expired.cpp +) +target_link_libraries(test_condition_time_expired + nav2_time_expired_condition_bt_node +) +ament_target_dependencies(test_condition_time_expired + ${dependencies} +) diff --git a/nav2_behavior_tree/test/plugins/condition/test_distance_traveled.cpp b/nav2_behavior_tree/test/plugins/condition/test_distance_traveled.cpp new file mode 100644 index 0000000000..32f553f6c6 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/condition/test_distance_traveled.cpp @@ -0,0 +1,134 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/robot_utils.hpp" + +#include "../../test_transform_handler.hpp" +#include "../../test_dummy_tree_node.hpp" +#include "nav2_behavior_tree/plugins/distance_traveled_condition.hpp" + +class DistanceTraveledConditionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + transform_handler_ = new nav2_behavior_tree::TransformHandler(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set( + "node", + rclcpp::Node::SharedPtr(transform_handler_)); + config_->blackboard->set>( + "tf_buffer", + transform_handler_->getBuffer()); + config_->blackboard->set( + "server_timeout", + std::chrono::milliseconds(10)); + config_->blackboard->set("path_updated", false); + config_->blackboard->set("initial_pose_received", false); + + transform_handler_->activate(); + transform_handler_->waitForTransform(); + } + + static void TearDownTestCase() + { + transform_handler_->deactivate(); + delete transform_handler_; + delete config_; + transform_handler_ = nullptr; + config_ = nullptr; + } + + void SetUp() + { + node_ = new nav2_behavior_tree::DistanceTraveledCondition("distance_traveled", *config_); + } + + void TearDown() + { + node_ = nullptr; + } + +protected: + static nav2_behavior_tree::TransformHandler * transform_handler_; + static BT::NodeConfiguration * config_; + static nav2_behavior_tree::DistanceTraveledCondition * node_; +}; + +nav2_behavior_tree::TransformHandler * DistanceTraveledConditionTestFixture::transform_handler_ = + nullptr; +BT::NodeConfiguration * DistanceTraveledConditionTestFixture::config_ = nullptr; +nav2_behavior_tree::DistanceTraveledCondition * DistanceTraveledConditionTestFixture::node_ = + nullptr; + +TEST_F(DistanceTraveledConditionTestFixture, test_behavior) +{ + EXPECT_EQ(node_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(node_->executeTick(), BT::NodeStatus::FAILURE); + + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 0; + pose.pose.position.y = 0; + pose.pose.orientation.w = 1; + + double traveled = 0; + for (int i = 1; i <= 20; i++) { + pose.pose.position.x = i * 0.51; + transform_handler_->updateRobotPose(pose.pose); + + // Wait for transforms to actually update + // updated pose is i * 0.51 + // we wait for the traveled distance to reach a value > i * 0.5 + // we can assume the current transform has been updated at this point + while (traveled < i * 0.5) { + if (nav2_util::getCurrentPose(pose, *transform_handler_->getBuffer())) { + traveled = pose.pose.position.x; + } + } + + if (i % 2) { + EXPECT_EQ(node_->executeTick(), BT::NodeStatus::FAILURE); + } else { + EXPECT_EQ(node_->executeTick(), BT::NodeStatus::SUCCESS); + } + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + bool all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/condition/test_time_expired.cpp b/nav2_behavior_tree/test/plugins/condition/test_time_expired.cpp new file mode 100644 index 0000000000..db19ce1444 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/condition/test_time_expired.cpp @@ -0,0 +1,116 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/robot_utils.hpp" + +#include "../../test_transform_handler.hpp" +#include "nav2_behavior_tree/plugins/time_expired_condition.hpp" + +using namespace std::chrono; // NOLINT +using namespace std::chrono_literals; // NOLINT + +class TimeExpiredConditionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + transform_handler_ = new nav2_behavior_tree::TransformHandler(); + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set( + "node", + rclcpp::Node::SharedPtr(transform_handler_)); + config_->blackboard->set>( + "tf_buffer", + transform_handler_->getBuffer()); + config_->blackboard->set( + "server_timeout", + std::chrono::milliseconds(10)); + config_->blackboard->set("path_updated", false); + config_->blackboard->set("initial_pose_received", false); + + transform_handler_->activate(); + transform_handler_->waitForTransform(); + } + + static void TearDownTestCase() + { + transform_handler_->deactivate(); + delete transform_handler_; + delete config_; + transform_handler_ = nullptr; + config_ = nullptr; + } + + void SetUp() + { + node_ = new nav2_behavior_tree::TimeExpiredCondition("time_expired", *config_); + } + + void TearDown() + { + node_ = nullptr; + } + +protected: + static nav2_behavior_tree::TransformHandler * transform_handler_; + static BT::NodeConfiguration * config_; + static nav2_behavior_tree::TimeExpiredCondition * node_; +}; + +nav2_behavior_tree::TransformHandler * TimeExpiredConditionTestFixture::transform_handler_ = + nullptr; +BT::NodeConfiguration * TimeExpiredConditionTestFixture::config_ = nullptr; +nav2_behavior_tree::TimeExpiredCondition * TimeExpiredConditionTestFixture::node_ = nullptr; + +TEST_F(TimeExpiredConditionTestFixture, test_behavior) +{ + EXPECT_EQ(node_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(node_->executeTick(), BT::NodeStatus::FAILURE); + + for (int i = 0; i < 20; ++i) { + rclcpp::sleep_for(500ms); + if (i % 2) { + EXPECT_EQ(node_->executeTick(), BT::NodeStatus::SUCCESS); + } else { + EXPECT_EQ(node_->executeTick(), BT::NodeStatus::FAILURE); + } + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + bool all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/decorator/test_distance_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_distance_controller.cpp index 53036a3ddc..a1b2509d95 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_distance_controller.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_distance_controller.cpp @@ -24,7 +24,7 @@ #include "../../test_transform_handler.hpp" #include "../../test_dummy_tree_node.hpp" -#include "../../../plugins/decorator/distance_controller.hpp" +#include "nav2_behavior_tree/plugins/distance_controller.hpp" class DistanceControllerTestFixture : public ::testing::Test { diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml index d561cd7bea..cb17f396a7 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml @@ -69,6 +69,8 @@ bt_navigator: - nav2_recovery_node_bt_node - nav2_pipeline_sequence_bt_node - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_distance_traveled_condition_bt_node bt_navigator_rclcpp_node: ros__parameters: diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml index 57a142aba3..726ce779e5 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml @@ -69,6 +69,8 @@ bt_navigator: - nav2_recovery_node_bt_node - nav2_pipeline_sequence_bt_node - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_distance_traveled_condition_bt_node bt_navigator_rclcpp_node: ros__parameters: diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index 50738827be..20bf4fad13 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -70,6 +70,8 @@ bt_navigator: - nav2_pipeline_sequence_bt_node - nav2_round_robin_node_bt_node - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_distance_traveled_condition_bt_node bt_navigator_rclcpp_node: ros__parameters: diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 8b62c9b9f3..7e28205aa1 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" @@ -52,7 +53,9 @@ BtNavigator::BtNavigator() "nav2_recovery_node_bt_node", "nav2_pipeline_sequence_bt_node", "nav2_round_robin_node_bt_node", - "nav2_transform_available_condition_bt_node" + "nav2_transform_available_condition_bt_node", + "nav2_time_expired_condition_bt_node", + "nav2_distance_traveled_condition_bt_node" }; // Declare this node's parameters