diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 3d8f697..c5334d1 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -54,6 +54,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/node_interfaces/node_services.cpp src/rclcpp/node_interfaces/node_timers.cpp src/rclcpp/node_interfaces/node_topics.cpp + src/rclcpp/node_interfaces/node_waitables.cpp src/rclcpp/parameter.cpp src/rclcpp/parameter_value.cpp src/rclcpp/parameter_client.cpp @@ -68,6 +69,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/timer.cpp src/rclcpp/type_support.cpp src/rclcpp/utilities.cpp + src/rclcpp/waitable.cpp ) # "watch" template for changes diff --git a/rclcpp/include/rclcpp/any_executable.hpp b/rclcpp/include/rclcpp/any_executable.hpp index 3f81013..7b9bac0 100644 --- a/rclcpp/include/rclcpp/any_executable.hpp +++ b/rclcpp/include/rclcpp/any_executable.hpp @@ -25,6 +25,7 @@ #include "rclcpp/subscription.hpp" #include "rclcpp/timer.hpp" #include "rclcpp/visibility_control.hpp" +#include "rclcpp/waitable.hpp" namespace rclcpp { @@ -45,6 +46,7 @@ struct AnyExecutable rclcpp::TimerBase::SharedPtr timer; rclcpp::ServiceBase::SharedPtr service; rclcpp::ClientBase::SharedPtr client; + rclcpp::Waitable::SharedPtr waitable; // These are used to keep the scope on the containing items rclcpp::callback_group::CallbackGroup::SharedPtr callback_group; rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base; diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index 810bfc3..e2bee1c 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -25,6 +25,7 @@ #include "rclcpp/subscription.hpp" #include "rclcpp/timer.hpp" #include "rclcpp/visibility_control.hpp" +#include "rclcpp/waitable.hpp" namespace rclcpp { @@ -35,6 +36,7 @@ namespace node_interfaces class NodeServices; class NodeTimers; class NodeTopics; +class NodeWaitables; } // namespace node_interfaces namespace callback_group @@ -51,6 +53,7 @@ class CallbackGroup friend class rclcpp::node_interfaces::NodeServices; friend class rclcpp::node_interfaces::NodeTimers; friend class rclcpp::node_interfaces::NodeTopics; + friend class rclcpp::node_interfaces::NodeWaitables; public: RCLCPP_SMART_PTR_DEFINITIONS(CallbackGroup) @@ -74,6 +77,10 @@ public: const std::vector & get_client_ptrs() const; + RCLCPP_PUBLIC + const std::vector & + get_waitable_ptrs() const; + RCLCPP_PUBLIC std::atomic_bool & can_be_taken_from(); @@ -101,6 +108,10 @@ protected: void add_client(const rclcpp::ClientBase::SharedPtr client_ptr); + RCLCPP_PUBLIC + void + add_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr); + CallbackGroupType type_; // Mutex to protect the subsequent vectors of pointers. mutable std::mutex mutex_; @@ -108,6 +119,7 @@ protected: std::vector timer_ptrs_; std::vector service_ptrs_; std::vector client_ptrs_; + std::vector waitable_ptrs_; std::atomic_bool can_be_taken_from_; }; diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp index ae7f372..416890e 100644 --- a/rclcpp/include/rclcpp/memory_strategy.hpp +++ b/rclcpp/include/rclcpp/memory_strategy.hpp @@ -25,6 +25,7 @@ #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/visibility_control.hpp" +#include "rclcpp/waitable.hpp" namespace rclcpp { @@ -52,6 +53,7 @@ public: virtual size_t number_of_ready_clients() const = 0; virtual size_t number_of_ready_timers() const = 0; virtual size_t number_of_guard_conditions() const = 0; + virtual size_t number_of_waitables() const = 0; virtual bool add_handles_to_wait_set(rcl_wait_set_t * wait_set) = 0; virtual void clear_handles() = 0; @@ -76,6 +78,11 @@ public: rclcpp::executor::AnyExecutable & any_exec, const WeakNodeVector & weak_nodes) = 0; + virtual void + get_next_waitable( + rclcpp::executor::AnyExecutable & any_exec, + const WeakNodeVector & weak_nodes) = 0; + virtual rcl_allocator_t get_allocator() = 0; @@ -113,6 +120,11 @@ public: get_group_by_client( rclcpp::ClientBase::SharedPtr client, const WeakNodeVector & weak_nodes); + + static rclcpp::callback_group::CallbackGroup::SharedPtr + get_group_by_waitable( + rclcpp::Waitable::SharedPtr waitable, + const WeakNodeVector & weak_nodes); }; } // namespace memory_strategy diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index d2a996d..f6be5b1 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -49,6 +49,7 @@ #include "rclcpp/node_interfaces/node_services_interface.hpp" #include "rclcpp/node_interfaces/node_timers_interface.hpp" #include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/node_interfaces/node_waitables_interface.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/publisher.hpp" #include "rclcpp/service.hpp" @@ -442,6 +443,11 @@ public: rclcpp::node_interfaces::NodeServicesInterface::SharedPtr get_node_services_interface(); + /// Return the Node's internal NodeWaitablesInterface implementation. + RCLCPP_PUBLIC + rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr + get_node_waitables_interface(); + /// Return the Node's internal NodeParametersInterface implementation. RCLCPP_PUBLIC rclcpp::node_interfaces::NodeParametersInterface::SharedPtr @@ -462,6 +468,7 @@ private: rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_; rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_; + rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_; bool use_intra_process_comms_; }; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_waitables.hpp b/rclcpp/include/rclcpp/node_interfaces/node_waitables.hpp new file mode 100644 index 0000000..1b05a3c --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_waitables.hpp @@ -0,0 +1,60 @@ +// Copyright 2018 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__NODE_INTERFACES__NODE_WAITABLES_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_WAITABLES_HPP_ + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/client.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_waitables_interface.hpp" +#include "rclcpp/waitable.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Implementation of the NodeWaitables part of the Node API. +class NodeWaitables : public NodeWaitablesInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeWaitables) + + RCLCPP_PUBLIC + explicit NodeWaitables(rclcpp::node_interfaces::NodeBaseInterface * node_base); + + RCLCPP_PUBLIC + virtual + ~NodeWaitables(); + + RCLCPP_PUBLIC + virtual + void + add_waitable( + rclcpp::Waitable::SharedPtr waitable_base_ptr, + rclcpp::callback_group::CallbackGroup::SharedPtr group); + +private: + RCLCPP_DISABLE_COPY(NodeWaitables) + + rclcpp::node_interfaces::NodeBaseInterface * node_base_; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_WAITABLES_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp new file mode 100644 index 0000000..ea05c73 --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp @@ -0,0 +1,45 @@ +// Copyright 2018 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__NODE_INTERFACES__NODE_WAITABLES_INTERFACE_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_WAITABLES_INTERFACE_HPP_ + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rclcpp/waitable.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Pure virtual interface class for the NodeWaitables part of the Node API. +class NodeWaitablesInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeWaitablesInterface) + + RCLCPP_PUBLIC + virtual + void + add_waitable( + rclcpp::Waitable::SharedPtr waitable_ptr, + rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_WAITABLES_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index 9465603..698688b 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -151,5 +151,6 @@ #include "rclcpp/time.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" +#include "rclcpp/waitable.hpp" #endif // RCLCPP__RCLCPP_HPP_ diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 224a3ac..ca1d2fb 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -87,30 +87,42 @@ public: service_handles_.clear(); client_handles_.clear(); timer_handles_.clear(); + waitable_handles_.clear(); } virtual void remove_null_handles(rcl_wait_set_t * wait_set) { - for (size_t i = 0; i < wait_set->size_of_subscriptions; ++i) { + // TODO(jacobperron): Check if wait set sizes are what we expect them to be? + // e.g. wait_set->size_of_clients == client_handles_.size() + + // Important to use subscription_handles_.size() instead of wait set's size since + // there may be more subscriptions in the wait set due to Waitables added to the end. + // The same logic applies for other entities. + for (size_t i = 0; i < subscription_handles_.size(); ++i) { if (!wait_set->subscriptions[i]) { subscription_handles_[i].reset(); } } - for (size_t i = 0; i < wait_set->size_of_services; ++i) { + for (size_t i = 0; i < service_handles_.size(); ++i) { if (!wait_set->services[i]) { service_handles_[i].reset(); } } - for (size_t i = 0; i < wait_set->size_of_clients; ++i) { + for (size_t i = 0; i < client_handles_.size(); ++i) { if (!wait_set->clients[i]) { client_handles_[i].reset(); } } - for (size_t i = 0; i < wait_set->size_of_timers; ++i) { + for (size_t i = 0; i < timer_handles_.size(); ++i) { if (!wait_set->timers[i]) { timer_handles_[i].reset(); } } + for (size_t i = 0; i < waitable_handles_.size(); ++i) { + if (!waitable_handles_[i]->is_ready(wait_set)) { + waitable_handles_[i].reset(); + } + } subscription_handles_.erase( std::remove(subscription_handles_.begin(), subscription_handles_.end(), nullptr), @@ -131,6 +143,11 @@ public: std::remove(timer_handles_.begin(), timer_handles_.end(), nullptr), timer_handles_.end() ); + + waitable_handles_.erase( + std::remove(waitable_handles_.begin(), waitable_handles_.end(), nullptr), + waitable_handles_.end() + ); } bool collect_entities(const WeakNodeVector & weak_nodes) @@ -175,6 +192,12 @@ public: timer_handles_.push_back(timer->get_timer_handle()); } } + for (auto & weak_waitable : group->get_waitable_ptrs()) { + auto waitable = weak_waitable.lock(); + if (waitable) { + waitable_handles_.push_back(waitable); + } + } } } return has_invalid_weak_nodes; @@ -227,6 +250,15 @@ public: return false; } } + + for (auto waitable : waitable_handles_) { + if (!waitable->add_to_wait_set(wait_set)) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't add waitable to wait set: %s", rcl_get_error_string().str); + return false; + } + } return true; } @@ -342,6 +374,39 @@ public: } } + virtual void + get_next_waitable(executor::AnyExecutable & any_exec, const WeakNodeVector & weak_nodes) + { + auto it = waitable_handles_.begin(); + while (it != waitable_handles_.end()) { + auto waitable = *it; + if (waitable) { + // Find the group for this handle and see if it can be serviced + auto group = get_group_by_waitable(waitable, weak_nodes); + if (!group) { + // Group was not found, meaning the waitable is not valid... + // Remove it from the ready list and continue looking + it = waitable_handles_.erase(it); + continue; + } + if (!group->can_be_taken_from().load()) { + // Group is mutually exclusive and is being used, so skip it for now + // Leave it to be checked next time, but continue searching + ++it; + continue; + } + // Otherwise it is safe to set and return the any_exec + any_exec.waitable = waitable; + any_exec.callback_group = group; + any_exec.node_base = get_node_by_group(group, weak_nodes); + waitable_handles_.erase(it); + return; + } + // Else, the waitable is no longer valid, remove it and continue + it = waitable_handles_.erase(it); + } + } + virtual rcl_allocator_t get_allocator() { return rclcpp::allocator::get_rcl_allocator(*allocator_.get()); @@ -349,27 +414,52 @@ public: size_t number_of_ready_subscriptions() const { - return subscription_handles_.size(); + size_t number_of_subscriptions = subscription_handles_.size(); + for (auto waitable : waitable_handles_) { + number_of_subscriptions += waitable->get_number_of_ready_subscriptions(); + } + return number_of_subscriptions; } size_t number_of_ready_services() const { - return service_handles_.size(); + size_t number_of_services = service_handles_.size(); + for (auto waitable : waitable_handles_) { + number_of_services += waitable->get_number_of_ready_services(); + } + return number_of_services; } size_t number_of_ready_clients() const { - return client_handles_.size(); + size_t number_of_clients = client_handles_.size(); + for (auto waitable : waitable_handles_) { + number_of_clients += waitable->get_number_of_ready_clients(); + } + return number_of_clients; } size_t number_of_guard_conditions() const { - return guard_conditions_.size(); + size_t number_of_guard_conditions = guard_conditions_.size(); + for (auto waitable : waitable_handles_) { + number_of_guard_conditions += waitable->get_number_of_ready_guard_conditions(); + } + return number_of_guard_conditions; } size_t number_of_ready_timers() const { - return timer_handles_.size(); + size_t number_of_timers = timer_handles_.size(); + for (auto waitable : waitable_handles_) { + number_of_timers += waitable->get_number_of_ready_timers(); + } + return number_of_timers; + } + + size_t number_of_waitables() const + { + return waitable_handles_.size(); } private: @@ -383,6 +473,7 @@ private: VectorRebind> service_handles_; VectorRebind> client_handles_; VectorRebind> timer_handles_; + VectorRebind> waitable_handles_; std::shared_ptr allocator_; }; diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp new file mode 100644 index 0000000..4f48bbc --- /dev/null +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -0,0 +1,141 @@ +// Copyright 2018 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__WAITABLE_HPP_ +#define RCLCPP__WAITABLE_HPP_ + +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +#include "rcl/wait.h" + +namespace rclcpp +{ + +class Waitable +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Waitable) + + /// Get the number of ready subscriptions + /** + * Returns a value of 0 by default. + * This should be overridden if the Waitable contains one or more subscriptions. + * \return The number of subscriptions associated with the Waitable. + */ + RCLCPP_PUBLIC + virtual + size_t + get_number_of_ready_subscriptions(); + + /// Get the number of ready timers + /** + * Returns a value of 0 by default. + * This should be overridden if the Waitable contains one or more timers. + * \return The number of timers associated with the Waitable. + */ + RCLCPP_PUBLIC + virtual + size_t + get_number_of_ready_timers(); + + /// Get the number of ready clients + /** + * Returns a value of 0 by default. + * This should be overridden if the Waitable contains one or more clients. + * \return The number of clients associated with the Waitable. + */ + RCLCPP_PUBLIC + virtual + size_t + get_number_of_ready_clients(); + + /// Get the number of ready services + /** + * Returns a value of 0 by default. + * This should be overridden if the Waitable contains one or more services. + * \return The number of services associated with the Waitable. + */ + RCLCPP_PUBLIC + virtual + size_t + get_number_of_ready_services(); + + /// Get the number of ready guard_conditions + /** + * Returns a value of 0 by default. + * This should be overridden if the Waitable contains one or more guard_conditions. + * \return The number of guard_conditions associated with the Waitable. + */ + RCLCPP_PUBLIC + virtual + size_t + get_number_of_ready_guard_conditions(); + + // TODO(jacobperron): smart pointer? + /// Add the Waitable to a wait set. + /** + * \param[in] wait_set A handle to the wait set to add the Waitable to. + * \return `true` if the Waitable is added successfully, `false` otherwise. + */ + RCLCPP_PUBLIC + virtual + bool + add_to_wait_set(rcl_wait_set_t * wait_set) = 0; + + /// Check if the Waitable is ready. + /** + * The input wait set should be the same that was used in a previously call to + * `add_to_wait_set()`. + * The wait set should also have been previously waited on with `rcl_wait()`. + * + * \param[in] wait_set A handle to the wait set the Waitable was previously added to + * and that has been waited on. + * \return `true` if the Waitable is ready, `false` otherwise. + */ + RCLCPP_PUBLIC + virtual + bool + is_ready(rcl_wait_set_t *) = 0; + + /// Execute any entities of the Waitable that are ready. + /** + * Before calling this method, the Waitable should be added to a wait set, + * waited on, and then updated. + * + * Example usage: + * + * ``` + * // ... create a wait set and a Waitable + * // Add the Waitable to the wait set + * bool add_ret = waitable.add_to_wait_set(wait_set); + * // ... error handling + * // Wait + * rcl_ret_t wait_ret = rcl_wait(wait_set); + * // ... error handling + * // Update the Waitable + * waitable.update(wait_set); + * // Execute any entities of the Waitable that may be ready + * waitable.execute(); + * ``` + */ + RCLCPP_PUBLIC + virtual + void + execute() = 0; +}; // class Waitable + +} // namespace rclcpp + +#endif // RCLCPP__WAITABLE_HPP_ diff --git a/rclcpp/src/rclcpp/callback_group.cpp b/rclcpp/src/rclcpp/callback_group.cpp index 52558ee..21a1acb 100644 --- a/rclcpp/src/rclcpp/callback_group.cpp +++ b/rclcpp/src/rclcpp/callback_group.cpp @@ -51,6 +51,13 @@ CallbackGroup::get_client_ptrs() const return client_ptrs_; } +const std::vector & +CallbackGroup::get_waitable_ptrs() const +{ + std::lock_guard lock(mutex_); + return waitable_ptrs_; +} + std::atomic_bool & CallbackGroup::can_be_taken_from() { @@ -91,3 +98,10 @@ CallbackGroup::add_client(const rclcpp::ClientBase::SharedPtr client_ptr) std::lock_guard lock(mutex_); client_ptrs_.push_back(client_ptr); } + +void +CallbackGroup::add_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr) +{ + std::lock_guard lock(mutex_); + waitable_ptrs_.push_back(waitable_ptr); +} diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index bd892fa..5f28668 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -281,6 +281,9 @@ Executor::execute_any_executable(AnyExecutable & any_exec) if (any_exec.client) { execute_client(any_exec.client); } + if (any_exec.waitable) { + any_exec.waitable->execute(); + } // Reset the callback_group, regardless of type any_exec.callback_group->can_be_taken_from().store(true); // Wake the wait, because it may need to be recalculated or work that @@ -427,6 +430,7 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) throw std::runtime_error("Couldn't clear wait set"); } + // The size of waitables are accounted for in size of the other entities rcl_ret_t ret = rcl_wait_set_resize( &wait_set_, memory_strategy_->number_of_ready_subscriptions(), memory_strategy_->number_of_guard_conditions(), memory_strategy_->number_of_ready_timers(), @@ -549,6 +553,11 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable) if (any_executable.client) { return true; } + // Check the waitables to see if there are any that are ready + memory_strategy_->get_next_waitable(any_executable, weak_nodes_); + if (any_executable.waitable) { + return true; + } // If there is no ready executable, return a null ptr return false; } diff --git a/rclcpp/src/rclcpp/memory_strategy.cpp b/rclcpp/src/rclcpp/memory_strategy.cpp index 5ef79a1..f77c534 100644 --- a/rclcpp/src/rclcpp/memory_strategy.cpp +++ b/rclcpp/src/rclcpp/memory_strategy.cpp @@ -200,3 +200,29 @@ MemoryStrategy::get_group_by_client( } return nullptr; } + +rclcpp::callback_group::CallbackGroup::SharedPtr +MemoryStrategy::get_group_by_waitable( + rclcpp::Waitable::SharedPtr waitable, + const WeakNodeVector & weak_nodes) +{ + for (auto & weak_node : weak_nodes) { + auto node = weak_node.lock(); + if (!node) { + continue; + } + for (auto & weak_group : node->get_callback_groups()) { + auto group = weak_group.lock(); + if (!group) { + continue; + } + for (auto & weak_waitable : group->get_waitable_ptrs()) { + auto group_waitable = weak_waitable.lock(); + if (group_waitable && group_waitable == waitable) { + return group; + } + } + } + } + return nullptr; +} diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 6934370..507319d 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -31,6 +31,7 @@ #include "rclcpp/node_interfaces/node_services.hpp" #include "rclcpp/node_interfaces/node_timers.hpp" #include "rclcpp/node_interfaces/node_topics.hpp" +#include "rclcpp/node_interfaces/node_waitables.hpp" using rclcpp::Node; using rclcpp::exceptions::throw_from_rcl_error; @@ -80,6 +81,7 @@ Node::Node( node_graph_, node_services_ )), + node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())), use_intra_process_comms_(use_intra_process_comms) { } @@ -276,3 +278,9 @@ Node::get_node_parameters_interface() { return node_parameters_; } + +rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr +Node::get_node_waitables_interface() +{ + return node_waitables_; +} diff --git a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp new file mode 100644 index 0000000..04a1c5c --- /dev/null +++ b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp @@ -0,0 +1,53 @@ +// Copyright 2018 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. + +#include "rclcpp/node_interfaces/node_waitables.hpp" + +#include + +using rclcpp::node_interfaces::NodeWaitables; + +NodeWaitables::NodeWaitables(rclcpp::node_interfaces::NodeBaseInterface * node_base) +: node_base_(node_base) +{} + +NodeWaitables::~NodeWaitables() +{} + +void +NodeWaitables::add_waitable( + rclcpp::Waitable::SharedPtr waitable_ptr, + rclcpp::callback_group::CallbackGroup::SharedPtr group) +{ + if (group) { + if (!node_base_->callback_group_in_node(group)) { + // TODO(jacobperron): use custom exception + throw std::runtime_error("Cannot create waitable, group not in node."); + } + group->add_waitable(waitable_ptr); + } else { + node_base_->get_default_callback_group()->add_waitable(waitable_ptr); + } + + // Notify the executor that a new waitable was created using the parent Node. + { + auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); + if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { + throw std::runtime_error( + std::string("Failed to notify wait set on waitable creation: ") + + rmw_get_error_string().str + ); + } + } +} diff --git a/rclcpp/src/rclcpp/waitable.cpp b/rclcpp/src/rclcpp/waitable.cpp new file mode 100644 index 0000000..a29b80d --- /dev/null +++ b/rclcpp/src/rclcpp/waitable.cpp @@ -0,0 +1,47 @@ +// Copyright 2018 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. + +#include "rclcpp/waitable.hpp" + +using rclcpp::Waitable; + +size_t +Waitable::get_number_of_ready_subscriptions() +{ + return 0u; +} + +size_t +Waitable::get_number_of_ready_timers() +{ + return 0u; +} + +size_t +Waitable::get_number_of_ready_clients() +{ + return 0u; +} + +size_t +Waitable::get_number_of_ready_services() +{ + return 0u; +} + +size_t +Waitable::get_number_of_ready_guard_conditions() +{ + return 0u; +} diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index ceb95a9..bb5c275 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -44,6 +44,7 @@ #include "rclcpp/node_interfaces/node_services_interface.hpp" #include "rclcpp/node_interfaces/node_timers_interface.hpp" #include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/node_interfaces/node_waitables_interface.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/publisher.hpp" #include "rclcpp/service.hpp" @@ -390,6 +391,11 @@ public: rclcpp::node_interfaces::NodeParametersInterface::SharedPtr get_node_parameters_interface(); + /// Return the Node's internal NodeWaitablesInterface implementation. + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr + get_node_waitables_interface(); + // // LIFECYCLE COMPONENTS // @@ -515,6 +521,7 @@ private: rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_; rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_; + rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_; bool use_intra_process_comms_; diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 4e667c7..72fe36c 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -35,6 +35,7 @@ #include "rclcpp/node_interfaces/node_services.hpp" #include "rclcpp/node_interfaces/node_timers.hpp" #include "rclcpp/node_interfaces/node_topics.hpp" +#include "rclcpp/node_interfaces/node_waitables.hpp" #include "rclcpp/parameter_service.hpp" #include "lifecycle_node_interface_impl.hpp" // implementation @@ -87,6 +88,7 @@ LifecycleNode::LifecycleNode( node_graph_, node_services_ )), + node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())), use_intra_process_comms_(use_intra_process_comms), impl_(new LifecycleNodeInterfaceImpl(node_base_, node_services_)) { @@ -297,6 +299,12 @@ LifecycleNode::get_node_parameters_interface() return node_parameters_; } +rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr +LifecycleNode::get_node_waitables_interface() +{ + return node_waitables_; +} + //// bool