FutureReturnCode
spin_node_until_future_complete(
rclcpp::executor::Executor & executor, rclcpp::node::Node::SharedPtr node_ptr,
std::shared_future<ResponseT> & future,
std::chrono::duration<int64_t, TimeT> timeout = std::chrono::duration<int64_t, TimeT>(-1))
{
// TODO(wjwwood): does not work recursively right, can't call spin_node_until_future_complete
// inside a callback executed by an executor.
// 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));
}
// 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;
}
FutureReturnCode
spin_until_future_complete(
std::shared_future<ResponseT> & future,
std::chrono::duration<int64_t, TimeT> timeout = std::chrono::duration<int64_t, TimeT>(-1))
{
// TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete
// inside a callback executed by an executor.
// 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));
if (status == std::future_status::ready) {
return FutureReturnCode::SUCCESS;
}
auto end_time = std::chrono::steady_clock::now();
std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
timeout);
if (timeout_ns > std::chrono::nanoseconds::zero()) {
end_time += timeout_ns;
}
std::chrono::nanoseconds timeout_left = timeout_ns;
while (rclcpp::utilities::ok()) {
// Do one item of work.
spin_once(timeout_left);
// Check if the future is set, return SUCCESS if it is.
status = future.wait_for(std::chrono::seconds(0));
if (status == std::future_status::ready) {
return FutureReturnCode::SUCCESS;
}
// If the original timeout is < 0, then this is blocking, never TIMEOUT.
if (timeout_ns < std::chrono::nanoseconds::zero()) {
continue;
}
// Otherwise check if we still have time to wait, return TIMEOUT if not.
auto now = std::chrono::steady_clock::now();
if (now >= end_time) {
return FutureReturnCode::TIMEOUT;
}
// Subtract the elapsed time from the original timeout.
timeout_left = std::chrono::duration_cast<std::chrono::nanoseconds>(end_time - now);
}
// The future did not complete before ok() returned false, return INTERRUPTED.
return FutureReturnCode::INTERRUPTED;
}
void dual_spin_until_future_complete(std::shared_future<FutureT> & future)
{
std::future_status status;
do {
server_executor.spin_some();
client_executor.spin_some();
status = future.wait_for(std::chrono::seconds(0));
} while (std::future_status::ready != status);
}
bool wait_for(const std::chrono::duration<Rep, Period>& timeout_duration) const {
const auto start = std::chrono::high_resolution_clock::now();
if (future.wait_for(timeout_duration) == std::future_status::ready) {
const auto end = std::chrono::high_resolution_clock::now();
auto elapsed = std::chrono::duration_cast<decltype(timeout_duration)>(end - start);
auto timeout_duration_left = timeout_duration - elapsed;
return f.wait_idle(timeout_duration_left);
}
return false;
}
bool is_future_signalled() const {
return future.wait_for(std::chrono::nanoseconds(0)) == std::future_status::ready;
}
