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 4c4d0bff7a6..eed3626601e 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 @@ -38,6 +38,11 @@ class BtActionNode : public BT::ActionNodeBase { node_ = config().blackboard->get("node"); + // Get the required items from the blackboard + server_timeout_ = + config().blackboard->get("server_timeout"); + getInput("server_timeout", server_timeout_); + // Initialize the input and output messages goal_ = typename ActionT::Goal(); result_ = typename rclcpp_action::ClientGoalHandle::WrappedResult(); @@ -179,7 +184,7 @@ class BtActionNode : public BT::ActionNodeBase { 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 @@ class BtActionNode : public BT::ActionNodeBase 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 @@ class BtActionNode : public BT::ActionNodeBase // 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 diff --git a/nav2_bringup/nav2_gazebo_spawner/nav2_gazebo_spawner/nav2_gazebo_spawner.py b/nav2_bringup/nav2_gazebo_spawner/nav2_gazebo_spawner/nav2_gazebo_spawner.py index 5f4de1d913f..05cc3c6dbc2 100644 --- a/nav2_bringup/nav2_gazebo_spawner/nav2_gazebo_spawner/nav2_gazebo_spawner.py +++ b/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: diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp index 7e25916b8bf..bb597fbd424 100644 --- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp +++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp @@ -52,27 +52,27 @@ class LifecycleManagerClient * @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 @@ class LifecycleManagerClient * @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_; diff --git a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp index 618d318935c..c7931539f7b 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp +++ b/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(); 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; } diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp index fabf1e3170a..772759aa47f 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp @@ -84,6 +84,9 @@ private Q_SLOTS: // 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_; diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp index 5d2b5147fc8..396eafd4fa3 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/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 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 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 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 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 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");