wait_result.hpp

// Copyright 2020 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
#ifndef RCLCPP__WAIT_RESULT_HPP_
#define RCLCPP__WAIT_RESULT_HPP_
 
#include <cassert>
#include <functional>
#include <iostream>
#include <memory>
#include <stdexcept>
#include <utility>
 
#include "rcl/wait.h"
 
#include "rclcpp/macros.hpp"
#include "rclcpp/wait_result_kind.hpp"
 
#include "rclcpp/client.hpp"
#include "rclcpp/service.hpp"
#include "rclcpp/subscription_base.hpp"
#include "rclcpp/timer.hpp"
 
namespace rclcpp
{
 
// TODO(wjwwood): the union-like design of this class could be replaced with
//   std::variant, when we have access to that...
 
template<class WaitSetT>
class WaitResult final
{
public:
 
  static
  WaitResult
  from_ready_wait_result_kind(WaitSetT & wait_set)
  {
    return WaitResult(WaitResultKind::Ready, wait_set);
  }
 
  static
  WaitResult
  from_timeout_wait_result_kind()
  {
    return WaitResult(WaitResultKind::Timeout);
  }
 
  static
  WaitResult
  from_empty_wait_result_kind()
  {
    return WaitResult(WaitResultKind::Empty);
  }
 
  WaitResultKind
  kind() const
  {
    return wait_result_kind_;
  }
 
 
  const WaitSetT &
  get_wait_set() const
  {
    if (this->kind() != WaitResultKind::Ready) {
      throw std::runtime_error("cannot access wait set when the result was not ready");
    }
    // This should never happen, defensive (and debug mode) check only.
    assert(wait_set_pointer_);
    return *wait_set_pointer_;
  }
 
 
  WaitSetT &
  get_wait_set()
  {
    if (this->kind() != WaitResultKind::Ready) {
      throw std::runtime_error("cannot access wait set when the result was not ready");
    }
    // This should never happen, defensive (and debug mode) check only.
    assert(wait_set_pointer_);
    return *wait_set_pointer_;
  }
 
  WaitResult(WaitResult && other) noexcept
  : wait_result_kind_(other.wait_result_kind_),
    wait_set_pointer_(std::exchange(other.wait_set_pointer_, nullptr))
  {}
 
  ~WaitResult()
  {
    if (wait_set_pointer_) {
      wait_set_pointer_->wait_result_release();
    }
  }
 
 
  std::pair<std::shared_ptr<rclcpp::TimerBase>, size_t>
  peek_next_ready_timer(size_t start_index = 0)
  {
    check_wait_result_dirty();
    auto ret = std::shared_ptr<rclcpp::TimerBase>{nullptr};
    size_t ii = start_index;
    if (this->kind() == WaitResultKind::Ready) {
      auto & wait_set = this->get_wait_set();
      auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set();
      for (; ii < wait_set.size_of_timers(); ++ii) {
        if (rcl_wait_set.timers[ii] != nullptr) {
          ret = wait_set.timers(ii);
          if (ret) {
            break;
          }
        }
      }
    }
    return {ret, ii};
  }
 
 
  void
  clear_timer_with_index(size_t index)
  {
    auto & wait_set = this->get_wait_set();
    auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set();
    if (index >= wait_set.size_of_timers()) {
      throw std::out_of_range("given timer index is out of range");
    }
    rcl_wait_set.timers[index] = nullptr;
  }
 
  std::shared_ptr<rclcpp::SubscriptionBase>
  next_ready_subscription()
  {
    check_wait_result_dirty();
    auto ret = std::shared_ptr<rclcpp::SubscriptionBase>{nullptr};
    if (this->kind() == WaitResultKind::Ready) {
      auto & wait_set = this->get_wait_set();
      auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set();
      for (size_t ii = 0; ii < wait_set.size_of_subscriptions(); ++ii) {
        if (rcl_wait_set.subscriptions[ii] != nullptr) {
          ret = wait_set.subscriptions(ii);
          rcl_wait_set.subscriptions[ii] = nullptr;
          if (ret) {
            break;
          }
        }
      }
    }
    return ret;
  }
 
  std::shared_ptr<rclcpp::ServiceBase>
  next_ready_service()
  {
    check_wait_result_dirty();
    auto ret = std::shared_ptr<rclcpp::ServiceBase>{nullptr};
    if (this->kind() == WaitResultKind::Ready) {
      auto & wait_set = this->get_wait_set();
      auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set();
      for (size_t ii = 0; ii < wait_set.size_of_services(); ++ii) {
        if (rcl_wait_set.services[ii] != nullptr) {
          ret = wait_set.services(ii);
          rcl_wait_set.services[ii] = nullptr;
          if (ret) {
            break;
          }
        }
      }
    }
    return ret;
  }
 
  std::shared_ptr<rclcpp::ClientBase>
  next_ready_client()
  {
    check_wait_result_dirty();
    auto ret = std::shared_ptr<rclcpp::ClientBase>{nullptr};
    if (this->kind() == WaitResultKind::Ready) {
      auto & wait_set = this->get_wait_set();
      auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set();
      for (size_t ii = 0; ii < wait_set.size_of_clients(); ++ii) {
        if (rcl_wait_set.clients[ii] != nullptr) {
          ret = wait_set.clients(ii);
          rcl_wait_set.clients[ii] = nullptr;
          if (ret) {
            break;
          }
        }
      }
    }
    return ret;
  }
 
  std::shared_ptr<rclcpp::Waitable>
  next_ready_waitable()
  {
    check_wait_result_dirty();
    auto waitable = std::shared_ptr<rclcpp::Waitable>{nullptr};
    auto data = std::shared_ptr<void>{nullptr};
 
    if (this->kind() == WaitResultKind::Ready) {
      auto & wait_set = this->get_wait_set();
      auto & rcl_wait_set = wait_set.get_rcl_wait_set();
      while (next_waitable_index_ < wait_set.size_of_waitables()) {
        auto cur_waitable = wait_set.waitables(next_waitable_index_++);
        if (cur_waitable != nullptr && cur_waitable->is_ready(rcl_wait_set)) {
          waitable = cur_waitable;
          break;
        }
      }
    }
 
    return waitable;
  }
 
private:
  RCLCPP_DISABLE_COPY(WaitResult)
 
  explicit WaitResult(WaitResultKind wait_result_kind)
  : wait_result_kind_(wait_result_kind)
  {
    // Should be enforced by the static factory methods on this class.
    assert(WaitResultKind::Ready != wait_result_kind);
  }
 
  WaitResult(WaitResultKind wait_result_kind, WaitSetT & wait_set)
  : wait_result_kind_(wait_result_kind),
    wait_set_pointer_(&wait_set)
  {
    // Should be enforced by the static factory methods on this class.
    assert(WaitResultKind::Ready == wait_result_kind);
    // Secure thread-safety (if provided) and shared ownership (if needed).
    this->get_wait_set().wait_result_acquire();
  }
 
  void
  check_wait_result_dirty()
  {
    // In the case that the wait set was modified while the result was out,
    // we must mark the wait result as no longer valid
    if (wait_set_pointer_ && this->get_wait_set().wait_result_dirty_) {
      this->wait_result_kind_ = WaitResultKind::Invalid;
    }
  }
 
  WaitResultKind wait_result_kind_;
 
  WaitSetT * wait_set_pointer_ = nullptr;
 
  size_t next_waitable_index_ = 0;
};
 
}  // namespace rclcpp
 
#endif  // RCLCPP__WAIT_RESULT_HPP_