thread_safe_synchronization.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_SET_POLICIES__THREAD_SAFE_SYNCHRONIZATION_HPP_
#define RCLCPP__WAIT_SET_POLICIES__THREAD_SAFE_SYNCHRONIZATION_HPP_
 
#include <chrono>
#include <functional>
#include <memory>
#include <utility>
 
#include "rclcpp/client.hpp"
#include "rclcpp/exceptions.hpp"
#include "rclcpp/guard_condition.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/service.hpp"
#include "rclcpp/subscription_base.hpp"
#include "rclcpp/subscription_wait_set_mask.hpp"
#include "rclcpp/timer.hpp"
#include "rclcpp/visibility_control.hpp"
#include "rclcpp/wait_result.hpp"
#include "rclcpp/wait_result_kind.hpp"
#include "rclcpp/wait_set_policies/detail/synchronization_policy_common.hpp"
#include "rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp"
#include "rclcpp/waitable.hpp"
 
namespace rclcpp
{
namespace wait_set_policies
{
 
 
class ThreadSafeSynchronization : public detail::SynchronizationPolicyCommon
{
protected:
  explicit ThreadSafeSynchronization(rclcpp::Context::SharedPtr context)
  : extra_guard_conditions_{{std::make_shared<rclcpp::GuardCondition>(context)}},
    wprw_lock_([this]() {this->interrupt_waiting_wait_set();})
  {}
  ~ThreadSafeSynchronization() = default;
 
 
  const std::array<std::shared_ptr<rclcpp::GuardCondition>, 1> &
  get_extra_guard_conditions()
  {
    return extra_guard_conditions_;
  }
 
 
  void
  interrupt_waiting_wait_set()
  {
    extra_guard_conditions_[0]->trigger();
  }
 
  void
  sync_add_subscription(
    std::shared_ptr<rclcpp::SubscriptionBase> && subscription,
    const rclcpp::SubscriptionWaitSetMask & mask,
    std::function<
      void(std::shared_ptr<rclcpp::SubscriptionBase>&&, const rclcpp::SubscriptionWaitSetMask &)
    > add_subscription_function)
  {
    using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
    std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
    add_subscription_function(std::move(subscription), mask);
  }
 
  void
  sync_remove_subscription(
    std::shared_ptr<rclcpp::SubscriptionBase> && subscription,
    const rclcpp::SubscriptionWaitSetMask & mask,
    std::function<
      void(std::shared_ptr<rclcpp::SubscriptionBase>&&, const rclcpp::SubscriptionWaitSetMask &)
    > remove_subscription_function)
  {
    using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
    std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
    remove_subscription_function(std::move(subscription), mask);
  }
 
  void
  sync_add_guard_condition(
    std::shared_ptr<rclcpp::GuardCondition> && guard_condition,
    std::function<void(std::shared_ptr<rclcpp::GuardCondition>&&)> add_guard_condition_function)
  {
    using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
    std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
    add_guard_condition_function(std::move(guard_condition));
  }
 
  void
  sync_remove_guard_condition(
    std::shared_ptr<rclcpp::GuardCondition> && guard_condition,
    std::function<void(std::shared_ptr<rclcpp::GuardCondition>&&)> remove_guard_condition_function)
  {
    using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
    std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
    remove_guard_condition_function(std::move(guard_condition));
  }
 
  void
  sync_add_timer(
    std::shared_ptr<rclcpp::TimerBase> && timer,
    std::function<void(std::shared_ptr<rclcpp::TimerBase>&&)> add_timer_function)
  {
    using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
    std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
    add_timer_function(std::move(timer));
  }
 
  void
  sync_remove_timer(
    std::shared_ptr<rclcpp::TimerBase> && timer,
    std::function<void(std::shared_ptr<rclcpp::TimerBase>&&)> remove_timer_function)
  {
    using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
    std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
    remove_timer_function(std::move(timer));
  }
 
  void
  sync_add_client(
    std::shared_ptr<rclcpp::ClientBase> && client,
    std::function<void(std::shared_ptr<rclcpp::ClientBase>&&)> add_client_function)
  {
    using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
    std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
    add_client_function(std::move(client));
  }
 
  void
  sync_remove_client(
    std::shared_ptr<rclcpp::ClientBase> && client,
    std::function<void(std::shared_ptr<rclcpp::ClientBase>&&)> remove_client_function)
  {
    using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
    std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
    remove_client_function(std::move(client));
  }
 
  void
  sync_add_service(
    std::shared_ptr<rclcpp::ServiceBase> && service,
    std::function<void(std::shared_ptr<rclcpp::ServiceBase>&&)> add_service_function)
  {
    using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
    std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
    add_service_function(std::move(service));
  }
 
  void
  sync_remove_service(
    std::shared_ptr<rclcpp::ServiceBase> && service,
    std::function<void(std::shared_ptr<rclcpp::ServiceBase>&&)> remove_service_function)
  {
    using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
    std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
    remove_service_function(std::move(service));
  }
 
  void
  sync_add_waitable(
    std::shared_ptr<rclcpp::Waitable> && waitable,
    std::shared_ptr<void> && associated_entity,
    std::function<
      void(std::shared_ptr<rclcpp::Waitable>&&, std::shared_ptr<void> &&)
    > add_waitable_function)
  {
    using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
    std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
    add_waitable_function(std::move(waitable), std::move(associated_entity));
  }
 
  void
  sync_remove_waitable(
    std::shared_ptr<rclcpp::Waitable> && waitable,
    std::function<void(std::shared_ptr<rclcpp::Waitable>&&)> remove_waitable_function)
  {
    using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
    std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
    remove_waitable_function(std::move(waitable));
  }
 
