.. _program_listing_file_include_rclcpp_executors_static_executor_entities_collector.hpp: Program Listing for File static_executor_entities_collector.hpp =============================================================== |exhale_lsh| :ref:`Return to documentation for file ` (``include/rclcpp/executors/static_executor_entities_collector.hpp``) .. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS .. code-block:: cpp // 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 #include #include #include #include #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> WeakCallbackGroupsToNodesMap; class StaticExecutorEntitiesCollector final : public rclcpp::Waitable, public std::enable_shared_from_this { 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 & data) override; RCLCPP_PUBLIC std::shared_ptr 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 get_all_callback_groups(); RCLCPP_PUBLIC std::vector get_manually_added_callback_groups(); RCLCPP_PUBLIC std::vector 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> WeakNodesToGuardConditionsMap; WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_; std::list weak_nodes_; // Mutex to protect vector of new nodes. std::mutex new_nodes_mutex_; std::vector 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_