Add timeouts to all spin_until_future_complete calls (#1998)

* Add timeouts to all spin_until_future_complete calls

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Update default timeout value

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

@@ -38,6 +38,11 @@ public:
   {
     node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
 
+    // Get the required items from the blackboard
+    server_timeout_ =
+      config().blackboard->get<std::chrono::milliseconds>("server_timeout");
+    getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
+
     // Initialize the input and output messages
     goal_ = typename ActionT::Goal();
     result_ = typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult();
@@ -179,7 +184,7 @@ public:
   {
     if (should_cancel_goal()) {
       auto future_cancel = action_client_->async_cancel_goal(goal_handle_);
-      if (rclcpp::spin_until_future_complete(node_, future_cancel) !=
+      if (rclcpp::spin_until_future_complete(node_, future_cancel, server_timeout_) !=
         rclcpp::FutureReturnCode::SUCCESS)
       {
         RCLCPP_ERROR(
@@ -225,7 +230,7 @@ protected:
 
     auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options);
 
-    if (rclcpp::spin_until_future_complete(node_, future_goal_handle) !=
+    if (rclcpp::spin_until_future_complete(node_, future_goal_handle, server_timeout_) !=
       rclcpp::FutureReturnCode::SUCCESS)
     {
       throw std::runtime_error("send_goal failed");
@@ -257,6 +262,10 @@ protected:
 
   // The node that will be used for any ROS operations
   rclcpp::Node::SharedPtr node_;
+
+  // The timeout value while waiting for response from a server when a
+  // new action goal is sent or canceled
+  std::chrono::milliseconds server_timeout_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_bringup/nav2_gazebo_spawner/nav2_gazebo_spawner/nav2_gazebo_spawner.py

@@ -36,6 +36,8 @@ def main():
                         help='the y component of the initial position [meters]')
     parser.add_argument('-z', type=float, default=0,
                         help='the z component of the initial position [meters]')
+    parser.add_argument('-k', '--timeout', type=float, default=10.0,
+                        help="Seconds to wait. Block until the future is complete if negative. Don't wait if 0.")
 
     group = parser.add_mutually_exclusive_group(required=True)
     group.add_argument('-t', '--turtlebot_type', type=str,
@@ -97,7 +99,7 @@ def main():
 
     node.get_logger().info('Sending service request to `/spawn_entity`')
     future = client.call_async(request)
-    rclpy.spin_until_future_complete(node, future)
+    rclpy.spin_until_future_complete(node, future, args.timeout)
     if future.result() is not None:
         print('response: %r' % future.result())
     else:

nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp

@@ -52,27 +52,27 @@ public:
    * @brief Make start up service call
    * @return true or false
    */
-  bool startup();
+  bool startup(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
   /**
    * @brief Make shutdown service call
    * @return true or false
    */
-  bool shutdown();
+  bool shutdown(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
   /**
    * @brief Make pause service call
    * @return true or false
    */
-  bool pause();
+  bool pause(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
   /**
    * @brief Make resume service call
    * @return true or false
    */
-  bool resume();
+  bool resume(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
   /**
    * @brief Make reset service call
    * @return true or false
    */
-  bool reset();
+  bool reset(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
   /**
    * @brief Check if lifecycle node manager server is active
    * @return ACTIVE or INACTIVE or TIMEOUT
@@ -103,7 +103,9 @@ protected:
    * @brief A generic method used to call startup, shutdown, etc.
    * @param command
    */
-  bool callService(uint8_t command);
+  bool callService(
+    uint8_t command,
+    const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
   // The node to use for the service call
   rclcpp::Node::SharedPtr node_;

nav2_lifecycle_manager/src/lifecycle_manager_client.cpp

@@ -40,33 +40,33 @@ LifecycleManagerClient::LifecycleManagerClient(const std::string & name)
 }
 
 bool
-LifecycleManagerClient::startup()
+LifecycleManagerClient::startup(const std::chrono::nanoseconds timeout)
 {
-  return callService(ManageLifecycleNodes::Request::STARTUP);
+  return callService(ManageLifecycleNodes::Request::STARTUP, timeout);
 }
 
 bool
-LifecycleManagerClient::shutdown()
+LifecycleManagerClient::shutdown(const std::chrono::nanoseconds timeout)
 {
-  return callService(ManageLifecycleNodes::Request::SHUTDOWN);
+  return callService(ManageLifecycleNodes::Request::SHUTDOWN, timeout);
 }
 
 bool
-LifecycleManagerClient::pause()
+LifecycleManagerClient::pause(const std::chrono::nanoseconds timeout)
 {
-  return callService(ManageLifecycleNodes::Request::PAUSE);
+  return callService(ManageLifecycleNodes::Request::PAUSE, timeout);
 }
 
 bool
-LifecycleManagerClient::resume()
+LifecycleManagerClient::resume(const std::chrono::nanoseconds timeout)
 {
-  return callService(ManageLifecycleNodes::Request::RESUME);
+  return callService(ManageLifecycleNodes::Request::RESUME, timeout);
 }
 
 bool
-LifecycleManagerClient::reset()
+LifecycleManagerClient::reset(const std::chrono::nanoseconds timeout)
 {
-  return callService(ManageLifecycleNodes::Request::RESET);
+  return callService(ManageLifecycleNodes::Request::RESET, timeout);
 }
 
 SystemStatus
@@ -101,7 +101,7 @@ LifecycleManagerClient::is_active(const std::chrono::nanoseconds timeout)
 }
 
 bool
-LifecycleManagerClient::callService(uint8_t command)
+LifecycleManagerClient::callService(uint8_t command, const std::chrono::nanoseconds timeout)
 {
   auto request = std::make_shared<ManageLifecycleNodes::Request>();
   request->command = command;
@@ -122,7 +122,13 @@ LifecycleManagerClient::callService(uint8_t command)
     node_->get_logger(), "Sending %s request",
     manage_service_name_.c_str());
   auto future_result = manager_client_->async_send_request(request);
-  rclcpp::spin_until_future_complete(node_, future_result);
+
+  if (rclcpp::spin_until_future_complete(node_, future_result, timeout) !=
+    rclcpp::FutureReturnCode::SUCCESS)
+  {
+    return false;
+  }
+
   return future_result.get()->success;
 }
 

nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp

@@ -84,6 +84,9 @@ private:
   // The (non-spinning) client node used to invoke the action client
   rclcpp::Node::SharedPtr client_node_;
 
+  // Timeout value when waiting for action servers to respnd
+  std::chrono::milliseconds server_timeout_;
+
   // A timer used to check on the completion status of the action
   QBasicTimer timer_;
 

nav2_rviz_plugins/src/nav2_panel.cpp

@@ -34,7 +34,9 @@ using nav2_util::geometry_utils::orientationAroundZAxis;
 GoalPoseUpdater GoalUpdater;
 
 Nav2Panel::Nav2Panel(QWidget * parent)
-: Panel(parent), client_nav_("lifecycle_manager_navigation"),
+: Panel(parent),
+  server_timeout_(10),
+  client_nav_("lifecycle_manager_navigation"),
   client_loc_("lifecycle_manager_localization")
 {
   // Create the control button and its tooltip
@@ -316,12 +318,12 @@ Nav2Panel::onPause()
     QtConcurrent::run(
     std::bind(
       &nav2_lifecycle_manager::LifecycleManagerClient::pause,
-      &client_nav_));
+      &client_nav_, std::placeholders::_1), server_timeout_);
   QFuture<void> futureLoc =
     QtConcurrent::run(
     std::bind(
       &nav2_lifecycle_manager::LifecycleManagerClient::pause,
-      &client_loc_));
+      &client_loc_, std::placeholders::_1), server_timeout_);
 }
 
 void
@@ -331,12 +333,12 @@ Nav2Panel::onResume()
     QtConcurrent::run(
     std::bind(
       &nav2_lifecycle_manager::LifecycleManagerClient::resume,
-      &client_nav_));
+      &client_nav_, std::placeholders::_1), server_timeout_);
   QFuture<void> futureLoc =
     QtConcurrent::run(
     std::bind(
       &nav2_lifecycle_manager::LifecycleManagerClient::resume,
-      &client_loc_));
+      &client_loc_, std::placeholders::_1), server_timeout_);
 }
 
 void
@@ -346,12 +348,12 @@ Nav2Panel::onStartup()
     QtConcurrent::run(
     std::bind(
       &nav2_lifecycle_manager::LifecycleManagerClient::startup,
-      &client_nav_));
+      &client_nav_, std::placeholders::_1), server_timeout_);
   QFuture<void> futureLoc =
     QtConcurrent::run(
     std::bind(
       &nav2_lifecycle_manager::LifecycleManagerClient::startup,
-      &client_loc_));
+      &client_loc_, std::placeholders::_1), server_timeout_);
 }
 
 void
