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;
}
~shared_fence() noexcept {
if (is_valid()) {
// Destroying while the vk_fence might still be in use will cause race conditions
assert(is_future_signalled());
future.wait();
}
}
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);
}
static void run(std::shared_ptr<queue_type> const& queue,
std::shared_ptr<boost::asynchronous::lockfree_queue<boost::asynchronous::any_callable> > const& private_queue,
std::shared_ptr<diag_type> diagnostics,
std::shared_future<boost::thread*> self,
std::weak_ptr<this_type> this_,
size_t index)
{
boost::thread* t = self.get();
boost::asynchronous::detail::single_queue_scheduler_policy<Q>::m_self_thread.reset(new thread_ptr_wrapper(t));
// thread scheduler => tss
boost::asynchronous::any_weak_scheduler<job_type> self_as_weak = boost::asynchronous::detail::lockable_weak_scheduler<this_type>(this_);
boost::asynchronous::get_thread_scheduler<job_type>(self_as_weak,true);
std::list<boost::asynchronous::any_continuation>& waiting =
boost::asynchronous::get_continuations(std::list<boost::asynchronous::any_continuation>(),true);
CPULoad cpu_load;
while(true)
{
try
{
{
bool popped = execute_one_job(queue,cpu_load,diagnostics,waiting,index);
if (!popped)
{
cpu_load.loop_done_no_job();
// nothing for us to do, give up our time slice
boost::this_thread::yield();
}
// check for shutdown
boost::asynchronous::any_callable djob;
popped = private_queue->try_pop(djob);
if (popped)
{
djob();
}
} // job destroyed (for destruction useful)
// check if we got an interruption job
boost::this_thread::interruption_point();
}
catch(boost::asynchronous::detail::shutdown_exception&)
{
// we are done, execute jobs posted short before to the end, then shutdown
while(execute_one_job(queue,cpu_load,diagnostics,waiting,index));
delete boost::asynchronous::detail::single_queue_scheduler_policy<Q>::m_self_thread.release();
return;
}
catch(boost::thread_interrupted&)
{
// task interrupted, no problem, just continue
}
catch(std::exception&)
{
// TODO, user-defined error
}
}
}
int factorial2(std::shared_future<int> f)
{
int n = f.get();
int result = 1;
for (int i = 1; i <= n; ++i)
result *= i;
return result;
}
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;
}
//Get value and sync
const T& get() {
return _future.get();
};
/**
* @brief Checks if the fence future is valid
*/
auto is_valid() const {
return future.valid();
}
bool is_future_signalled() const {
return future.wait_for(std::chrono::nanoseconds(0)) == std::future_status::ready;
}
/**
* @brief Wait for the fence to be signaled
*/
void wait_idle() const {
future.wait();
f.wait_idle();
}
void get_wait(typename std::enable_if<std::is_void<S>::value>::type* = nullptr) const {
future.get();
f.wait_idle();
}
decltype(auto) get_wait(typename std::enable_if<!std::is_void<S>::value>::type* = nullptr) const {
auto& val = future.get();
f.wait_idle();
return val;
}
