diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index e7abf148c5..0569c58bc9 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -180,6 +180,9 @@ list(APPEND plugin_libs nav2_controller_selector_bt_node) add_library(nav2_goal_checker_selector_bt_node SHARED plugins/action/goal_checker_selector_node.cpp) list(APPEND plugin_libs nav2_goal_checker_selector_bt_node) +add_library(nav2_goal_updated_controller_bt_node SHARED plugins/decorator/goal_updated_controller.cpp) +list(APPEND plugin_libs nav2_goal_updated_controller_bt_node) + foreach(bt_plugin ${plugin_libs}) ament_target_dependencies(${bt_plugin} ${dependencies}) target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp new file mode 100644 index 0000000000..4ac0ab44ee --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp @@ -0,0 +1,68 @@ +// Copyright (c) 2018 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__DECORATOR__GOAL_UPDATED_CONTROLLER_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATED_CONTROLLER_HPP_ + +#include +#include +#include + +#include "behaviortree_cpp_v3/decorator_node.h" + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief A BT::DecoratorNode that ticks its child if the goal was updated + */ +class GoalUpdatedController : public BT::DecoratorNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::GoalUpdatedController + * @param name Name for the XML tag for this node + * @param conf BT node configuration + */ + GoalUpdatedController( + const std::string & name, + const BT::NodeConfiguration & conf); + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ + static BT::PortsList providedPorts() + { + return {}; + } + +private: + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ + BT::NodeStatus tick() override; + + bool goal_was_updated_; + geometry_msgs::msg::PoseStamped goal_; + std::vector goals_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATED_CONTROLLER_HPP_ diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 3a22a90083..3f5fa4b522 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -254,5 +254,8 @@ Length multiplication factor to check if the path is significantly longer + + + diff --git a/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp b/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp new file mode 100644 index 0000000000..f8a2d8cefc --- /dev/null +++ b/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp @@ -0,0 +1,88 @@ +// Copyright (c) 2018 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. + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "behaviortree_cpp_v3/decorator_node.h" +#include "nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp" + + +namespace nav2_behavior_tree +{ + +GoalUpdatedController::GoalUpdatedController( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::DecoratorNode(name, conf) +{ +} + +BT::NodeStatus GoalUpdatedController::tick() +{ + if (status() == BT::NodeStatus::IDLE) { + // Reset since we're starting a new iteration of + // the goal updated controller (moving from IDLE to RUNNING) + + config().blackboard->get>("goals", goals_); + config().blackboard->get("goal", goal_); + + goal_was_updated_ = true; + } + + setStatus(BT::NodeStatus::RUNNING); + + std::vector current_goals; + config().blackboard->get>("goals", current_goals); + geometry_msgs::msg::PoseStamped current_goal; + config().blackboard->get("goal", current_goal); + + if (goal_ != current_goal || goals_ != current_goals) { + goal_ = current_goal; + goals_ = current_goals; + goal_was_updated_ = true; + } + + // The child gets ticked the first time through and any time the goal has + // changed or preempted. In addition, once the child begins to run, it is ticked each time + // 'til completion + if ((child_node_->status() == BT::NodeStatus::RUNNING) || goal_was_updated_) { + goal_was_updated_ = false; + const BT::NodeStatus child_state = child_node_->executeTick(); + + switch (child_state) { + case BT::NodeStatus::RUNNING: + return BT::NodeStatus::RUNNING; + + case BT::NodeStatus::SUCCESS: + return BT::NodeStatus::SUCCESS; + + case BT::NodeStatus::FAILURE: + default: + return BT::NodeStatus::FAILURE; + } + } + + return status(); +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("GoalUpdatedController"); +} diff --git a/nav2_behavior_tree/test/plugins/decorator/CMakeLists.txt b/nav2_behavior_tree/test/plugins/decorator/CMakeLists.txt index a1da83658b..ed6504a682 100644 --- a/nav2_behavior_tree/test/plugins/decorator/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/decorator/CMakeLists.txt @@ -18,6 +18,10 @@ ament_add_gtest(test_single_trigger_node test_single_trigger_node.cpp) target_link_libraries(test_single_trigger_node nav2_single_trigger_bt_node) ament_target_dependencies(test_single_trigger_node ${dependencies}) +ament_add_gtest(test_goal_updated_controller test_goal_updated_controller.cpp) +target_link_libraries(test_goal_updated_controller nav2_goal_updated_controller_bt_node) +ament_target_dependencies(test_goal_updated_controller ${dependencies}) + ament_add_gtest(test_decorator_path_longer_on_approach test_path_longer_on_approach.cpp) target_link_libraries(test_decorator_path_longer_on_approach nav2_path_longer_on_approach_bt_node) ament_target_dependencies(test_decorator_path_longer_on_approach ${dependencies}) diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp new file mode 100644 index 0000000000..d8e2a24fb0 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp @@ -0,0 +1,108 @@ +// 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 "../../test_behavior_tree_fixture.hpp" +#include "nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp" + +using namespace std::chrono; // NOLINT +using namespace std::chrono_literals; // NOLINT + +class GoalUpdatedControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture +{ +public: + void SetUp() + { + // Setting fake goals on blackboard + geometry_msgs::msg::PoseStamped goal1; + goal1.header.stamp = node_->now(); + std::vector poses1; + poses1.push_back(goal1); + config_->blackboard->set("goal", goal1); + config_->blackboard->set>("goals", poses1); + + bt_node_ = std::make_shared( + "goal_updated_controller", *config_); + dummy_node_ = std::make_shared(); + bt_node_->setChild(dummy_node_.get()); + } + + void TearDown() + { + dummy_node_.reset(); + bt_node_.reset(); + } + +protected: + static std::shared_ptr bt_node_; + static std::shared_ptr dummy_node_; +}; + +std::shared_ptr +GoalUpdatedControllerTestFixture::bt_node_ = nullptr; +std::shared_ptr +GoalUpdatedControllerTestFixture::dummy_node_ = nullptr; + +TEST_F(GoalUpdatedControllerTestFixture, test_behavior) +{ + // Creating updated fake-goals + geometry_msgs::msg::PoseStamped goal2; + goal2.header.stamp = node_->now(); + std::vector poses2; + poses2.push_back(goal2); + + // starting in idle + EXPECT_EQ(bt_node_->status(), BT::NodeStatus::IDLE); + + // tick for the first time, dummy node should be ticked + dummy_node_->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE); + + // tick again with updated goal, dummy node should be ticked + config_->blackboard->set("goal", goal2); + dummy_node_->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE); + + // tick again without update, dummy node should not be ticked + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); + EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE); + + // tick again with updated goals, dummy node should be ticked + config_->blackboard->set>("goals", poses2); + dummy_node_->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE); +} + +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_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index 88bebc3590..a35fe9a870 100644 --- a/nav2_bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml @@ -82,6 +82,7 @@ bt_navigator: - nav2_path_expiring_timer_condition - nav2_distance_traveled_condition_bt_node - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node - nav2_is_battery_low_condition_bt_node - nav2_navigate_through_poses_action_bt_node - nav2_navigate_to_pose_action_bt_node diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml index 7ec0f1edda..618ba12bdf 100644 --- a/nav2_bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml @@ -82,6 +82,7 @@ bt_navigator: - nav2_path_expiring_timer_condition - nav2_distance_traveled_condition_bt_node - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node - nav2_is_battery_low_condition_bt_node - nav2_navigate_through_poses_action_bt_node - nav2_navigate_to_pose_action_bt_node diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 7180548b82..1b98e85c91 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -82,6 +82,7 @@ bt_navigator: - nav2_path_expiring_timer_condition - nav2_distance_traveled_condition_bt_node - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node - nav2_is_battery_low_condition_bt_node - nav2_navigate_through_poses_action_bt_node - nav2_navigate_to_pose_action_bt_node diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml new file mode 100644 index 0000000000..46d18fe654 --- /dev/null +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 0e31d13cd0..79b50992b4 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -64,6 +64,7 @@ BtNavigator::BtNavigator(const rclcpp::NodeOptions & options) "nav2_path_expiring_timer_condition", "nav2_distance_traveled_condition_bt_node", "nav2_single_trigger_bt_node", + "nav2_goal_updated_controller_bt_node", "nav2_is_battery_low_condition_bt_node", "nav2_navigate_through_poses_action_bt_node", "nav2_navigate_to_pose_action_bt_node", diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml index c068d67ff9..b4f98cedea 100644 --- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -78,6 +78,7 @@ bt_navigator: - nav2_time_expired_condition_bt_node - nav2_distance_traveled_condition_bt_node - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node - nav2_is_battery_low_condition_bt_node - nav2_navigate_through_poses_action_bt_node - nav2_navigate_to_pose_action_bt_node diff --git a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml index f4fa961a3d..1e021bb4f7 100644 --- a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml @@ -79,6 +79,7 @@ bt_navigator: - nav2_time_expired_condition_bt_node - nav2_distance_traveled_condition_bt_node - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node - nav2_is_battery_low_condition_bt_node - nav2_navigate_through_poses_action_bt_node - nav2_navigate_to_pose_action_bt_node diff --git a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml index df05269447..fe03fe138d 100644 --- a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml @@ -79,6 +79,7 @@ bt_navigator: - nav2_time_expired_condition_bt_node - nav2_distance_traveled_condition_bt_node - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node - nav2_is_battery_low_condition_bt_node - nav2_navigate_through_poses_action_bt_node - nav2_navigate_to_pose_action_bt_node