@@ -361,12 +363,12 @@ Nav2Panel::onShutdown()
     QtConcurrent::run(
     std::bind(
       &nav2_lifecycle_manager::LifecycleManagerClient::reset,
-      &client_nav_));
+      &client_nav_, std::placeholders::_1), server_timeout_);
   QFuture<void> futureLoc =
     QtConcurrent::run(
     std::bind(
       &nav2_lifecycle_manager::LifecycleManagerClient::reset,
-      &client_loc_));
+      &client_loc_, std::placeholders::_1), server_timeout_);
   timer_.stop();
 }
 
@@ -409,7 +411,7 @@ Nav2Panel::onCancelButtonPressed()
     auto future_cancel =
       waypoint_follower_action_client_->async_cancel_goal(waypoint_follower_goal_handle_);
 
-    if (rclcpp::spin_until_future_complete(client_node_, future_cancel) !=
+    if (rclcpp::spin_until_future_complete(client_node_, future_cancel, server_timeout_) !=
       rclcpp::FutureReturnCode::SUCCESS)
     {
       RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel waypoint follower");
@@ -418,7 +420,7 @@ Nav2Panel::onCancelButtonPressed()
   } else {
     auto future_cancel = navigation_action_client_->async_cancel_goal(navigation_goal_handle_);
 
-    if (rclcpp::spin_until_future_complete(client_node_, future_cancel) !=
+    if (rclcpp::spin_until_future_complete(client_node_, future_cancel, server_timeout_) !=
       rclcpp::FutureReturnCode::SUCCESS)
     {
       RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel goal");
@@ -522,7 +524,7 @@ Nav2Panel::startWaypointFollowing(std::vector<geometry_msgs::msg::PoseStamped> p
 
   auto future_goal_handle =
     waypoint_follower_action_client_->async_send_goal(waypoint_follower_goal_, send_goal_options);
-  if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle) !=
+  if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle, server_timeout_) !=
     rclcpp::FutureReturnCode::SUCCESS)
   {
     RCLCPP_ERROR(client_node_->get_logger(), "Send goal call failed");
@@ -562,7 +564,7 @@ Nav2Panel::startNavigation(geometry_msgs::msg::PoseStamped pose)
 
   auto future_goal_handle =
     navigation_action_client_->async_send_goal(navigation_goal_, send_goal_options);
-  if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle) !=
+  if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle, server_timeout_) !=
     rclcpp::FutureReturnCode::SUCCESS)
   {
     RCLCPP_ERROR(client_node_->get_logger(), "Send goal call failed");