  void
  sync_prune_deleted_entities(std::function<void()> prune_deleted_entities_function)
  {
    using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
    std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
    prune_deleted_entities_function();
  }
 
  template<class WaitResultT>
  WaitResultT
  sync_wait(
    std::chrono::nanoseconds time_to_wait_ns,
    std::function<void()> rebuild_rcl_wait_set,
    std::function<rcl_wait_set_t & ()> get_rcl_wait_set,
    std::function<WaitResultT(WaitResultKind wait_result_kind)> create_wait_result)
  {
    // Assumption: this function assumes that some measure has been taken to
    // ensure none of the entities being waited on by the wait set are allowed
    // to go out of scope and therefore be deleted.
    // In the case of the StaticStorage policy, this is ensured because it
    // retains shared ownership of all entites for the duration of its own life.
    // In the case of the DynamicStorage policy, this is ensured by the function
    // which calls this function, by acquiring shared ownership of the entites
    // for the duration of this function.
 
    // Setup looping predicate.
    auto start = std::chrono::steady_clock::now();
    std::function<bool()> should_loop = this->create_loop_predicate(time_to_wait_ns, start);
 
    // Wait until exit condition is met.
    do {
      {
        // We have to prevent the entity sets from being mutated while building
        // the rcl wait set.
        using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
        std::lock_guard<WritePreferringReadWriteLock::ReadMutex> lock(wprw_lock_.get_read_mutex());
        // Rebuild the wait set.
        // This will resize the wait set if needed, due to e.g. adding or removing
        // entities since the last wait, but this should never occur in static
        // storage wait sets since they cannot be changed after construction.
        // This will also clear the wait set and re-add all the entities, which
        // prepares it to be waited on again.
        rebuild_rcl_wait_set();
      }
 
      rcl_wait_set_t & rcl_wait_set = get_rcl_wait_set();
 
      // Wait unconditionally until timeout condition occurs since we assume
      // there are no conditions that would require the wait to stop and reset,
      // like asynchronously adding or removing an entity, i.e. explicitly
      // providing no thread-safety.
 
      // Calculate how much time there is left to wait, unless blocking indefinitely.
      auto time_left_to_wait_ns = this->calculate_time_left_to_wait(time_to_wait_ns, start);
 
      // Then wait for entities to become ready.
 
      // It is ok to wait while not having the lock acquired, because the state
      // in the rcl wait set will not be updated until this method calls
      // rebuild_rcl_wait_set().
      rcl_ret_t ret = rcl_wait(&rcl_wait_set, time_left_to_wait_ns.count());
      if (RCL_RET_OK == ret) {
        // Something has become ready in the wait set, first check if it was
        // the guard condition added by this class and/or a user defined guard condition.
        const rcl_guard_condition_t * interrupt_guard_condition_ptr =
          &(extra_guard_conditions_[0]->get_rcl_guard_condition());
        bool was_interrupted_by_this_class = false;
        bool any_user_guard_conditions_triggered = false;
        for (size_t index = 0; index < rcl_wait_set.size_of_guard_conditions; ++index) {
          const rcl_guard_condition_t * current = rcl_wait_set.guard_conditions[index];
          if (nullptr != current) {
            // Something is ready.
            if (rcl_wait_set.guard_conditions[index] == interrupt_guard_condition_ptr) {
              // This means that this class triggered a guard condition to interrupt this wait.
              was_interrupted_by_this_class = true;
            } else {
              // This means it was a user guard condition.
              any_user_guard_conditions_triggered = true;
            }
          }
        }
 
        if (!was_interrupted_by_this_class || any_user_guard_conditions_triggered) {
          // In this case we know:
          //   - something was ready
          //   - it was either:
          //     - not interrupted by this class, or
          //     - maybe it was, but there were also user defined guard conditions.
          //
          // We cannot ignore user defined guard conditions, but we can ignore
          // other kinds of user defined entities, because they will still be
          // ready next time we wait, whereas guard conditions are cleared.
          // Therefore we need to create a WaitResult and return it.
 
          // The WaitResult will call sync_wait_result_acquire() and
          // sync_wait_result_release() to ensure thread-safety by preventing
          // the mutation of the entity sets while introspecting after waiting.
          return create_wait_result(WaitResultKind::Ready);
        }
        // If we get here the we interrupted the wait set and there were no user
        // guard conditions that needed to be handled.
        // So we will loop and it will re-acquire the lock and rebuild the
        // rcl wait set.
      } else if (RCL_RET_TIMEOUT == ret) {
        // The wait set timed out, exit the loop.
        break;
      } else if (RCL_RET_WAIT_SET_EMPTY == ret) {
        // Wait set was empty, return Empty.
        return create_wait_result(WaitResultKind::Empty);
      } else {
        // Some other error case, throw.
        rclcpp::exceptions::throw_from_rcl_error(ret);
      }
    } while (should_loop());
 
    // Wait did not result in ready items, return timeout.
    return create_wait_result(WaitResultKind::Timeout);
  }
 
  void
  sync_wait_result_acquire()
  {
    wprw_lock_.get_read_mutex().lock();
  }
 
  void
  sync_wait_result_release()
  {
    wprw_lock_.get_read_mutex().unlock();
  }
 
protected:
  std::array<std::shared_ptr<rclcpp::GuardCondition>, 1> extra_guard_conditions_;
  rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock wprw_lock_;
};
 
}  // namespace wait_set_policies
}  // namespace rclcpp
 
#endif  // RCLCPP__WAIT_SET_POLICIES__THREAD_SAFE_SYNCHRONIZATION_HPP_