Program Listing for File static_executor_entities_collector.hpp

↰ Return to documentation for file (include/rclcpp/executors/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_