adding looping timeout for lifecycle service clients + adding string …

…name of action for BT action nodes (ros-navigation#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
Alisabunina · Jul 12, 2022 · dab4ddc · dab4ddc
1 parent 5e91314
commit dab4ddc
Show file tree

Hide file tree

Showing 6 changed files with 33 additions and 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
@@ -17,6 +17,7 @@
 
 #include <memory>
 #include <string>
+#include <chrono>
 
 #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
@@ -5,7 +5,7 @@
 <root main_tree_to_execute="MainTree">
   <BehaviorTree ID="MainTree">
     <PipelineSequence name="NavigateWithReplanning">
-      <GoalUpdatedController> <!-- only tick child if goal was updated-->
+      <GoalUpdatedController>
         <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
       </GoalUpdatedController>
       <FollowPath path="{path}"  controller_id="FollowPath"/>

diff --git 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
@@ -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
@@ -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
@@ -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(