diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index 8428aa6..ff3b463 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -15,8 +15,11 @@ #ifndef RCLCPP_RCLCPP_EXECUTORS_HPP_ #define RCLCPP_RCLCPP_EXECUTORS_HPP_ +#include #include #include +#include +#include namespace rclcpp { @@ -26,6 +29,22 @@ namespace executors using rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor; using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor; +template +std::shared_future & +spin_node_until_future_complete( + rclcpp::executor::Executor & executor, rclcpp::node::Node::SharedPtr & node_ptr, + std::shared_future & future) +{ + std::future_status status; + // TODO: does not work recursively right, can't call spin_node_until_future_complete + // inside a callback executed by an executor. + do { + executor.spin_node_some(node_ptr); + status = future.wait_for(std::chrono::seconds(0)); + } while (status != std::future_status::ready && rclcpp::utilities::ok()); + return future; +} + } // namespace executors } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index 040cf38..02d835a 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -76,6 +76,17 @@ void spin(Node::SharedPtr & node_ptr) executor.spin(); } +template +std::shared_future & +spin_until_future_complete( + Node::SharedPtr & node_ptr, std::shared_future & future) +{ + rclcpp::executors::SingleThreadedExecutor executor; + rclcpp::executors::spin_node_until_future_complete( + executor, node_ptr, future); + return future; +} + } /* namespace rclcpp */ #endif /* RCLCPP_RCLCPP_RCLCPP_HPP_ */