From dab4ddcb39dbeaca1daa8ad384c60c9d3982759a Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 12 Jul 2022 13:24:44 -0700 Subject: [PATCH] adding looping timeout for lifecycle service clients + adding string name of action for BT action nodes (#3071) * adding looping timeout for lifecycle service clients + adding string name of action for BT action nodes * fix linting * remove inline comment * adding goal updated controller node to test --- .../include/nav2_behavior_tree/bt_action_node.hpp | 13 +++++++------ ...vigate_w_replanning_only_if_goal_is_updated.xml | 2 +- .../test/component/test_map_server_node.cpp | 2 +- .../src/behavior_tree/test_behavior_tree_node.cpp | 4 +++- nav2_util/include/nav2_util/service_client.hpp | 9 +++++++++ nav2_util/src/lifecycle_service_client.cpp | 14 ++++++++++++-- 6 files changed, 33 insertions(+), 11 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 8de0b27977..06fc6dd810 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 @@ -17,6 +17,7 @@ #include #include +#include #include "behaviortree_cpp_v3/action_node.h" #include "nav2_util/node_utils.hpp" @@ -26,6 +27,8 @@ namespace nav2_behavior_tree { +using namespace std::chrono_literals; // NOLINT + /** * @brief Abstract class representing an action based BT node * @tparam ActionT Type of action @@ -90,13 +93,11 @@ class BtActionNode : public BT::ActionNodeBase // Make sure the server is actually there before continuing RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str()); - static constexpr std::chrono::milliseconds wait_for_server_timeout = - std::chrono::milliseconds(1000); - if (!action_client_->wait_for_action_server(wait_for_server_timeout)) { + if (!action_client_->wait_for_action_server(1s)) { RCLCPP_ERROR( - node_->get_logger(), "\"%s\" action server not available after waiting for %li ms", - action_name.c_str(), wait_for_server_timeout.count()); - throw std::runtime_error("Action server not available"); + node_->get_logger(), "\"%s\" action server not available after waiting for 1 s", + action_name.c_str()); + throw std::runtime_error(std::string("Action server %s not available", action_name.c_str())); } } 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 index 46d18fe654..3e889440a8 100644 --- 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 @@ -5,7 +5,7 @@ - + diff --git a/nav2_map_server/test/component/test_map_server_node.cpp b/nav2_map_server/test/component/test_map_server_node.cpp index ae3a80982f..f47dfbf718 100644 --- a/nav2_map_server/test/component/test_map_server_node.cpp +++ b/nav2_map_server/test/component/test_map_server_node.cpp @@ -26,7 +26,7 @@ #include "nav2_msgs/srv/load_map.hpp" using namespace std::chrono_literals; using namespace rclcpp; // NOLINT - + #define TEST_DIR TEST_DIRECTORY using std::experimental::filesystem::path; diff --git a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp index 2f76823ae4..b5bb56a6f1 100644 --- a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp +++ b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp @@ -94,7 +94,8 @@ class BehaviorTreeHandler "nav2_wait_cancel_bt_node", "nav2_spin_cancel_bt_node", "nav2_back_up_cancel_bt_node", - "nav2_drive_on_heading_cancel_bt_node" + "nav2_drive_on_heading_cancel_bt_node", + "nav2_goal_updated_controller_bt_node" }; for (const auto & p : plugin_libs) { factory_.registerFromPlugin(BT::SharedLibrary::getOSName(p)); @@ -216,6 +217,7 @@ TEST_F(BehaviorTreeTestFixture, TestBTXMLFiles) if (boost::filesystem::exists(root) && boost::filesystem::is_directory(root)) { for (auto const & entry : boost::filesystem::recursive_directory_iterator(root)) { if (boost::filesystem::is_regular_file(entry) && entry.path().extension() == ".xml") { + std::cout << entry.path().string() << std::endl; EXPECT_EQ(bt_handler->loadBehaviorTree(entry.path().string()), true); } } diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index 4af107abf1..9be5912e8c 100644 --- a/nav2_util/include/nav2_util/service_client.hpp +++ b/nav2_util/include/nav2_util/service_client.hpp @@ -132,6 +132,15 @@ class ServiceClient return client_->wait_for_service(timeout); } + /** + * @brief Gets the service name + * @return string Service name + */ + std::string getServiceName() + { + return service_name_; + } + protected: std::string service_name_; rclcpp::Node::SharedPtr node_; diff --git a/nav2_util/src/lifecycle_service_client.cpp b/nav2_util/src/lifecycle_service_client.cpp index 788c44123a..70656d9f29 100644 --- a/nav2_util/src/lifecycle_service_client.cpp +++ b/nav2_util/src/lifecycle_service_client.cpp @@ -36,7 +36,12 @@ LifecycleServiceClient::LifecycleServiceClient(const string & lifecycle_node_nam get_state_(lifecycle_node_name + "/get_state", node_) { // Block until server is up - get_state_.wait_for_service(); + rclcpp::Rate r(20); + while (!get_state_.wait_for_service(2s)) { + RCLCPP_INFO( + node_->get_logger(), "Waiting for service %s...", get_state_.getServiceName().c_str()); + r.sleep(); + } } LifecycleServiceClient::LifecycleServiceClient( @@ -47,7 +52,12 @@ LifecycleServiceClient::LifecycleServiceClient( get_state_(lifecycle_node_name + "/get_state", node_) { // Block until server is up - get_state_.wait_for_service(); + rclcpp::Rate r(20); + while (!get_state_.wait_for_service(2s)) { + RCLCPP_INFO( + node_->get_logger(), "Waiting for service %s...", get_state_.getServiceName().c_str()); + r.sleep(); + } } bool LifecycleServiceClient::change_state(