diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index c168912..045cc48 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -29,20 +29,56 @@ namespace executors using rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor; using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor; -template -std::shared_future & +/// Return codes to be used with spin_until_future_complete. +/** + * SUCCESS: The future is complete and can be accessed with "get" without blocking. + * INTERRUPTED: The future is not complete, spinning was interrupted by Ctrl-C or another error. + * TIMEOUT: Spinning timed out. + */ +enum FutureReturnCode {SUCCESS, INTERRUPTED, TIMEOUT}; + +/// Spin (blocking) until the future is complete, until the function times out (if applicable), +/// or until rclcpp is interrupted. +/** + * \param[in] executor The executor which will spin the node. + * \param[in] node_ptr The node to spin. + * \param[in] future The future to wait on. If SUCCESS, the future is safe to access after this function + * \param[in] timeout Optional timeout parameter, which gets passed to Executor::spin_node_once. + -1 is block forever, 0 is non-blocking. + If the time spent inside the blocking loop exceeds this timeout, return a TIMEOUT return code. + * \return The return code, one of SUCCESS, INTERRUPTED, or TIMEOUT. + */ +template +FutureReturnCode spin_node_until_future_complete( rclcpp::executor::Executor & executor, rclcpp::node::Node::SharedPtr node_ptr, - std::shared_future & future) + std::shared_future & future, + std::chrono::duration timeout = std::chrono::duration(-1)) { - std::future_status status; // TODO(wjwwood): does not work recursively right, can't call spin_node_until_future_complete // inside a callback executed by an executor. - do { - executor.spin_node_once(node_ptr); + + // Check the future before entering the while loop. + // If the future is already complete, don't try to spin. + std::future_status status = future.wait_for(std::chrono::seconds(0)); + + auto start_time = std::chrono::system_clock::now(); + + while (status != std::future_status::ready && rclcpp::utilities::ok()) { + executor.spin_node_once(node_ptr, timeout); + if (timeout.count() >= 0) { + if (start_time + timeout < std::chrono::system_clock::now()) { + return TIMEOUT; + } + } status = future.wait_for(std::chrono::seconds(0)); - } while (status != std::future_status::ready && rclcpp::utilities::ok()); - return future; + } + + // If the future completed, and we weren't interrupted by ctrl-C, return the response + if (status == std::future_status::ready) { + return FutureReturnCode::SUCCESS; + } + return FutureReturnCode::INTERRUPTED; } } // namespace executors diff --git a/rclcpp/include/rclcpp/parameter_client.hpp b/rclcpp/include/rclcpp/parameter_client.hpp index fe27e3e..03bf28e 100644 --- a/rclcpp/include/rclcpp/parameter_client.hpp +++ b/rclcpp/include/rclcpp/parameter_client.hpp @@ -274,28 +274,53 @@ public: get_parameters(std::vector parameter_names) { auto f = async_parameters_client_->get_parameters(parameter_names); - return rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f).get(); + if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) == + rclcpp::executors::FutureReturnCode::SUCCESS) + { + return f.get(); + } + // Return an empty vector if unsuccessful + return std::vector(); } std::vector get_parameter_types(std::vector parameter_names) { auto f = async_parameters_client_->get_parameter_types(parameter_names); - return rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f).get(); + + if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) == + rclcpp::executors::FutureReturnCode::SUCCESS) + { + return f.get(); + } + return std::vector(); } std::vector set_parameters(std::vector parameters) { auto f = async_parameters_client_->set_parameters(parameters); - return rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f).get(); + + if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) == + rclcpp::executors::FutureReturnCode::SUCCESS) + { + return f.get(); + } + return std::vector(); } rcl_interfaces::msg::SetParametersResult set_parameters_atomically(std::vector parameters) { auto f = async_parameters_client_->set_parameters_atomically(parameters); - return rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f).get(); + + if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) == + rclcpp::executors::FutureReturnCode::SUCCESS) + { + return f.get(); + } + + throw std::runtime_error("Unable to get result of set parameters service call."); } rcl_interfaces::msg::ListParametersResult @@ -304,7 +329,14 @@ public: uint64_t depth) { auto f = async_parameters_client_->list_parameters(parameter_prefixes, depth); - return rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f).get(); + + if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) == + rclcpp::executors::FutureReturnCode::SUCCESS) + { + return f.get(); + } + + throw std::runtime_error("Unable to get result of list parameters service call."); } template diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index b63457a..623bf75 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -98,14 +98,13 @@ void spin(Node::SharedPtr node_ptr) } template -std::shared_future & +rclcpp::executors::FutureReturnCode spin_until_future_complete( Node::SharedPtr node_ptr, std::shared_future & future) { rclcpp::executors::SingleThreadedExecutor executor; - rclcpp::executors::spin_node_until_future_complete( + return rclcpp::executors::spin_node_until_future_complete( executor, node_ptr, future); - return future; } } /* namespace rclcpp */