static_executor_entities_collector.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__EXECUTORS__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_
#define RCLCPP__EXECUTORS__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_
 
#include <chrono>
#include <list>
#include <map>
#include <memory>
#include <vector>
 
#include "rcl/guard_condition.h"
#include "rcl/wait.h"
 
#include "rclcpp/experimental/executable_list.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/memory_strategy.hpp"
#include "rclcpp/visibility_control.hpp"
#include "rclcpp/waitable.hpp"
 
namespace rclcpp
{
namespace executors
{
typedef std::map<rclcpp::CallbackGroup::WeakPtr,
    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
    std::owner_less<rclcpp::CallbackGroup::WeakPtr>> WeakCallbackGroupsToNodesMap;
 
class StaticExecutorEntitiesCollector final
  : public rclcpp::Waitable,
  public std::enable_shared_from_this<StaticExecutorEntitiesCollector>
{
public:
  RCLCPP_SMART_PTR_DEFINITIONS(StaticExecutorEntitiesCollector)
 
  // Constructor
  RCLCPP_PUBLIC
  StaticExecutorEntitiesCollector() = default;
 
  // Destructor
  RCLCPP_PUBLIC
  ~StaticExecutorEntitiesCollector();
 
 
  RCLCPP_PUBLIC
  void
  init(
    rcl_wait_set_t * p_wait_set,
    rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy);
 
  RCLCPP_PUBLIC
  bool
  is_init() {return initialized_;}
 
  RCLCPP_PUBLIC
  void
  fini();
 
  RCLCPP_PUBLIC
  void
  execute(std::shared_ptr<void> & data) override;
 
 
  RCLCPP_PUBLIC
  std::shared_ptr<void>
  take_data() override;
 
 
  RCLCPP_PUBLIC
  void
  refresh_wait_set(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
  RCLCPP_PUBLIC
  void
  add_to_wait_set(rcl_wait_set_t * wait_set) override;
 
  RCLCPP_PUBLIC
  size_t
  get_number_of_ready_guard_conditions() override;
 
 
  RCLCPP_PUBLIC
  bool
  add_callback_group(
    rclcpp::CallbackGroup::SharedPtr group_ptr,
    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
 
 
  RCLCPP_PUBLIC
  bool
  add_callback_group(
    rclcpp::CallbackGroup::SharedPtr group_ptr,
    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
    WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
 
  RCLCPP_PUBLIC
  bool
  remove_callback_group(
    rclcpp::CallbackGroup::SharedPtr group_ptr);
 
 
  RCLCPP_PUBLIC
  bool
  remove_callback_group_from_map(
    rclcpp::CallbackGroup::SharedPtr group_ptr,
    WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
  RCLCPP_PUBLIC
  bool
  add_node(
    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
 
  RCLCPP_PUBLIC
  bool
  remove_node(
    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
 
  RCLCPP_PUBLIC
  std::vector<rclcpp::CallbackGroup::WeakPtr>
  get_all_callback_groups();
 
 
  RCLCPP_PUBLIC
  std::vector<rclcpp::CallbackGroup::WeakPtr>
  get_manually_added_callback_groups();
 
 
  RCLCPP_PUBLIC
  std::vector<rclcpp::CallbackGroup::WeakPtr>
  get_automatically_added_callback_groups_from_nodes();
 
 
  RCLCPP_PUBLIC
  bool
  is_ready(rcl_wait_set_t * wait_set) override;
 
 
  RCLCPP_PUBLIC
  size_t
  get_number_of_timers() {return exec_list_.number_of_timers;}
 
 
  RCLCPP_PUBLIC
  size_t
  get_number_of_subscriptions() {return exec_list_.number_of_subscriptions;}
 
 
  RCLCPP_PUBLIC
  size_t
  get_number_of_services() {return exec_list_.number_of_services;}
 
 
  RCLCPP_PUBLIC
  size_t
  get_number_of_clients() {return exec_list_.number_of_clients;}
 
 
  RCLCPP_PUBLIC
  size_t
  get_number_of_waitables() {return exec_list_.number_of_waitables;}
 
  RCLCPP_PUBLIC
  rclcpp::SubscriptionBase::SharedPtr
  get_subscription(size_t i) {return exec_list_.subscription[i];}
 
  RCLCPP_PUBLIC
  rclcpp::TimerBase::SharedPtr
  get_timer(size_t i) {return exec_list_.timer[i];}
 
  RCLCPP_PUBLIC
  rclcpp::ServiceBase::SharedPtr
  get_service(size_t i) {return exec_list_.service[i];}
 
  RCLCPP_PUBLIC
  rclcpp::ClientBase::SharedPtr
  get_client(size_t i) {return exec_list_.client[i];}
 
  RCLCPP_PUBLIC
  rclcpp::Waitable::SharedPtr
  get_waitable(size_t i) {return exec_list_.waitable[i];}
 
private:
 
  void
  prepare_wait_set();
 
  void
  fill_executable_list();
 
  void
  fill_memory_strategy();
 
 
  bool
  has_node(
    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) const;
 
 
  void
  add_callback_groups_from_nodes_associated_to_executor();
 
  void
  fill_executable_list_from_map(const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
  rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy_;
 
  // maps callback groups to nodes.
  WeakCallbackGroupsToNodesMap weak_groups_associated_with_executor_to_nodes_;
  // maps callback groups to nodes.
  WeakCallbackGroupsToNodesMap weak_groups_to_nodes_associated_with_executor_;
 
  typedef std::map<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
      const rclcpp::GuardCondition *,
      std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>
    WeakNodesToGuardConditionsMap;
  WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_;
 
  std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
 
  // Mutex to protect vector of new nodes.
  std::mutex new_nodes_mutex_;
  std::vector<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> new_nodes_;
 
  rcl_wait_set_t * p_wait_set_ = nullptr;
 
  rclcpp::experimental::ExecutableList exec_list_;
 
  bool initialized_ = false;
};
 
}  // namespace executors
}  // namespace rclcpp
 
#endif  // RCLCPP__EXECUTORS__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_