From cc6192e3499a8bf2973bddc1622c9245588b96a5 Mon Sep 17 00:00:00 2001 From: Pradheep-office Date: Mon, 21 Mar 2022 11:03:18 +0100 Subject: [PATCH 1/6] adding Bt to cancel wait --- nav2_behavior_tree/CMakeLists.txt | 3 + .../plugins/action/wait_cancel_node.hpp | 59 ++++++ .../plugins/action/wait_cancel_node.cpp | 47 +++++ .../test/plugins/action/CMakeLists.txt | 4 + .../plugins/action/test_wait_cancel_node.cpp | 171 ++++++++++++++++++ 5 files changed, 284 insertions(+) create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_cancel_node.hpp create mode 100644 nav2_behavior_tree/plugins/action/wait_cancel_node.cpp create mode 100644 nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 0d2240961a..6a63bdd2e8 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -60,6 +60,9 @@ list(APPEND plugin_libs nav2_compute_path_through_poses_action_bt_node) add_library(nav2_controller_cancel_bt_node SHARED plugins/action/controller_cancel_node.cpp) list(APPEND plugin_libs nav2_controller_cancel_bt_node) +add_library(nav2_wait_cancel_bt_node SHARED plugins/action/wait_cancel_node.cpp) +list(APPEND plugin_libs nav2_wait_cancel_bt_node) + add_library(nav2_smooth_path_action_bt_node SHARED plugins/action/smooth_path_action.cpp) list(APPEND plugin_libs nav2_smooth_path_action_bt_node) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_cancel_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_cancel_node.hpp new file mode 100644 index 0000000000..e0815c2dbb --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_cancel_node.hpp @@ -0,0 +1,59 @@ +// Copyright (c) 2022 Neobotix GmbH +// +// 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__ACTION__WAIT_CANCEL_NODE_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__WAIT_CANCEL_NODE_HPP_ + +#include +#include + +#include "nav2_msgs/action/wait.hpp" + +#include "nav2_behavior_tree/bt_cancel_action_node.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::Wait + */ +class WaitCancel : public BtCancelActionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::FollowPathAction + * @param xml_tag_name Name for the XML tag for this node + * @param action_name Action name this node creates a client for + * @param conf BT node configuration + */ + WaitCancel( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf); + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ + static BT::PortsList providedPorts() + { + return providedBasicPorts( + { + }); + } +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__WAIT_CANCEL_NODE_HPP_ diff --git a/nav2_behavior_tree/plugins/action/wait_cancel_node.cpp b/nav2_behavior_tree/plugins/action/wait_cancel_node.cpp new file mode 100644 index 0000000000..9365b7e06f --- /dev/null +++ b/nav2_behavior_tree/plugins/action/wait_cancel_node.cpp @@ -0,0 +1,47 @@ +// Copyright (c) 2022 Neobotix GmbH +// +// 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 "std_msgs/msg/string.hpp" + +#include "nav2_behavior_tree/plugins/action/wait_cancel_node.hpp" + +namespace nav2_behavior_tree +{ + +WaitCancel::WaitCancel( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: BtCancelActionNode(xml_tag_name, action_name, conf) +{ +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "wait", config); + }; + + factory.registerBuilder( + "CancelWait", builder); +} diff --git a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt index a2f1d4cd1d..f74f897fbb 100644 --- a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt @@ -19,6 +19,10 @@ ament_add_gtest(test_action_controller_cancel_action test_controller_cancel_node target_link_libraries(test_action_controller_cancel_action nav2_controller_cancel_bt_node) ament_target_dependencies(test_action_controller_cancel_action ${dependencies}) +ament_add_gtest(test_action_wait_cancel_action test_wait_cancel_node.cpp) +target_link_libraries(test_action_wait_cancel_action nav2_wait_cancel_bt_node) +ament_target_dependencies(test_action_wait_cancel_action ${dependencies}) + ament_add_gtest(test_action_clear_costmap_service test_clear_costmap_service.cpp) target_link_libraries(test_action_clear_costmap_service nav2_clear_costmap_service_bt_node) ament_target_dependencies(test_action_clear_costmap_service ${dependencies}) diff --git a/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp new file mode 100644 index 0000000000..9e72f1413c --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp @@ -0,0 +1,171 @@ +// Copyright (c) 2022 Neobotix GmbH +// +// 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 "behaviortree_cpp_v3/bt_factory.h" + +#include "../../test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/wait_cancel_node.hpp" +#include "lifecycle_msgs/srv/change_state.hpp" + +class CancelWaitServer : public TestActionServer +{ +public: + CancelWaitServer() + : TestActionServer("wait") + {} + +protected: + void execute( + const typename std::shared_ptr> + goal_handle) + { + while (!goal_handle->is_canceling()) { + // waiting here until goal cancels + std::this_thread::sleep_for(std::chrono::milliseconds(15)); + } + } +}; + +class CancelWaitActionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("cancel_wait_action_test_fixture"); + factory_ = std::make_shared(); + + 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", + node_); + config_->blackboard->set( + "server_timeout", + std::chrono::milliseconds(20)); + config_->blackboard->set( + "bt_loop_duration", + std::chrono::milliseconds(10)); + client_ = rclcpp_action::create_client( + node_, "wait"); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "wait", config); + }; + + factory_->registerBuilder("CancelWait", builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + action_server_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + + static std::shared_ptr action_server_; + static std::shared_ptr> client_; + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr CancelWaitActionTestFixture::node_ = nullptr; +std::shared_ptr +CancelWaitActionTestFixture::action_server_ = nullptr; +std::shared_ptr> +CancelWaitActionTestFixture::client_ = nullptr; + +BT::NodeConfiguration * CancelWaitActionTestFixture::config_ = nullptr; +std::shared_ptr +CancelWaitActionTestFixture::factory_ = nullptr; +std::shared_ptr CancelWaitActionTestFixture::tree_ = nullptr; + +TEST_F(CancelWaitActionTestFixture, test_ports) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + + // Creating a dummy goal_msg + auto goal_msg = nav2_msgs::action::Wait::Goal(); + + // Setting a waiting time for 5 Seconds. + goal_msg.time.sec = 5; + + // Waiting for server and sending a goal + client_->wait_for_action_server(); + client_->async_send_goal(goal_msg, send_goal_options); + + // Adding a sleep so that the goal is indeed older than 10ms as described in our abstract class + std::this_thread::sleep_for(std::chrono::milliseconds(15)); + + // Executing tick + tree_->rootNode()->executeTick(); + + // BT node should return success, once when the goal is cancelled + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + + // Adding another test case to check if the goal is infact cancelling + EXPECT_EQ(action_server_->isGoalCancelled(), true); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize action server and spin on new thread + CancelWaitActionTestFixture::action_server_ = std::make_shared(); + std::thread server_thread([]() { + rclcpp::spin(CancelWaitActionTestFixture::action_server_); + }); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + server_thread.join(); + + return all_successful; +} From d24b23c53df53c7ea355f71d56bd73ecaf31d114 Mon Sep 17 00:00:00 2001 From: Pradheep-office Date: Mon, 21 Mar 2022 11:36:10 +0100 Subject: [PATCH 2/6] Adding cancel for spining recovery --- nav2_behavior_tree/CMakeLists.txt | 3 + .../plugins/action/spin_cancel_node.hpp | 59 ++++++ .../plugins/action/wait_cancel_node.hpp | 2 +- .../plugins/action/spin_cancel_node.cpp | 47 +++++ .../test/plugins/action/CMakeLists.txt | 4 + .../plugins/action/test_spin_cancel_node.cpp | 171 ++++++++++++++++++ 6 files changed, 285 insertions(+), 1 deletion(-) create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_cancel_node.hpp create mode 100644 nav2_behavior_tree/plugins/action/spin_cancel_node.cpp create mode 100644 nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 6a63bdd2e8..87db5cb708 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -63,6 +63,9 @@ list(APPEND plugin_libs nav2_controller_cancel_bt_node) add_library(nav2_wait_cancel_bt_node SHARED plugins/action/wait_cancel_node.cpp) list(APPEND plugin_libs nav2_wait_cancel_bt_node) +add_library(nav2_spin_cancel_bt_node SHARED plugins/action/spin_cancel_node.cpp) +list(APPEND plugin_libs nav2_spin_cancel_bt_node) + add_library(nav2_smooth_path_action_bt_node SHARED plugins/action/smooth_path_action.cpp) list(APPEND plugin_libs nav2_smooth_path_action_bt_node) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_cancel_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_cancel_node.hpp new file mode 100644 index 0000000000..0e747fa70d --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_cancel_node.hpp @@ -0,0 +1,59 @@ +// Copyright (c) 2022 Neobotix GmbH +// +// 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__ACTION__SPIN_CANCEL_NODE_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SPIN_CANCEL_NODE_HPP_ + +#include +#include + +#include "nav2_msgs/action/spin.hpp" + +#include "nav2_behavior_tree/bt_cancel_action_node.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::Wait + */ +class SpinCancel : public BtCancelActionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::WaitAction + * @param xml_tag_name Name for the XML tag for this node + * @param action_name Action name this node creates a client for + * @param conf BT node configuration + */ + SpinCancel( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf); + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ + static BT::PortsList providedPorts() + { + return providedBasicPorts( + { + }); + } +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SPIN_CANCEL_NODE_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_cancel_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_cancel_node.hpp index e0815c2dbb..1545e9da2e 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_cancel_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_cancel_node.hpp @@ -32,7 +32,7 @@ class WaitCancel : public BtCancelActionNode { public: /** - * @brief A constructor for nav2_behavior_tree::FollowPathAction + * @brief A constructor for nav2_behavior_tree::WaitAction * @param xml_tag_name Name for the XML tag for this node * @param action_name Action name this node creates a client for * @param conf BT node configuration diff --git a/nav2_behavior_tree/plugins/action/spin_cancel_node.cpp b/nav2_behavior_tree/plugins/action/spin_cancel_node.cpp new file mode 100644 index 0000000000..62ddbc4719 --- /dev/null +++ b/nav2_behavior_tree/plugins/action/spin_cancel_node.cpp @@ -0,0 +1,47 @@ +// Copyright (c) 2022 Neobotix GmbH +// +// 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 "std_msgs/msg/string.hpp" + +#include "nav2_behavior_tree/plugins/action/spin_cancel_node.hpp" + +namespace nav2_behavior_tree +{ + +SpinCancel::SpinCancel( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: BtCancelActionNode(xml_tag_name, action_name, conf) +{ +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "spin", config); + }; + + factory.registerBuilder( + "CancelSpin", builder); +} diff --git a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt index f74f897fbb..a26a6a4b4a 100644 --- a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt @@ -23,6 +23,10 @@ ament_add_gtest(test_action_wait_cancel_action test_wait_cancel_node.cpp) target_link_libraries(test_action_wait_cancel_action nav2_wait_cancel_bt_node) ament_target_dependencies(test_action_wait_cancel_action ${dependencies}) +ament_add_gtest(test_action_spin_cancel_action test_spin_cancel_node.cpp) +target_link_libraries(test_action_spin_cancel_action nav2_spin_cancel_bt_node) +ament_target_dependencies(test_action_spin_cancel_action ${dependencies}) + ament_add_gtest(test_action_clear_costmap_service test_clear_costmap_service.cpp) target_link_libraries(test_action_clear_costmap_service nav2_clear_costmap_service_bt_node) ament_target_dependencies(test_action_clear_costmap_service ${dependencies}) diff --git a/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp new file mode 100644 index 0000000000..7909067381 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp @@ -0,0 +1,171 @@ +// Copyright (c) 2022 Neobotix GmbH +// +// 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 "behaviortree_cpp_v3/bt_factory.h" + +#include "../../test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/spin_cancel_node.hpp" +#include "lifecycle_msgs/srv/change_state.hpp" + +class CancelSpinServer : public TestActionServer +{ +public: + CancelSpinServer() + : TestActionServer("spin") + {} + +protected: + void execute( + const typename std::shared_ptr> + goal_handle) + { + while (!goal_handle->is_canceling()) { + // Spining here until goal cancels + std::this_thread::sleep_for(std::chrono::milliseconds(15)); + } + } +}; + +class CancelSpinActionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("cancel_spin_action_test_fixture"); + factory_ = std::make_shared(); + + 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", + node_); + config_->blackboard->set( + "server_timeout", + std::chrono::milliseconds(20)); + config_->blackboard->set( + "bt_loop_duration", + std::chrono::milliseconds(10)); + client_ = rclcpp_action::create_client( + node_, "spin"); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "spin", config); + }; + + factory_->registerBuilder("CancelSpin", builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + action_server_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + + static std::shared_ptr action_server_; + static std::shared_ptr> client_; + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr CancelSpinActionTestFixture::node_ = nullptr; +std::shared_ptr +CancelSpinActionTestFixture::action_server_ = nullptr; +std::shared_ptr> +CancelSpinActionTestFixture::client_ = nullptr; + +BT::NodeConfiguration * CancelSpinActionTestFixture::config_ = nullptr; +std::shared_ptr +CancelSpinActionTestFixture::factory_ = nullptr; +std::shared_ptr CancelSpinActionTestFixture::tree_ = nullptr; + +TEST_F(CancelSpinActionTestFixture, test_ports) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + + // Creating a dummy goal_msg + auto goal_msg = nav2_msgs::action::Spin::Goal(); + + // Setting target yaw + goal_msg.target_yaw = 1.57; + + // Spining for server and sending a goal + client_->wait_for_action_server(); + client_->async_send_goal(goal_msg, send_goal_options); + + // Adding a sleep so that the goal is indeed older than 10ms as described in our abstract class + std::this_thread::sleep_for(std::chrono::milliseconds(15)); + + // Executing tick + tree_->rootNode()->executeTick(); + + // BT node should return success, once when the goal is cancelled + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + + // Adding another test case to check if the goal is infact cancelling + EXPECT_EQ(action_server_->isGoalCancelled(), true); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize action server and spin on new thread + CancelSpinActionTestFixture::action_server_ = std::make_shared(); + std::thread server_thread([]() { + rclcpp::spin(CancelSpinActionTestFixture::action_server_); + }); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + server_thread.join(); + + return all_successful; +} From bd198ce8fa02abdace52e396e7082c2609321575 Mon Sep 17 00:00:00 2001 From: Pradheep-office Date: Mon, 21 Mar 2022 11:54:40 +0100 Subject: [PATCH 3/6] adding BT for cancelling the backup behavior --- nav2_behavior_tree/CMakeLists.txt | 3 + .../plugins/action/back_up_cancel_node.hpp | 59 ++++++ .../plugins/action/back_up_cancel_node.cpp | 47 +++++ .../test/plugins/action/CMakeLists.txt | 4 + .../action/test_back_up_cancel_node.cpp | 171 ++++++++++++++++++ 5 files changed, 284 insertions(+) create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_cancel_node.hpp create mode 100644 nav2_behavior_tree/plugins/action/back_up_cancel_node.cpp create mode 100644 nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 87db5cb708..8e4b119536 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -66,6 +66,9 @@ list(APPEND plugin_libs nav2_wait_cancel_bt_node) add_library(nav2_spin_cancel_bt_node SHARED plugins/action/spin_cancel_node.cpp) list(APPEND plugin_libs nav2_spin_cancel_bt_node) +add_library(nav2_back_up_cancel_bt_node SHARED plugins/action/back_up_cancel_node.cpp) +list(APPEND plugin_libs nav2_back_up_cancel_bt_node) + add_library(nav2_smooth_path_action_bt_node SHARED plugins/action/smooth_path_action.cpp) list(APPEND plugin_libs nav2_smooth_path_action_bt_node) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_cancel_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_cancel_node.hpp new file mode 100644 index 0000000000..d59bbff5a5 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_cancel_node.hpp @@ -0,0 +1,59 @@ +// Copyright (c) 2022 Neobotix GmbH +// +// 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__ACTION__BACK_UP_CANCEL_NODE_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__BACK_UP_CANCEL_NODE_HPP_ + +#include +#include + +#include "nav2_msgs/action/back_up.hpp" + +#include "nav2_behavior_tree/bt_cancel_action_node.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::BackUp + */ +class BackUpCancel : public BtCancelActionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::BackUpAction + * @param xml_tag_name Name for the XML tag for this node + * @param action_name Action name this node creates a client for + * @param conf BT node configuration + */ + BackUpCancel( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf); + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ + static BT::PortsList providedPorts() + { + return providedBasicPorts( + { + }); + } +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__BACK_UP_CANCEL_NODE_HPP_ diff --git a/nav2_behavior_tree/plugins/action/back_up_cancel_node.cpp b/nav2_behavior_tree/plugins/action/back_up_cancel_node.cpp new file mode 100644 index 0000000000..3baa1ffb1e --- /dev/null +++ b/nav2_behavior_tree/plugins/action/back_up_cancel_node.cpp @@ -0,0 +1,47 @@ +// Copyright (c) 2022 Neobotix GmbH +// +// 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 "std_msgs/msg/string.hpp" + +#include "nav2_behavior_tree/plugins/action/back_up_cancel_node.hpp" + +namespace nav2_behavior_tree +{ + +BackUpCancel::BackUpCancel( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: BtCancelActionNode(xml_tag_name, action_name, conf) +{ +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "backup", config); + }; + + factory.registerBuilder( + "CancelBackUp", builder); +} diff --git a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt index a26a6a4b4a..e719035a6a 100644 --- a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt @@ -27,6 +27,10 @@ ament_add_gtest(test_action_spin_cancel_action test_spin_cancel_node.cpp) target_link_libraries(test_action_spin_cancel_action nav2_spin_cancel_bt_node) ament_target_dependencies(test_action_spin_cancel_action ${dependencies}) +ament_add_gtest(test_action_back_up_cancel_action test_back_up_cancel_node.cpp) +target_link_libraries(test_action_back_up_cancel_action nav2_back_up_cancel_bt_node) +ament_target_dependencies(test_action_back_up_cancel_action ${dependencies}) + ament_add_gtest(test_action_clear_costmap_service test_clear_costmap_service.cpp) target_link_libraries(test_action_clear_costmap_service nav2_clear_costmap_service_bt_node) ament_target_dependencies(test_action_clear_costmap_service ${dependencies}) diff --git a/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp new file mode 100644 index 0000000000..5bef6868e5 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp @@ -0,0 +1,171 @@ +// Copyright (c) 2022 Neobotix GmbH +// +// 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 "behaviortree_cpp_v3/bt_factory.h" + +#include "../../test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/back_up_cancel_node.hpp" +#include "lifecycle_msgs/srv/change_state.hpp" + +class CancelBackUpServer : public TestActionServer +{ +public: + CancelBackUpServer() + : TestActionServer("back_up") + {} + +protected: + void execute( + const typename std::shared_ptr> + goal_handle) + { + while (!goal_handle->is_canceling()) { + // BackUping here until goal cancels + std::this_thread::sleep_for(std::chrono::milliseconds(15)); + } + } +}; + +class CancelBackUpActionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("cancel_back_up_action_test_fixture"); + factory_ = std::make_shared(); + + 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", + node_); + config_->blackboard->set( + "server_timeout", + std::chrono::milliseconds(20)); + config_->blackboard->set( + "bt_loop_duration", + std::chrono::milliseconds(10)); + client_ = rclcpp_action::create_client( + node_, "back_up"); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "back_up", config); + }; + + factory_->registerBuilder("CancelBackUp", builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + action_server_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + + static std::shared_ptr action_server_; + static std::shared_ptr> client_; + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr CancelBackUpActionTestFixture::node_ = nullptr; +std::shared_ptr +CancelBackUpActionTestFixture::action_server_ = nullptr; +std::shared_ptr> +CancelBackUpActionTestFixture::client_ = nullptr; + +BT::NodeConfiguration * CancelBackUpActionTestFixture::config_ = nullptr; +std::shared_ptr +CancelBackUpActionTestFixture::factory_ = nullptr; +std::shared_ptr CancelBackUpActionTestFixture::tree_ = nullptr; + +TEST_F(CancelBackUpActionTestFixture, test_ports) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + + // Creating a dummy goal_msg + auto goal_msg = nav2_msgs::action::BackUp::Goal(); + + // Setting target pose + goal_msg.target.x = 0.5; + + // BackUping for server and sending a goal + client_->wait_for_action_server(); + client_->async_send_goal(goal_msg, send_goal_options); + + // Adding a sleep so that the goal is indeed older than 10ms as described in our abstract class + std::this_thread::sleep_for(std::chrono::milliseconds(15)); + + // Executing tick + tree_->rootNode()->executeTick(); + + // BT node should return success, once when the goal is cancelled + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + + // Adding another test case to check if the goal is infact cancelling + EXPECT_EQ(action_server_->isGoalCancelled(), true); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize action server and back_up on new thread + CancelBackUpActionTestFixture::action_server_ = std::make_shared(); + std::thread server_thread([]() { + rclcpp::spin(CancelBackUpActionTestFixture::action_server_); + }); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + server_thread.join(); + + return all_successful; +} From b1f2402abb77ee5dd2b0c0de3b572989e2050d2b Mon Sep 17 00:00:00 2001 From: Pradheep-office Date: Mon, 21 Mar 2022 12:04:55 +0100 Subject: [PATCH 4/6] adding cancel recoveries to param files for wider test coverage --- nav2_bringup/params/nav2_multirobot_params_1.yaml | 3 +++ nav2_bringup/params/nav2_multirobot_params_2.yaml | 3 +++ nav2_bringup/params/nav2_params.yaml | 3 +++ nav2_bt_navigator/src/bt_navigator.cpp | 5 ++++- nav2_system_tests/src/costmap_filters/keepout_params.yaml | 3 +++ .../src/costmap_filters/speed_global_params.yaml | 3 +++ .../src/costmap_filters/speed_local_params.yaml | 3 +++ 7 files changed, 22 insertions(+), 1 deletion(-) diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index 137fd5baf0..6f82ff9da9 100644 --- a/nav2_bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml @@ -95,6 +95,9 @@ bt_navigator: - nav2_controller_selector_bt_node - nav2_goal_checker_selector_bt_node - nav2_controller_cancel_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node bt_navigator_rclcpp_node: ros__parameters: diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml index 5c96d4d41c..2046dd823e 100644 --- a/nav2_bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml @@ -95,6 +95,9 @@ bt_navigator: - nav2_controller_selector_bt_node - nav2_goal_checker_selector_bt_node - nav2_controller_cancel_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node bt_navigator_rclcpp_node: ros__parameters: diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index fe30862e98..e0ae510e76 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -95,6 +95,9 @@ bt_navigator: - nav2_controller_selector_bt_node - nav2_goal_checker_selector_bt_node - nav2_controller_cancel_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_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 dee7542b0d..f6d49d1350 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -68,7 +68,10 @@ BtNavigator::BtNavigator(const rclcpp::NodeOptions & options) "nav2_planner_selector_bt_node", "nav2_controller_selector_bt_node", "nav2_goal_checker_selector_bt_node", - "nav2_controller_cancel_bt_node" + "nav2_controller_cancel_bt_node", + "nav2_wait_cancel_bt_node", + "nav2_spin_cancel_bt_node", + "nav2_back_up_cancel_bt_node" }; declare_parameter("plugin_lib_names", plugin_libs); diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml index 15a1c24df7..21a7ddab0a 100644 --- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -89,6 +89,9 @@ bt_navigator: - nav2_navigate_to_pose_action_bt_node - nav2_remove_passed_goals_action_bt_node - nav2_controller_cancel_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node bt_navigator_rclcpp_node: ros__parameters: 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 ce9d7c0305..def48882c6 100644 --- a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml @@ -90,6 +90,9 @@ bt_navigator: - nav2_navigate_to_pose_action_bt_node - nav2_remove_passed_goals_action_bt_node - nav2_controller_cancel_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node bt_navigator_rclcpp_node: ros__parameters: 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 2adec089ff..b4ead39bc9 100644 --- a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml @@ -90,6 +90,9 @@ bt_navigator: - nav2_navigate_to_pose_action_bt_node - nav2_remove_passed_goals_action_bt_node - nav2_controller_cancel_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node bt_navigator_rclcpp_node: ros__parameters: From f2157bcb1c956d76c3ad8fdb6b0e63413dad861f Mon Sep 17 00:00:00 2001 From: Pradheep-office Date: Tue, 22 Mar 2022 11:39:52 +0100 Subject: [PATCH 5/6] updating BT's XML --- nav2_behavior_tree/nav2_tree_nodes.xml | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 1344ac7375..f15a817ae3 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -16,7 +16,19 @@ Service name to cancel the controller server - + + + Service name to cancel the backup recovery + + + + Service name to cancel the spin recovery + + + + Service name to cancel the wait recovery + + Service name From 1a57e7b26d06c5eade589c139a6db3d13433f783 Mon Sep 17 00:00:00 2001 From: pradheep Date: Tue, 22 Mar 2022 19:28:09 +0100 Subject: [PATCH 6/6] updating XML nodes --- nav2_behavior_tree/nav2_tree_nodes.xml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index f15a817ae3..8a5e81a830 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -15,18 +15,22 @@ Service name to cancel the controller server + Server timeout Service name to cancel the backup recovery + Server timeout Service name to cancel the spin recovery + Server timeout Service name to cancel the wait recovery + Server timeout