Compare commits
1 Commits
mjcarroll/
...
fujitatomo
| Author | SHA1 | Date | |
|---|---|---|---|
|
9e2c101ca7 |
@@ -8,8 +8,7 @@ rclcpp provides the standard C++ API for interacting with ROS 2.
|
||||
|
||||
`#include "rclcpp/rclcpp.hpp"` allows use of the most common elements of the ROS 2 system.
|
||||
|
||||
The link to the latest API documentation can be found on the rclcpp package info page, at the [ROS Index](https://index.ros.org/p/rclcpp/).
|
||||
|
||||
Visit the [rclcpp API documentation](http://docs.ros2.org/latest/api/rclcpp/) for a complete list of its main components.
|
||||
|
||||
### Examples
|
||||
|
||||
|
||||
@@ -2,75 +2,6 @@
|
||||
Changelog for package rclcpp
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
20.0.0 (2023-04-13)
|
||||
-------------------
|
||||
* applied tracepoints for ring_buffer (`#2091 <https://github.com/ros2/rclcpp/issues/2091>`_)
|
||||
* Dynamic Subscription (REP-2011 Subset): Stubs for rclcpp (`#2165 <https://github.com/ros2/rclcpp/issues/2165>`_)
|
||||
* Add type_hash to cpp TopicEndpointInfo (`#2137 <https://github.com/ros2/rclcpp/issues/2137>`_)
|
||||
* Trigger the intraprocess guard condition with data (`#2164 <https://github.com/ros2/rclcpp/issues/2164>`_)
|
||||
* Minor grammar fix (`#2149 <https://github.com/ros2/rclcpp/issues/2149>`_)
|
||||
* Fix unnecessary allocations in executor.cpp (`#2135 <https://github.com/ros2/rclcpp/issues/2135>`_)
|
||||
* add Logger::get_effective_level(). (`#2141 <https://github.com/ros2/rclcpp/issues/2141>`_)
|
||||
* Remove deprecated header (`#2139 <https://github.com/ros2/rclcpp/issues/2139>`_)
|
||||
* Implement matched event (`#2105 <https://github.com/ros2/rclcpp/issues/2105>`_)
|
||||
* use allocator via init_options argument. (`#2129 <https://github.com/ros2/rclcpp/issues/2129>`_)
|
||||
* Fixes to silence some clang warnings. (`#2127 <https://github.com/ros2/rclcpp/issues/2127>`_)
|
||||
* Documentation improvements on the executor (`#2125 <https://github.com/ros2/rclcpp/issues/2125>`_)
|
||||
* Avoid losing waitable handles while using MultiThreadedExecutor (`#2109 <https://github.com/ros2/rclcpp/issues/2109>`_)
|
||||
* Hook up the incompatible type event inside of rclcpp (`#2069 <https://github.com/ros2/rclcpp/issues/2069>`_)
|
||||
* Update all rclcpp packages to C++17. (`#2121 <https://github.com/ros2/rclcpp/issues/2121>`_)
|
||||
* Fix clang warning: bugprone-use-after-move (`#2116 <https://github.com/ros2/rclcpp/issues/2116>`_)
|
||||
* Contributors: Barry Xu, Chris Lalancette, Christopher Wecht, Emerson Knapp, Michael Carroll, Tomoya Fujita, Yadu, mauropasse, methylDragon, ymski
|
||||
|
||||
19.3.0 (2023-03-01)
|
||||
-------------------
|
||||
* Fix memory leak in tracetools::get_symbol() (`#2104 <https://github.com/ros2/rclcpp/issues/2104>`_)
|
||||
* Service introspection (`#1985 <https://github.com/ros2/rclcpp/issues/1985>`_)
|
||||
* Allow publishing borrowed messages with intra-process enabled (`#2108 <https://github.com/ros2/rclcpp/issues/2108>`_)
|
||||
* to fix flaky test about TestTimeSource.callbacks (`#2111 <https://github.com/ros2/rclcpp/issues/2111>`_)
|
||||
* Contributors: Brian, Chen Lihui, Christophe Bedard, Miguel Company
|
||||
|
||||
19.2.0 (2023-02-24)
|
||||
-------------------
|
||||
* to create a sublogger while getting child of Logger (`#1717 <https://github.com/ros2/rclcpp/issues/1717>`_)
|
||||
* Fix documentation of Context class (`#2107 <https://github.com/ros2/rclcpp/issues/2107>`_)
|
||||
* fixes for rmw callbacks in qos_event class (`#2102 <https://github.com/ros2/rclcpp/issues/2102>`_)
|
||||
* Contributors: Alberto Soragna, Chen Lihui, Silvio Traversaro
|
||||
|
||||
19.1.0 (2023-02-14)
|
||||
-------------------
|
||||
* Add support for timers on reset callback (`#1979 <https://github.com/ros2/rclcpp/issues/1979>`_)
|
||||
* Topic node guard condition in executor (`#2074 <https://github.com/ros2/rclcpp/issues/2074>`_)
|
||||
* Fix bug on the disorder of calling shutdown callback (`#2097 <https://github.com/ros2/rclcpp/issues/2097>`_)
|
||||
* Contributors: Barry Xu, Chen Lihui, mauropasse
|
||||
|
||||
19.0.0 (2023-01-30)
|
||||
-------------------
|
||||
* Add default constructor to NodeInterfaces (`#2094 <https://github.com/ros2/rclcpp/issues/2094>`_)
|
||||
* Fix clock state cached time to be a copy, not a reference. (`#2092 <https://github.com/ros2/rclcpp/issues/2092>`_)
|
||||
* Fix -Wmaybe-uninitialized warning (`#2081 <https://github.com/ros2/rclcpp/issues/2081>`_)
|
||||
* Fix the keep_last warning when using system defaults. (`#2082 <https://github.com/ros2/rclcpp/issues/2082>`_)
|
||||
* Add in a fix for older compilers. (`#2075 <https://github.com/ros2/rclcpp/issues/2075>`_)
|
||||
* Contributors: Alexander Hans, Chris Lalancette, Shane Loretz
|
||||
|
||||
18.0.0 (2022-12-29)
|
||||
-------------------
|
||||
* Implement Unified Node Interface (NodeInterfaces class) (`#2041 <https://github.com/ros2/rclcpp/issues/2041>`_)
|
||||
* Do not throw exception if trying to dequeue an empty intra-process buffer (`#2061 <https://github.com/ros2/rclcpp/issues/2061>`_)
|
||||
* Move event callback binding to PublisherBase and SubscriptionBase (`#2066 <https://github.com/ros2/rclcpp/issues/2066>`_)
|
||||
* Implement validity checks for rclcpp::Clock (`#2040 <https://github.com/ros2/rclcpp/issues/2040>`_)
|
||||
* Explicitly set callback type (`#2059 <https://github.com/ros2/rclcpp/issues/2059>`_)
|
||||
* Fix logging macros to build with msvc and cpp20 (`#2063 <https://github.com/ros2/rclcpp/issues/2063>`_)
|
||||
* Add clock type to node_options (`#1982 <https://github.com/ros2/rclcpp/issues/1982>`_)
|
||||
* Fix nullptr dereference in prune_requests_older_than (`#2008 <https://github.com/ros2/rclcpp/issues/2008>`_)
|
||||
* Remove templating on to_rcl_subscription_options (`#2056 <https://github.com/ros2/rclcpp/issues/2056>`_)
|
||||
* Fix SharedFuture from async_send_request never becoming valid (`#2044 <https://github.com/ros2/rclcpp/issues/2044>`_)
|
||||
* Add in a warning for a KeepLast depth of 0. (`#2048 <https://github.com/ros2/rclcpp/issues/2048>`_)
|
||||
* Mark rclcpp::Clock::now() as const (`#2050 <https://github.com/ros2/rclcpp/issues/2050>`_)
|
||||
* Fix a case that did not throw ParameterUninitializedException (`#2036 <https://github.com/ros2/rclcpp/issues/2036>`_)
|
||||
* Update maintainers (`#2043 <https://github.com/ros2/rclcpp/issues/2043>`_)
|
||||
* Contributors: Alberto Soragna, Audrow Nash, Chen Lihui, Chris Lalancette, Jeffery Hsu, Lei Liu, Mateusz Szczygielski, Shane Loretz, andrei, mauropasse, methylDragon
|
||||
|
||||
17.1.0 (2022-11-02)
|
||||
-------------------
|
||||
* MultiThreadExecutor number of threads is at least 2+ in default. (`#2032 <https://github.com/ros2/rclcpp/issues/2032>`_)
|
||||
|
||||
@@ -25,7 +25,6 @@ find_package(tracetools REQUIRED)
|
||||
# Default to C++17
|
||||
if(NOT CMAKE_CXX_STANDARD)
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
endif()
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
# About -Wno-sign-conversion: With Clang, -Wconversion implies -Wsign-conversion. There are a number of
|
||||
@@ -49,25 +48,16 @@ set(${PROJECT_NAME}_SRCS
|
||||
src/rclcpp/detail/rmw_implementation_specific_subscription_payload.cpp
|
||||
src/rclcpp/detail/utilities.cpp
|
||||
src/rclcpp/duration.cpp
|
||||
src/rclcpp/dynamic_typesupport/dynamic_message.cpp
|
||||
src/rclcpp/dynamic_typesupport/dynamic_message_type.cpp
|
||||
src/rclcpp/dynamic_typesupport/dynamic_message_type_builder.cpp
|
||||
src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp
|
||||
src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp
|
||||
src/rclcpp/event.cpp
|
||||
src/rclcpp/exceptions/exceptions.cpp
|
||||
src/rclcpp/executable_list.cpp
|
||||
src/rclcpp/executor.cpp
|
||||
src/rclcpp/executors.cpp
|
||||
src/rclcpp/executors/executor_entities_collection.cpp
|
||||
src/rclcpp/executors/executor_entities_collector.cpp
|
||||
src/rclcpp/executors/executor_notify_waitable.cpp
|
||||
src/rclcpp/executors/multi_threaded_executor.cpp
|
||||
src/rclcpp/executors/single_threaded_executor.cpp
|
||||
src/rclcpp/executors/static_executor_entities_collector.cpp
|
||||
src/rclcpp/executors/static_single_threaded_executor.cpp
|
||||
src/rclcpp/expand_topic_or_service_name.cpp
|
||||
src/rclcpp/experimental/executors/events_executor/events_executor.cpp
|
||||
src/rclcpp/experimental/timers_manager.cpp
|
||||
src/rclcpp/future_return_code.cpp
|
||||
src/rclcpp/generic_publisher.cpp
|
||||
src/rclcpp/generic_subscription.cpp
|
||||
@@ -102,7 +92,7 @@ set(${PROJECT_NAME}_SRCS
|
||||
src/rclcpp/parameter_value.cpp
|
||||
src/rclcpp/publisher_base.cpp
|
||||
src/rclcpp/qos.cpp
|
||||
src/rclcpp/event_handler.cpp
|
||||
src/rclcpp/qos_event.cpp
|
||||
src/rclcpp/qos_overriding_options.cpp
|
||||
src/rclcpp/serialization.cpp
|
||||
src/rclcpp/serialized_message.cpp
|
||||
|
||||
@@ -191,14 +191,10 @@ public:
|
||||
#ifndef TRACETOOLS_DISABLED
|
||||
std::visit(
|
||||
[this](auto && arg) {
|
||||
if (TRACEPOINT_ENABLED(rclcpp_callback_register)) {
|
||||
char * symbol = tracetools::get_symbol(arg);
|
||||
DO_TRACEPOINT(
|
||||
rclcpp_callback_register,
|
||||
static_cast<const void *>(this),
|
||||
symbol);
|
||||
std::free(symbol);
|
||||
}
|
||||
TRACEPOINT(
|
||||
rclcpp_callback_register,
|
||||
static_cast<const void *>(this),
|
||||
tracetools::get_symbol(arg));
|
||||
}, callback_);
|
||||
#endif // TRACETOOLS_DISABLED
|
||||
}
|
||||
|
||||
@@ -965,14 +965,10 @@ public:
|
||||
#ifndef TRACETOOLS_DISABLED
|
||||
std::visit(
|
||||
[this](auto && callback) {
|
||||
if (TRACEPOINT_ENABLED(rclcpp_callback_register)) {
|
||||
char * symbol = tracetools::get_symbol(callback);
|
||||
DO_TRACEPOINT(
|
||||
rclcpp_callback_register,
|
||||
static_cast<const void *>(this),
|
||||
symbol);
|
||||
std::free(symbol);
|
||||
}
|
||||
TRACEPOINT(
|
||||
rclcpp_callback_register,
|
||||
static_cast<const void *>(this),
|
||||
tracetools::get_symbol(callback));
|
||||
}, callback_variant_);
|
||||
#endif // TRACETOOLS_DISABLED
|
||||
}
|
||||
|
||||
@@ -93,54 +93,11 @@ public:
|
||||
* determines whether a callback group is automatically added to an executor
|
||||
* with the node with which it is associated.
|
||||
*/
|
||||
[[deprecated("Use CallbackGroup constructor with context function argument")]]
|
||||
RCLCPP_PUBLIC
|
||||
explicit CallbackGroup(
|
||||
CallbackGroupType group_type,
|
||||
bool automatically_add_to_executor_with_node = true);
|
||||
|
||||
/// Constructor for CallbackGroup.
|
||||
/**
|
||||
* Callback Groups have a type, either 'Mutually Exclusive' or 'Reentrant'
|
||||
* and when creating one the type must be specified.
|
||||
*
|
||||
* Callbacks in Reentrant Callback Groups must be able to:
|
||||
* - run at the same time as themselves (reentrant)
|
||||
* - run at the same time as other callbacks in their group
|
||||
* - run at the same time as other callbacks in other groups
|
||||
*
|
||||
* Callbacks in Mutually Exclusive Callback Groups:
|
||||
* - will not be run multiple times simultaneously (non-reentrant)
|
||||
* - will not be run at the same time as other callbacks in their group
|
||||
* - but must run at the same time as callbacks in other groups
|
||||
*
|
||||
* Additionally, callback groups have a property which determines whether or
|
||||
* not they are added to an executor with their associated node automatically.
|
||||
* When creating a callback group the automatically_add_to_executor_with_node
|
||||
* argument determines this behavior, and if true it will cause the newly
|
||||
* created callback group to be added to an executor with the node when the
|
||||
* Executor::add_node method is used.
|
||||
* If false, this callback group will not be added automatically and would
|
||||
* have to be added to an executor manually using the
|
||||
* Executor::add_callback_group method.
|
||||
*
|
||||
* Whether the node was added to the executor before creating the callback
|
||||
* group, or after, is irrelevant; the callback group will be automatically
|
||||
* added to the executor in either case.
|
||||
*
|
||||
* \param[in] group_type The type of the callback group.
|
||||
* \param[in] get_node_context Lambda to retrieve the node context when
|
||||
* checking that the creating node is valid and using the guard condition.
|
||||
* \param[in] automatically_add_to_executor_with_node A boolean that
|
||||
* determines whether a callback group is automatically added to an executor
|
||||
* with the node with which it is associated.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
explicit CallbackGroup(
|
||||
CallbackGroupType group_type,
|
||||
std::function<rclcpp::Context::SharedPtr(void)> get_node_context,
|
||||
bool automatically_add_to_executor_with_node = true);
|
||||
|
||||
/// Default destructor.
|
||||
RCLCPP_PUBLIC
|
||||
~CallbackGroup();
|
||||
@@ -180,42 +137,14 @@ public:
|
||||
return _find_ptrs_if_impl<rclcpp::Waitable, Function>(func, waitable_ptrs_);
|
||||
}
|
||||
|
||||
/// Get the total number of entities in this callback group.
|
||||
/**
|
||||
* \return the number of entities in the callback group.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
size_t size() const;
|
||||
|
||||
/// Return a reference to the 'can be taken' atomic boolean.
|
||||
/**
|
||||
* The resulting bool will be true in the case that no executor is currently
|
||||
* using an executable entity from this group.
|
||||
* The resulting bool will be false in the case that an executor is currently
|
||||
* using an executable entity from this group, and the group policy doesn't
|
||||
* allow a second take (eg mutual exclusion)
|
||||
* \return a reference to the flag
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
std::atomic_bool &
|
||||
can_be_taken_from();
|
||||
|
||||
/// Get the group type.
|
||||
/**
|
||||
* \return the group type
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
const CallbackGroupType &
|
||||
type() const;
|
||||
|
||||
/// Collect all of the entity pointers contained in this callback group.
|
||||
/**
|
||||
* \param[in] sub_func Function to execute for each subscription
|
||||
* \param[in] service_func Function to execute for each service
|
||||
* \param[in] client_func Function to execute for each client
|
||||
* \param[in] timer_func Function to execute for each timer
|
||||
* \param[in] waitable_fuinc Function to execute for each waitable
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void collect_all_ptrs(
|
||||
std::function<void(const rclcpp::SubscriptionBase::SharedPtr &)> sub_func,
|
||||
@@ -249,24 +178,11 @@ public:
|
||||
bool
|
||||
automatically_add_to_executor_with_node() const;
|
||||
|
||||
/// Retrieve the guard condition used to signal changes to this callback group.
|
||||
/**
|
||||
* \param[in] context_ptr context to use when creating the guard condition
|
||||
* \return guard condition if it is valid, otherwise nullptr.
|
||||
*/
|
||||
[[deprecated("Use get_notify_guard_condition() without arguments")]]
|
||||
/// Defer creating the notify guard condition and return it.
|
||||
RCLCPP_PUBLIC
|
||||
rclcpp::GuardCondition::SharedPtr
|
||||
get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr);
|
||||
|
||||
/// Retrieve the guard condition used to signal changes to this callback group.
|
||||
/**
|
||||
* \return guard condition if it is valid, otherwise nullptr.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
rclcpp::GuardCondition::SharedPtr
|
||||
get_notify_guard_condition();
|
||||
|
||||
/// Trigger the notify guard condition.
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
@@ -318,8 +234,6 @@ protected:
|
||||
std::shared_ptr<rclcpp::GuardCondition> notify_guard_condition_ = nullptr;
|
||||
std::recursive_mutex notify_guard_condition_mutex_;
|
||||
|
||||
std::function<rclcpp::Context::SharedPtr(void)> get_context_;
|
||||
|
||||
private:
|
||||
template<typename TypeT, typename Function>
|
||||
typename TypeT::SharedPtr _find_ptrs_if_impl(
|
||||
|
||||
@@ -16,15 +16,14 @@
|
||||
#define RCLCPP__CLIENT_HPP_
|
||||
|
||||
#include <atomic>
|
||||
#include <functional>
|
||||
#include <future>
|
||||
#include <unordered_map>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <optional> // NOLINT, cpplint doesn't think this is a cpp std header
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
#include <tuple>
|
||||
#include <unordered_map>
|
||||
#include <utility>
|
||||
#include <variant> // NOLINT
|
||||
#include <vector>
|
||||
@@ -32,10 +31,8 @@
|
||||
#include "rcl/client.h"
|
||||
#include "rcl/error_handling.h"
|
||||
#include "rcl/event_callback.h"
|
||||
#include "rcl/service_introspection.h"
|
||||
#include "rcl/wait.h"
|
||||
|
||||
#include "rclcpp/clock.hpp"
|
||||
#include "rclcpp/detail/cpp_callback_trampoline.hpp"
|
||||
#include "rclcpp/exceptions.hpp"
|
||||
#include "rclcpp/expand_topic_or_service_name.hpp"
|
||||
@@ -315,7 +312,7 @@ public:
|
||||
// This two-step setting, prevents a gap where the old std::function has
|
||||
// been replaced but the middleware hasn't been told about the new one yet.
|
||||
set_on_new_response_callback(
|
||||
rclcpp::detail::cpp_callback_trampoline<decltype(new_callback), const void *, size_t>,
|
||||
rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
|
||||
static_cast<const void *>(&new_callback));
|
||||
|
||||
// Store the std::function to keep it in scope, also overwrites the existing one.
|
||||
@@ -323,8 +320,7 @@ public:
|
||||
|
||||
// Set it again, now using the permanent storage.
|
||||
set_on_new_response_callback(
|
||||
rclcpp::detail::cpp_callback_trampoline<
|
||||
decltype(on_new_response_callback_), const void *, size_t>,
|
||||
rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
|
||||
static_cast<const void *>(&on_new_response_callback_));
|
||||
}
|
||||
|
||||
@@ -470,13 +466,15 @@ public:
|
||||
rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
|
||||
const std::string & service_name,
|
||||
rcl_client_options_t & client_options)
|
||||
: ClientBase(node_base, node_graph),
|
||||
srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle<ServiceT>())
|
||||
: ClientBase(node_base, node_graph)
|
||||
{
|
||||
using rosidl_typesupport_cpp::get_service_type_support_handle;
|
||||
auto service_type_support_handle =
|
||||
get_service_type_support_handle<ServiceT>();
|
||||
rcl_ret_t ret = rcl_client_init(
|
||||
this->get_client_handle().get(),
|
||||
this->get_rcl_node_handle(),
|
||||
srv_type_support_handle_,
|
||||
service_type_support_handle,
|
||||
service_name.c_str(),
|
||||
&client_options);
|
||||
if (ret != RCL_RET_OK) {
|
||||
@@ -771,9 +769,7 @@ public:
|
||||
auto old_size = pending_requests_.size();
|
||||
for (auto it = pending_requests_.begin(), last = pending_requests_.end(); it != last; ) {
|
||||
if (it->second.first < time_point) {
|
||||
if (pruned_requests) {
|
||||
pruned_requests->push_back(it->first);
|
||||
}
|
||||
pruned_requests->push_back(it->first);
|
||||
it = pending_requests_.erase(it);
|
||||
} else {
|
||||
++it;
|
||||
@@ -782,33 +778,6 @@ public:
|
||||
return old_size - pending_requests_.size();
|
||||
}
|
||||
|
||||
/// Configure client introspection.
|
||||
/**
|
||||
* \param[in] clock clock to use to generate introspection timestamps
|
||||
* \param[in] qos_service_event_pub QoS settings to use when creating the introspection publisher
|
||||
* \param[in] introspection_state the state to set introspection to
|
||||
*/
|
||||
void
|
||||
configure_introspection(
|
||||
Clock::SharedPtr clock, const QoS & qos_service_event_pub,
|
||||
rcl_service_introspection_state_t introspection_state)
|
||||
{
|
||||
rcl_publisher_options_t pub_opts = rcl_publisher_get_default_options();
|
||||
pub_opts.qos = qos_service_event_pub.get_rmw_qos_profile();
|
||||
|
||||
rcl_ret_t ret = rcl_client_configure_service_introspection(
|
||||
client_handle_.get(),
|
||||
node_handle_.get(),
|
||||
clock->get_clock_handle(),
|
||||
srv_type_support_handle_,
|
||||
pub_opts,
|
||||
introspection_state);
|
||||
|
||||
if (RCL_RET_OK != ret) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(ret, "failed to configure client introspection");
|
||||
}
|
||||
}
|
||||
|
||||
protected:
|
||||
using CallbackTypeValueVariant = std::tuple<CallbackType, SharedFuture, Promise>;
|
||||
using CallbackWithRequestTypeValueVariant = std::tuple<
|
||||
@@ -823,14 +792,16 @@ protected:
|
||||
async_send_request_impl(const Request & request, CallbackInfoVariant value)
|
||||
{
|
||||
int64_t sequence_number;
|
||||
std::lock_guard<std::mutex> lock(pending_requests_mutex_);
|
||||
rcl_ret_t ret = rcl_send_request(get_client_handle().get(), &request, &sequence_number);
|
||||
if (RCL_RET_OK != ret) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request");
|
||||
}
|
||||
pending_requests_.try_emplace(
|
||||
sequence_number,
|
||||
std::make_pair(std::chrono::system_clock::now(), std::move(value)));
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(pending_requests_mutex_);
|
||||
pending_requests_.try_emplace(
|
||||
sequence_number,
|
||||
std::make_pair(std::chrono::system_clock::now(), std::move(value)));
|
||||
}
|
||||
return sequence_number;
|
||||
}
|
||||
|
||||
@@ -859,9 +830,6 @@ protected:
|
||||
CallbackInfoVariant>>
|
||||
pending_requests_;
|
||||
std::mutex pending_requests_mutex_;
|
||||
|
||||
private:
|
||||
const rosidl_service_type_support_t * srv_type_support_handle_;
|
||||
};
|
||||
|
||||
} // namespace rclcpp
|
||||
|
||||
@@ -137,51 +137,6 @@ public:
|
||||
Duration rel_time,
|
||||
Context::SharedPtr context = contexts::get_global_default_context());
|
||||
|
||||
/**
|
||||
* Check if the clock is started.
|
||||
*
|
||||
* A started clock is a clock that reflects non-zero time.
|
||||
* Typically a clock will be unstarted if it is using RCL_ROS_TIME with ROS time and
|
||||
* nothing has been published on the clock topic yet.
|
||||
*
|
||||
* \return true if clock is started
|
||||
* \throws std::runtime_error if the clock is not rcl_clock_valid
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
started();
|
||||
|
||||
/**
|
||||
* Wait until clock to start.
|
||||
*
|
||||
* \rclcpp::Clock::started
|
||||
* \param context the context to wait in
|
||||
* \return true if clock was already started or became started
|
||||
* \throws std::runtime_error if the context is invalid or clock is not rcl_clock_valid
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
wait_until_started(Context::SharedPtr context = contexts::get_global_default_context());
|
||||
|
||||
/**
|
||||
* Wait for clock to start, with timeout.
|
||||
*
|
||||
* The timeout is waited in steady time.
|
||||
*
|
||||
* \rclcpp::Clock::started
|
||||
* \param timeout the maximum time to wait for.
|
||||
* \param context the context to wait in.
|
||||
* \param wait_tick_ns the time to wait between each iteration of the wait loop (in nanoseconds).
|
||||
* \return true if clock was or became valid
|
||||
* \throws std::runtime_error if the context is invalid or clock is not rcl_clock_valid
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
wait_until_started(
|
||||
const rclcpp::Duration & timeout,
|
||||
Context::SharedPtr context = contexts::get_global_default_context(),
|
||||
const rclcpp::Duration & wait_tick_ns = rclcpp::Duration(0, static_cast<uint32_t>(1e7)));
|
||||
|
||||
/**
|
||||
* Returns the clock of the type `RCL_ROS_TIME` is active.
|
||||
*
|
||||
|
||||
@@ -65,11 +65,8 @@ using PreShutdownCallbackHandle = ShutdownCallbackHandle;
|
||||
/// Context which encapsulates shared state between nodes and other similar entities.
|
||||
/**
|
||||
* A context also represents the lifecycle between init and shutdown of rclcpp.
|
||||
* Nodes may be attached to a particular context by passing to the rclcpp::Node
|
||||
* constructor a rclcpp::NodeOptions instance in which the Context is set via
|
||||
* rclcpp::NodeOptions::context.
|
||||
* Nodes will be automatically removed from the context when destructed.
|
||||
* Contexts may be shutdown by calling rclcpp::shutdown.
|
||||
* It is often used in conjunction with rclcpp::init, or rclcpp::init_local,
|
||||
* and rclcpp::shutdown.
|
||||
*/
|
||||
class Context : public std::enable_shared_from_this<Context>
|
||||
{
|
||||
@@ -379,10 +376,10 @@ private:
|
||||
// attempt to acquire another sub context.
|
||||
std::recursive_mutex sub_contexts_mutex_;
|
||||
|
||||
std::vector<std::shared_ptr<OnShutdownCallback>> on_shutdown_callbacks_;
|
||||
std::unordered_set<std::shared_ptr<OnShutdownCallback>> on_shutdown_callbacks_;
|
||||
mutable std::mutex on_shutdown_callbacks_mutex_;
|
||||
|
||||
std::vector<std::shared_ptr<PreShutdownCallback>> pre_shutdown_callbacks_;
|
||||
std::unordered_set<std::shared_ptr<PreShutdownCallback>> pre_shutdown_callbacks_;
|
||||
mutable std::mutex pre_shutdown_callbacks_mutex_;
|
||||
|
||||
/// Condition variable for timed sleep (see sleep_for).
|
||||
@@ -401,22 +398,20 @@ private:
|
||||
|
||||
using ShutdownCallback = ShutdownCallbackHandle::ShutdownCallbackType;
|
||||
|
||||
template<ShutdownType shutdown_type>
|
||||
RCLCPP_LOCAL
|
||||
ShutdownCallbackHandle
|
||||
add_shutdown_callback(
|
||||
ShutdownType shutdown_type,
|
||||
ShutdownCallback callback);
|
||||
|
||||
template<ShutdownType shutdown_type>
|
||||
RCLCPP_LOCAL
|
||||
bool
|
||||
remove_shutdown_callback(
|
||||
ShutdownType shutdown_type,
|
||||
const ShutdownCallbackHandle & callback_handle);
|
||||
|
||||
template<ShutdownType shutdown_type>
|
||||
RCLCPP_LOCAL
|
||||
std::vector<rclcpp::Context::ShutdownCallback>
|
||||
get_shutdown_callback() const;
|
||||
get_shutdown_callback(ShutdownType shutdown_type) const;
|
||||
};
|
||||
|
||||
/// Return a copy of the list of context shared pointers.
|
||||
|
||||
@@ -41,7 +41,6 @@ namespace detail
|
||||
* so no exceptions should be thrown at this point, and doing so results in
|
||||
* undefined behavior.
|
||||
*
|
||||
* \tparam UserDataRealT Declared type of the passed function
|
||||
* \tparam UserDataT Deduced type based on what is passed for user data,
|
||||
* usually this type is either `void *` or `const void *`.
|
||||
* \tparam Args the arguments being passed to the callback
|
||||
@@ -51,7 +50,6 @@ namespace detail
|
||||
* \returns whatever the callback returns, if anything
|
||||
*/
|
||||
template<
|
||||
typename UserDataRealT,
|
||||
typename UserDataT,
|
||||
typename ... Args,
|
||||
typename ReturnT = void
|
||||
@@ -59,7 +57,7 @@ template<
|
||||
ReturnT
|
||||
cpp_callback_trampoline(UserDataT user_data, Args ... args) noexcept
|
||||
{
|
||||
auto & actual_callback = *static_cast<const UserDataRealT *>(user_data);
|
||||
auto & actual_callback = *reinterpret_cast<const std::function<ReturnT(Args...)> *>(user_data);
|
||||
return actual_callback(args ...);
|
||||
}
|
||||
|
||||
|
||||
@@ -1,47 +0,0 @@
|
||||
// Copyright 2022 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__DETAIL__TEMPLATE_CONTAINS_HPP_
|
||||
#define RCLCPP__DETAIL__TEMPLATE_CONTAINS_HPP_
|
||||
|
||||
#include <type_traits>
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace detail
|
||||
{
|
||||
|
||||
/// Template meta-function that checks if a given T is contained in the list Us.
|
||||
template<typename T, typename ... Us>
|
||||
struct template_contains;
|
||||
|
||||
template<typename ... Args>
|
||||
inline constexpr bool template_contains_v = template_contains<Args ...>::value;
|
||||
|
||||
template<typename T, typename NextT, typename ... Us>
|
||||
struct template_contains<T, NextT, Us ...>
|
||||
{
|
||||
enum { value = (std::is_same_v<T, NextT>|| template_contains_v<T, Us ...>)};
|
||||
};
|
||||
|
||||
template<typename T>
|
||||
struct template_contains<T>
|
||||
{
|
||||
enum { value = false };
|
||||
};
|
||||
|
||||
} // namespace detail
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__DETAIL__TEMPLATE_CONTAINS_HPP_
|
||||
@@ -1,49 +0,0 @@
|
||||
// Copyright 2022 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__DETAIL__TEMPLATE_UNIQUE_HPP_
|
||||
#define RCLCPP__DETAIL__TEMPLATE_UNIQUE_HPP_
|
||||
|
||||
#include <type_traits>
|
||||
|
||||
#include "rclcpp/detail/template_contains.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace detail
|
||||
{
|
||||
|
||||
/// Template meta-function that checks if a given list Ts contains unique types.
|
||||
template<typename ... Ts>
|
||||
struct template_unique;
|
||||
|
||||
template<typename ... Args>
|
||||
inline constexpr bool template_unique_v = template_unique<Args ...>::value;
|
||||
|
||||
template<typename NextT, typename ... Ts>
|
||||
struct template_unique<NextT, Ts ...>
|
||||
{
|
||||
enum { value = !template_contains_v<NextT, Ts ...>&& template_unique_v<Ts ...>};
|
||||
};
|
||||
|
||||
template<typename T>
|
||||
struct template_unique<T>
|
||||
{
|
||||
enum { value = true };
|
||||
};
|
||||
|
||||
} // namespace detail
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__DETAIL__TEMPLATE_UNIQUE_HPP_
|
||||
@@ -1,70 +0,0 @@
|
||||
// Copyright 2023 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__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_HPP_
|
||||
#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_HPP_
|
||||
|
||||
#include <rcl/allocator.h>
|
||||
#include <rcl/types.h>
|
||||
#include <rosidl_dynamic_typesupport/types.h>
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp"
|
||||
#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace dynamic_typesupport
|
||||
{
|
||||
|
||||
/// Utility wrapper class for rosidl_dynamic_typesupport_dynamic_data_t
|
||||
/// STUBBED OUT
|
||||
class DynamicMessage : public std::enable_shared_from_this<DynamicMessage>
|
||||
{
|
||||
public:
|
||||
RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessage)
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual ~DynamicMessage();
|
||||
|
||||
protected:
|
||||
// NOTE(methylDragon):
|
||||
// This is just here to extend the lifetime of the serialization support
|
||||
// It isn't actually used by the builder since the builder should compose its own support
|
||||
//
|
||||
// ... Though ideally it should be the exact same support as the one stored in the
|
||||
// DynamicSerializationSupport
|
||||
DynamicSerializationSupport::SharedPtr serialization_support_;
|
||||
|
||||
rosidl_dynamic_typesupport_dynamic_data_t rosidl_dynamic_data_;
|
||||
bool is_loaned_;
|
||||
|
||||
// Used for returning the loaned value, and lifetime management
|
||||
DynamicMessage::SharedPtr parent_data_;
|
||||
|
||||
private:
|
||||
RCLCPP_DISABLE_COPY(DynamicMessage)
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessage();
|
||||
};
|
||||
|
||||
} // namespace dynamic_typesupport
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_HPP_
|
||||
@@ -1,64 +0,0 @@
|
||||
// Copyright 2023 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__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_HPP_
|
||||
#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_HPP_
|
||||
|
||||
#include <rcl/allocator.h>
|
||||
#include <rosidl_dynamic_typesupport/types.h>
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace dynamic_typesupport
|
||||
{
|
||||
|
||||
/// Utility wrapper class for `rosidl_dynamic_typesupport_dynamic_type_t`
|
||||
/// STUBBED OUT
|
||||
class DynamicMessageType : public std::enable_shared_from_this<DynamicMessageType>
|
||||
{
|
||||
public:
|
||||
RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessageType)
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual ~DynamicMessageType();
|
||||
|
||||
protected:
|
||||
// NOTE(methylDragon):
|
||||
// This is just here to extend the lifetime of the serialization support
|
||||
// It isn't actually used by the builder since the builder should compose its own support
|
||||
//
|
||||
// ... Though ideally it should be the exact same support as the one stored in the
|
||||
// `DynamicSerializationSupport`
|
||||
DynamicSerializationSupport::SharedPtr serialization_support_;
|
||||
|
||||
rosidl_dynamic_typesupport_dynamic_type_t rosidl_dynamic_type_;
|
||||
|
||||
private:
|
||||
RCLCPP_DISABLE_COPY(DynamicMessageType)
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessageType();
|
||||
};
|
||||
|
||||
} // namespace dynamic_typesupport
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_HPP_
|
||||
@@ -1,65 +0,0 @@
|
||||
// Copyright 2023 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__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_BUILDER_HPP_
|
||||
#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_BUILDER_HPP_
|
||||
|
||||
#include <rcl/allocator.h>
|
||||
#include <rosidl_dynamic_typesupport/types.h>
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace dynamic_typesupport
|
||||
{
|
||||
|
||||
/// Utility wrapper class for `rosidl_dynamic_typesupport_dynamic_type_builder_t *`
|
||||
/// STUBBED OUT
|
||||
class DynamicMessageTypeBuilder : public std::enable_shared_from_this<DynamicMessageTypeBuilder>
|
||||
{
|
||||
public:
|
||||
RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessageTypeBuilder)
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual ~DynamicMessageTypeBuilder();
|
||||
|
||||
protected:
|
||||
// NOTE(methylDragon):
|
||||
// This is just here to extend the lifetime of the serialization support
|
||||
// It isn't actually used by the builder since the builder should compose its own support
|
||||
//
|
||||
// ... Though ideally it should be the exact same support as the one stored in the
|
||||
// `DynamicSerializationSupport`
|
||||
DynamicSerializationSupport::SharedPtr serialization_support_;
|
||||
|
||||
rosidl_dynamic_typesupport_dynamic_type_builder_t rosidl_dynamic_type_builder_;
|
||||
|
||||
private:
|
||||
RCLCPP_DISABLE_COPY(DynamicMessageTypeBuilder)
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessageTypeBuilder();
|
||||
};
|
||||
|
||||
} // namespace dynamic_typesupport
|
||||
} // namespace rclcpp
|
||||
|
||||
|
||||
#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_BUILDER_HPP_
|
||||
@@ -1,67 +0,0 @@
|
||||
// Copyright 2023 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__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_SUPPORT_HPP_
|
||||
#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_SUPPORT_HPP_
|
||||
|
||||
#include <rcl/allocator.h>
|
||||
|
||||
#include <rosidl_dynamic_typesupport/dynamic_message_type_support_struct.h>
|
||||
#include <rosidl_dynamic_typesupport/types.h>
|
||||
#include <rosidl_runtime_c/message_type_support_struct.h>
|
||||
#include <rosidl_runtime_c/type_description/type_description__struct.h>
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "rclcpp/dynamic_typesupport/dynamic_message.hpp"
|
||||
#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp"
|
||||
#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
|
||||
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace dynamic_typesupport
|
||||
{
|
||||
|
||||
/// Utility wrapper class for `rosidl_message_type_support_t` containing managed
|
||||
/// STUBBED OUT
|
||||
class DynamicMessageTypeSupport : public std::enable_shared_from_this<DynamicMessageTypeSupport>
|
||||
{
|
||||
public:
|
||||
RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessageTypeSupport)
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual ~DynamicMessageTypeSupport();
|
||||
|
||||
protected:
|
||||
DynamicSerializationSupport::SharedPtr serialization_support_;
|
||||
DynamicMessageType::SharedPtr dynamic_message_type_;
|
||||
DynamicMessage::SharedPtr dynamic_message_;
|
||||
|
||||
rosidl_message_type_support_t rosidl_message_type_support_;
|
||||
|
||||
private:
|
||||
RCLCPP_DISABLE_COPY(DynamicMessageTypeSupport)
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicMessageTypeSupport();
|
||||
};
|
||||
|
||||
} // namespace dynamic_typesupport
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_SUPPORT_HPP_
|
||||
@@ -1,60 +0,0 @@
|
||||
// Copyright 2023 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__DYNAMIC_TYPESUPPORT__DYNAMIC_SERIALIZATION_SUPPORT_HPP_
|
||||
#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_SERIALIZATION_SUPPORT_HPP_
|
||||
|
||||
#include <rcl/allocator.h>
|
||||
#include <rosidl_dynamic_typesupport/api/serialization_support.h>
|
||||
#include <rosidl_dynamic_typesupport/types.h>
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace dynamic_typesupport
|
||||
{
|
||||
|
||||
/// Utility wrapper class for rosidl_dynamic_typesupport_serialization_support_t
|
||||
class DynamicSerializationSupport : public std::enable_shared_from_this<DynamicSerializationSupport>
|
||||
{
|
||||
public:
|
||||
RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicSerializationSupport)
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
explicit DynamicSerializationSupport(rcl_allocator_t allocator = rcl_get_default_allocator());
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
DynamicSerializationSupport(
|
||||
const std::string & serialization_library_name,
|
||||
rcl_allocator_t allocator = rcl_get_default_allocator());
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual ~DynamicSerializationSupport();
|
||||
|
||||
protected:
|
||||
rosidl_dynamic_typesupport_serialization_support_t rosidl_serialization_support_;
|
||||
|
||||
private:
|
||||
RCLCPP_DISABLE_COPY(DynamicSerializationSupport)
|
||||
};
|
||||
|
||||
} // namespace dynamic_typesupport
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_SERIALIZATION_SUPPORT_HPP_
|
||||
@@ -1,311 +0,0 @@
|
||||
// Copyright 2019 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__EVENT_HANDLER_HPP_
|
||||
#define RCLCPP__EVENT_HANDLER_HPP_
|
||||
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
|
||||
#include "rcl/error_handling.h"
|
||||
#include "rcl/event_callback.h"
|
||||
#include "rmw/impl/cpp/demangle.hpp"
|
||||
#include "rmw/incompatible_qos_events_statuses.h"
|
||||
#include "rmw/events_statuses/incompatible_type.h"
|
||||
|
||||
#include "rcutils/logging_macros.h"
|
||||
|
||||
#include "rclcpp/detail/cpp_callback_trampoline.hpp"
|
||||
#include "rclcpp/exceptions.hpp"
|
||||
#include "rclcpp/function_traits.hpp"
|
||||
#include "rclcpp/logging.hpp"
|
||||
#include "rclcpp/waitable.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
|
||||
using QOSDeadlineRequestedInfo = rmw_requested_deadline_missed_status_t;
|
||||
using QOSDeadlineOfferedInfo = rmw_offered_deadline_missed_status_t;
|
||||
using QOSLivelinessChangedInfo = rmw_liveliness_changed_status_t;
|
||||
using QOSLivelinessLostInfo = rmw_liveliness_lost_status_t;
|
||||
using QOSMessageLostInfo = rmw_message_lost_status_t;
|
||||
using QOSOfferedIncompatibleQoSInfo = rmw_offered_qos_incompatible_event_status_t;
|
||||
using QOSRequestedIncompatibleQoSInfo = rmw_requested_qos_incompatible_event_status_t;
|
||||
|
||||
using IncompatibleTypeInfo = rmw_incompatible_type_status_t;
|
||||
using MatchedInfo = rmw_matched_status_t;
|
||||
|
||||
using QOSDeadlineRequestedCallbackType = std::function<void (QOSDeadlineRequestedInfo &)>;
|
||||
using QOSDeadlineOfferedCallbackType = std::function<void (QOSDeadlineOfferedInfo &)>;
|
||||
using QOSLivelinessChangedCallbackType = std::function<void (QOSLivelinessChangedInfo &)>;
|
||||
using QOSLivelinessLostCallbackType = std::function<void (QOSLivelinessLostInfo &)>;
|
||||
using QOSMessageLostCallbackType = std::function<void (QOSMessageLostInfo &)>;
|
||||
using QOSOfferedIncompatibleQoSCallbackType = std::function<void (QOSOfferedIncompatibleQoSInfo &)>;
|
||||
using QOSRequestedIncompatibleQoSCallbackType =
|
||||
std::function<void (QOSRequestedIncompatibleQoSInfo &)>;
|
||||
|
||||
using IncompatibleTypeCallbackType = std::function<void (IncompatibleTypeInfo &)>;
|
||||
using PublisherMatchedCallbackType = std::function<void (MatchedInfo &)>;
|
||||
using SubscriptionMatchedCallbackType = std::function<void (MatchedInfo &)>;
|
||||
|
||||
/// Contains callbacks for various types of events a Publisher can receive from the middleware.
|
||||
struct PublisherEventCallbacks
|
||||
{
|
||||
QOSDeadlineOfferedCallbackType deadline_callback;
|
||||
QOSLivelinessLostCallbackType liveliness_callback;
|
||||
QOSOfferedIncompatibleQoSCallbackType incompatible_qos_callback;
|
||||
IncompatibleTypeCallbackType incompatible_type_callback;
|
||||
PublisherMatchedCallbackType matched_callback;
|
||||
};
|
||||
|
||||
/// Contains callbacks for non-message events that a Subscription can receive from the middleware.
|
||||
struct SubscriptionEventCallbacks
|
||||
{
|
||||
QOSDeadlineRequestedCallbackType deadline_callback;
|
||||
QOSLivelinessChangedCallbackType liveliness_callback;
|
||||
QOSRequestedIncompatibleQoSCallbackType incompatible_qos_callback;
|
||||
QOSMessageLostCallbackType message_lost_callback;
|
||||
IncompatibleTypeCallbackType incompatible_type_callback;
|
||||
SubscriptionMatchedCallbackType matched_callback;
|
||||
};
|
||||
|
||||
class UnsupportedEventTypeException : public exceptions::RCLErrorBase, public std::runtime_error
|
||||
{
|
||||
public:
|
||||
RCLCPP_PUBLIC
|
||||
UnsupportedEventTypeException(
|
||||
rcl_ret_t ret,
|
||||
const rcl_error_state_t * error_state,
|
||||
const std::string & prefix);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
UnsupportedEventTypeException(
|
||||
const exceptions::RCLErrorBase & base_exc,
|
||||
const std::string & prefix);
|
||||
};
|
||||
|
||||
class EventHandlerBase : public Waitable
|
||||
{
|
||||
public:
|
||||
enum class EntityType : std::size_t
|
||||
{
|
||||
Event,
|
||||
};
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual ~EventHandlerBase();
|
||||
|
||||
/// Get the number of ready events
|
||||
RCLCPP_PUBLIC
|
||||
size_t
|
||||
get_number_of_ready_events() override;
|
||||
|
||||
/// Add the Waitable to a wait set.
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_to_wait_set(rcl_wait_set_t * wait_set) override;
|
||||
|
||||
/// Check if the Waitable is ready.
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
is_ready(rcl_wait_set_t * wait_set) override;
|
||||
|
||||
/// Set a callback to be called when each new event instance occurs.
|
||||
/**
|
||||
* The callback receives a size_t which is the number of events that occurred
|
||||
* since the last time this callback was called.
|
||||
* Normally this is 1, but can be > 1 if events occurred before any
|
||||
* callback was set.
|
||||
*
|
||||
* The callback also receives an int identifier argument.
|
||||
* This is needed because a Waitable may be composed of several distinct entities,
|
||||
* such as subscriptions, services, etc.
|
||||
* The application should provide a generic callback function that will be then
|
||||
* forwarded by the waitable to all of its entities.
|
||||
* Before forwarding, a different value for the identifier argument will be
|
||||
* bond to the function.
|
||||
* This implies that the provided callback can use the identifier to behave
|
||||
* differently depending on which entity triggered the waitable to become ready.
|
||||
*
|
||||
* Since this callback is called from the middleware, you should aim to make
|
||||
* it fast and not blocking.
|
||||
* If you need to do a lot of work or wait for some other event, you should
|
||||
* spin it off to another thread, otherwise you risk blocking the middleware.
|
||||
*
|
||||
* Calling it again will clear any previously set callback.
|
||||
*
|
||||
* An exception will be thrown if the callback is not callable.
|
||||
*
|
||||
* This function is thread-safe.
|
||||
*
|
||||
* If you want more information available in the callback, like the qos event
|
||||
* or other information, you may use a lambda with captures or std::bind.
|
||||
*
|
||||
* \sa rmw_event_set_callback
|
||||
* \sa rcl_event_set_callback
|
||||
*
|
||||
* \param[in] callback functor to be called when a new event occurs
|
||||
*/
|
||||
void
|
||||
set_on_ready_callback(std::function<void(size_t, int)> callback) override
|
||||
{
|
||||
if (!callback) {
|
||||
throw std::invalid_argument(
|
||||
"The callback passed to set_on_ready_callback "
|
||||
"is not callable.");
|
||||
}
|
||||
|
||||
// Note: we bind the int identifier argument to this waitable's entity types
|
||||
auto new_callback =
|
||||
[callback, this](size_t number_of_events) {
|
||||
try {
|
||||
callback(number_of_events, static_cast<int>(EntityType::Event));
|
||||
} catch (const std::exception & exception) {
|
||||
RCLCPP_ERROR_STREAM(
|
||||
// TODO(wjwwood): get this class access to the node logger it is associated with
|
||||
rclcpp::get_logger("rclcpp"),
|
||||
"rclcpp::EventHandlerBase@" << this <<
|
||||
" caught " << rmw::impl::cpp::demangle(exception) <<
|
||||
" exception in user-provided callback for the 'on ready' callback: " <<
|
||||
exception.what());
|
||||
} catch (...) {
|
||||
RCLCPP_ERROR_STREAM(
|
||||
rclcpp::get_logger("rclcpp"),
|
||||
"rclcpp::EventHandlerBase@" << this <<
|
||||
" caught unhandled exception in user-provided callback " <<
|
||||
"for the 'on ready' callback");
|
||||
}
|
||||
};
|
||||
|
||||
std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
|
||||
|
||||
// Set it temporarily to the new callback, while we replace the old one.
|
||||
// This two-step setting, prevents a gap where the old std::function has
|
||||
// been replaced but the middleware hasn't been told about the new one yet.
|
||||
set_on_new_event_callback(
|
||||
rclcpp::detail::cpp_callback_trampoline<decltype(new_callback), const void *, size_t>,
|
||||
static_cast<const void *>(&new_callback));
|
||||
|
||||
// Store the std::function to keep it in scope, also overwrites the existing one.
|
||||
on_new_event_callback_ = new_callback;
|
||||
|
||||
// Set it again, now using the permanent storage.
|
||||
set_on_new_event_callback(
|
||||
rclcpp::detail::cpp_callback_trampoline<
|
||||
decltype(on_new_event_callback_), const void *, size_t>,
|
||||
static_cast<const void *>(&on_new_event_callback_));
|
||||
}
|
||||
|
||||
/// Unset the callback registered for new events, if any.
|
||||
void
|
||||
clear_on_ready_callback() override
|
||||
{
|
||||
std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
|
||||
if (on_new_event_callback_) {
|
||||
set_on_new_event_callback(nullptr, nullptr);
|
||||
on_new_event_callback_ = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
protected:
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
set_on_new_event_callback(rcl_event_callback_t callback, const void * user_data);
|
||||
|
||||
std::recursive_mutex callback_mutex_;
|
||||
std::function<void(size_t)> on_new_event_callback_{nullptr};
|
||||
|
||||
rcl_event_t event_handle_;
|
||||
size_t wait_set_event_index_;
|
||||
};
|
||||
|
||||
using QOSEventHandlerBase [[deprecated("Use rclcpp::EventHandlerBase")]] = EventHandlerBase;
|
||||
|
||||
template<typename EventCallbackT, typename ParentHandleT>
|
||||
class EventHandler : public EventHandlerBase
|
||||
{
|
||||
public:
|
||||
template<typename InitFuncT, typename EventTypeEnum>
|
||||
EventHandler(
|
||||
const EventCallbackT & callback,
|
||||
InitFuncT init_func,
|
||||
ParentHandleT parent_handle,
|
||||
EventTypeEnum event_type)
|
||||
: parent_handle_(parent_handle), event_callback_(callback)
|
||||
{
|
||||
event_handle_ = rcl_get_zero_initialized_event();
|
||||
rcl_ret_t ret = init_func(&event_handle_, parent_handle.get(), event_type);
|
||||
if (ret != RCL_RET_OK) {
|
||||
if (ret == RCL_RET_UNSUPPORTED) {
|
||||
UnsupportedEventTypeException exc(ret, rcl_get_error_state(), "Failed to initialize event");
|
||||
rcl_reset_error();
|
||||
throw exc;
|
||||
} else {
|
||||
rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to initialize event");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Take data so that the callback cannot be scheduled again
|
||||
std::shared_ptr<void>
|
||||
take_data() override
|
||||
{
|
||||
EventCallbackInfoT callback_info;
|
||||
rcl_ret_t ret = rcl_take_event(&event_handle_, &callback_info);
|
||||
if (ret != RCL_RET_OK) {
|
||||
RCUTILS_LOG_ERROR_NAMED(
|
||||
"rclcpp",
|
||||
"Couldn't take event info: %s", rcl_get_error_string().str);
|
||||
return nullptr;
|
||||
}
|
||||
return std::static_pointer_cast<void>(std::make_shared<EventCallbackInfoT>(callback_info));
|
||||
}
|
||||
|
||||
std::shared_ptr<void>
|
||||
take_data_by_entity_id(size_t id) override
|
||||
{
|
||||
(void)id;
|
||||
return take_data();
|
||||
}
|
||||
|
||||
/// Execute any entities of the Waitable that are ready.
|
||||
void
|
||||
execute(std::shared_ptr<void> & data) override
|
||||
{
|
||||
if (!data) {
|
||||
throw std::runtime_error("'data' is empty");
|
||||
}
|
||||
auto callback_ptr = std::static_pointer_cast<EventCallbackInfoT>(data);
|
||||
event_callback_(*callback_ptr);
|
||||
callback_ptr.reset();
|
||||
}
|
||||
|
||||
private:
|
||||
using EventCallbackInfoT = typename std::remove_reference<typename
|
||||
rclcpp::function_traits::function_traits<EventCallbackT>::template argument_type<0>>::type;
|
||||
|
||||
ParentHandleT parent_handle_;
|
||||
EventCallbackT event_callback_;
|
||||
};
|
||||
|
||||
template<typename EventCallbackT, typename ParentHandleT>
|
||||
using QOSEventHandler [[deprecated("Use rclcpp::EventHandler")]] = EventHandler<EventCallbackT,
|
||||
ParentHandleT>;
|
||||
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__EVENT_HANDLER_HPP_
|
||||
@@ -19,7 +19,6 @@
|
||||
#include <cassert>
|
||||
#include <chrono>
|
||||
#include <cstdlib>
|
||||
#include <deque>
|
||||
#include <iostream>
|
||||
#include <list>
|
||||
#include <map>
|
||||
@@ -30,27 +29,28 @@
|
||||
|
||||
#include "rcl/guard_condition.h"
|
||||
#include "rcl/wait.h"
|
||||
#include "rclcpp/executors/executor_notify_waitable.hpp"
|
||||
#include "rcpputils/scope_exit.hpp"
|
||||
|
||||
#include "rclcpp/context.hpp"
|
||||
#include "rclcpp/contexts/default_context.hpp"
|
||||
#include "rclcpp/guard_condition.hpp"
|
||||
#include "rclcpp/executor_options.hpp"
|
||||
#include "rclcpp/executors/executor_entities_collection.hpp"
|
||||
#include "rclcpp/executors/executor_entities_collector.hpp"
|
||||
#include "rclcpp/future_return_code.hpp"
|
||||
#include "rclcpp/memory_strategies.hpp"
|
||||
#include "rclcpp/memory_strategy.hpp"
|
||||
#include "rclcpp/node_interfaces/node_base_interface.hpp"
|
||||
#include "rclcpp/utilities.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
#include "rclcpp/wait_set.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
|
||||
typedef std::map<rclcpp::CallbackGroup::WeakPtr,
|
||||
rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
|
||||
std::owner_less<rclcpp::CallbackGroup::WeakPtr>> WeakCallbackGroupsToNodesMap;
|
||||
|
||||
// Forward declaration is used in convenience method signature.
|
||||
class Node;
|
||||
class ExecutorImplementation;
|
||||
|
||||
/// Coordinate the order and timing of available communication tasks.
|
||||
/**
|
||||
@@ -315,16 +315,6 @@ public:
|
||||
virtual void
|
||||
spin_all(std::chrono::nanoseconds max_duration);
|
||||
|
||||
|
||||
/// Collect work once and execute the next available work, optionally within a duration.
|
||||
/**
|
||||
* This function can be overridden. The default implementation is suitable for
|
||||
* a single-thread model of execution.
|
||||
* Adding subscriptions, timers, services, etc. with blocking callbacks will cause this function
|
||||
* to block (which may have unintended consequences).
|
||||
* \param[in] timeout The maximum amount of time to spend waiting for work.
|
||||
* `-1` is potentially block forever waiting for work.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
virtual void
|
||||
spin_once(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
|
||||
@@ -402,6 +392,17 @@ public:
|
||||
void
|
||||
cancel();
|
||||
|
||||
/// Support dynamic switching of the memory strategy.
|
||||
/**
|
||||
* Switching the memory strategy while the executor is spinning in another threading could have
|
||||
* unintended consequences.
|
||||
* \param[in] memory_strategy Shared pointer to the memory strategy to set.
|
||||
* \throws std::runtime_error if memory_strategy is null
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
set_memory_strategy(memory_strategy::MemoryStrategy::SharedPtr memory_strategy);
|
||||
|
||||
/// Returns true if the executor is currently spinning.
|
||||
/**
|
||||
* This function can be called asynchronously from any thread.
|
||||
@@ -412,29 +413,12 @@ public:
|
||||
is_spinning();
|
||||
|
||||
protected:
|
||||
/// Add a node to executor, execute the next available unit of work, and remove the node.
|
||||
/**
|
||||
* Implementation of spin_node_once using std::chrono::nanoseconds
|
||||
* \param[in] node Shared pointer to the node to add.
|
||||
* \param[in] timeout How long to wait for work to become available. Negative values cause
|
||||
* spin_node_once to block indefinitely (the default behavior). A timeout of 0 causes this
|
||||
* function to be non-blocking.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
spin_node_once_nanoseconds(
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node,
|
||||
std::chrono::nanoseconds timeout);
|
||||
|
||||
/// Collect work and execute available work, optionally within a duration.
|
||||
/**
|
||||
* Implementation of spin_some and spin_all.
|
||||
* The exhaustive flag controls if the function will re-collect available work within the duration.
|
||||
*
|
||||
* \param[in] max_duration The maximum amount of time to spend executing work, or 0 for no limit.
|
||||
* \param[in] exhaustive when set to true, continue to collect work and execute (spin_all)
|
||||
* when set to false, return when all collected work is executed (spin_some)
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive);
|
||||
@@ -449,97 +433,124 @@ protected:
|
||||
void
|
||||
execute_any_executable(AnyExecutable & any_exec);
|
||||
|
||||
/// Run subscription executable.
|
||||
/**
|
||||
* Do necessary setup and tear-down as well as executing the subscription.
|
||||
* \param[in] subscription Subscription to execute
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
static void
|
||||
execute_subscription(
|
||||
rclcpp::SubscriptionBase::SharedPtr subscription);
|
||||
|
||||
/// Run timer executable.
|
||||
/**
|
||||
* Do necessary setup and tear-down as well as executing the timer callback.
|
||||
* \param[in] timer Timer to execute
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
static void
|
||||
execute_timer(rclcpp::TimerBase::SharedPtr timer);
|
||||
|
||||
/// Run service server executable.
|
||||
/**
|
||||
* Do necessary setup and tear-down as well as executing the service server callback.
|
||||
* \param[in] service Service to execute
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
static void
|
||||
execute_service(rclcpp::ServiceBase::SharedPtr service);
|
||||
|
||||
/// Run service client executable.
|
||||
/**
|
||||
* Do necessary setup and tear-down as well as executing the service client callback.
|
||||
* \param[in] service Service to execute
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
static void
|
||||
execute_client(rclcpp::ClientBase::SharedPtr client);
|
||||
|
||||
/// Gather all of the waitable entities from associated nodes and callback groups.
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
collect_entities();
|
||||
|
||||
/// Block until more work becomes avilable or timeout is reached.
|
||||
/**
|
||||
* Builds a set of waitable entities, which are passed to the middleware.
|
||||
* After building wait set, waits on middleware to notify.
|
||||
* \param[in] timeout duration to wait for new work to become available.
|
||||
* \throws std::runtime_error if the wait set can be cleared
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
|
||||
|
||||
/// Check for executable in ready state and populate union structure.
|
||||
RCLCPP_PUBLIC
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
|
||||
get_node_by_group(
|
||||
const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes,
|
||||
rclcpp::CallbackGroup::SharedPtr group);
|
||||
|
||||
/// Return true if the node has been added to this executor.
|
||||
/**
|
||||
* \param[out] any_executable populated union structure of ready executable
|
||||
* \return true if an executable was ready and any_executable was populated,
|
||||
* otherwise false
|
||||
* \param[in] node_ptr a shared pointer that points to a node base interface
|
||||
* \param[in] weak_groups_to_nodes map to nodes to lookup
|
||||
* \return true if the node is associated with the executor, otherwise false
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
has_node(
|
||||
const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
|
||||
const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
rclcpp::CallbackGroup::SharedPtr
|
||||
get_group_by_timer(rclcpp::TimerBase::SharedPtr timer);
|
||||
|
||||
/// Add a callback group to an executor
|
||||
/**
|
||||
* \see rclcpp::Executor::add_callback_group
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
virtual void
|
||||
add_callback_group_to_map(
|
||||
rclcpp::CallbackGroup::SharedPtr group_ptr,
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
|
||||
WeakCallbackGroupsToNodesMap & weak_groups_to_nodes,
|
||||
bool notify = true) RCPPUTILS_TSA_REQUIRES(mutex_);
|
||||
|
||||
/// Remove a callback group from the executor.
|
||||
/**
|
||||
* \see rclcpp::Executor::remove_callback_group
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
virtual void
|
||||
remove_callback_group_from_map(
|
||||
rclcpp::CallbackGroup::SharedPtr group_ptr,
|
||||
WeakCallbackGroupsToNodesMap & weak_groups_to_nodes,
|
||||
bool notify = true) RCPPUTILS_TSA_REQUIRES(mutex_);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
get_next_ready_executable(AnyExecutable & any_executable);
|
||||
|
||||
/// Wait for executable in ready state and populate union structure.
|
||||
/**
|
||||
* If an executable is ready, it will return immediately, otherwise
|
||||
* block based on the timeout for work to become ready.
|
||||
*
|
||||
* \param[out] any_executable populated union structure of ready executable
|
||||
* \param[in] timeout duration of time to wait for work, a negative value
|
||||
* (the defualt behavior), will make this function block indefinitely
|
||||
* \return true if an executable was ready and any_executable was populated,
|
||||
* otherwise false
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
get_next_ready_executable_from_map(
|
||||
AnyExecutable & any_executable,
|
||||
const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
get_next_executable(
|
||||
AnyExecutable & any_executable,
|
||||
std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
|
||||
|
||||
/// Add all callback groups that can be automatically added from associated nodes.
|
||||
/**
|
||||
* The executor, before collecting entities, verifies if any callback group from
|
||||
* nodes associated with the executor, which is not already associated to an executor,
|
||||
* can be automatically added to this executor.
|
||||
* This takes care of any callback group that has been added to a node but not explicitly added
|
||||
* to the executor.
|
||||
* It is important to note that in order for the callback groups to be automatically added to an
|
||||
* executor through this function, the node of the callback groups needs to have been added
|
||||
* through the `add_node` method.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
virtual void
|
||||
add_callback_groups_from_nodes_associated_to_executor() RCPPUTILS_TSA_REQUIRES(mutex_);
|
||||
|
||||
/// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins.
|
||||
std::atomic_bool spinning;
|
||||
|
||||
/// Guard condition for signaling the rmw layer to wake up for special events.
|
||||
std::shared_ptr<rclcpp::GuardCondition> interrupt_guard_condition_;
|
||||
rclcpp::GuardCondition interrupt_guard_condition_;
|
||||
|
||||
/// Guard condition for signaling the rmw layer to wake up for system shutdown.
|
||||
std::shared_ptr<rclcpp::GuardCondition> shutdown_guard_condition_;
|
||||
|
||||
/// Wait set for managing entities that the rmw layer waits on.
|
||||
rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set();
|
||||
|
||||
// Mutex to protect the subsequent memory_strategy_.
|
||||
mutable std::mutex mutex_;
|
||||
|
||||
/// The memory strategy: an interface for handling user-defined memory allocation strategies.
|
||||
memory_strategy::MemoryStrategy::SharedPtr
|
||||
memory_strategy_ RCPPUTILS_TSA_PT_GUARDED_BY(mutex_);
|
||||
|
||||
/// The context associated with this executor.
|
||||
std::shared_ptr<rclcpp::Context> context_;
|
||||
|
||||
@@ -549,39 +560,33 @@ protected:
|
||||
virtual void
|
||||
spin_once_impl(std::chrono::nanoseconds timeout);
|
||||
|
||||
/// Waitable containing guard conditions controlling the executor flow.
|
||||
/**
|
||||
* This waitable contains the interrupt and shutdown guard condition, as well
|
||||
* as the guard condition associated with each node and callback group.
|
||||
* By default, if any change is detected in the monitored entities, the notify
|
||||
* waitable will awake the executor and rebuild the collections.
|
||||
*/
|
||||
std::shared_ptr<rclcpp::executors::ExecutorNotifyWaitable> notify_waitable_;
|
||||
typedef std::map<rclcpp::CallbackGroup::WeakPtr,
|
||||
const rclcpp::GuardCondition *,
|
||||
std::owner_less<rclcpp::CallbackGroup::WeakPtr>>
|
||||
WeakCallbackGroupsToGuardConditionsMap;
|
||||
|
||||
std::atomic_bool entities_need_rebuild_;
|
||||
/// maps callback groups to guard conditions
|
||||
WeakCallbackGroupsToGuardConditionsMap
|
||||
weak_groups_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
|
||||
|
||||
/// Collector used to associate executable entities from nodes and guard conditions
|
||||
rclcpp::executors::ExecutorEntitiesCollector collector_;
|
||||
/// maps callback groups associated to nodes
|
||||
WeakCallbackGroupsToNodesMap
|
||||
weak_groups_associated_with_executor_to_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
|
||||
|
||||
/// Waitset to be waited on.
|
||||
rclcpp::WaitSet wait_set_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
|
||||
/// maps callback groups to nodes associated with executor
|
||||
WeakCallbackGroupsToNodesMap
|
||||
weak_groups_to_nodes_associated_with_executor_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
|
||||
|
||||
/// Hold the current state of the collection being waited on by the waitset
|
||||
rclcpp::executors::ExecutorEntitiesCollection current_collection_ RCPPUTILS_TSA_GUARDED_BY(
|
||||
mutex_);
|
||||
/// maps all callback groups to nodes
|
||||
WeakCallbackGroupsToNodesMap
|
||||
weak_groups_to_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
|
||||
|
||||
/// Hold the current state of the notify waitable being waited on by the waitset
|
||||
std::shared_ptr<rclcpp::executors::ExecutorNotifyWaitable> current_notify_waitable_
|
||||
RCPPUTILS_TSA_GUARDED_BY(mutex_);
|
||||
|
||||
/// Hold the list of executables currently available to be executed.
|
||||
std::deque<rclcpp::AnyExecutable> ready_executables_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
|
||||
/// nodes that are associated with the executor
|
||||
std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>
|
||||
weak_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
|
||||
|
||||
/// shutdown callback handle registered to Context
|
||||
rclcpp::OnShutdownCallbackHandle shutdown_callback_handle_;
|
||||
|
||||
/// Pointer to implementation
|
||||
std::unique_ptr<ExecutorImplementation> impl_;
|
||||
};
|
||||
|
||||
} // namespace rclcpp
|
||||
|
||||
@@ -21,7 +21,6 @@
|
||||
#include "rclcpp/executors/multi_threaded_executor.hpp"
|
||||
#include "rclcpp/executors/single_threaded_executor.hpp"
|
||||
#include "rclcpp/executors/static_single_threaded_executor.hpp"
|
||||
#include "rclcpp/experimental/executors/events_executor/events_executor.hpp"
|
||||
#include "rclcpp/node.hpp"
|
||||
#include "rclcpp/utilities.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
@@ -1,213 +0,0 @@
|
||||
// Copyright 2023 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__EXECUTOR_ENTITIES_COLLECTION_HPP_
|
||||
#define RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_
|
||||
|
||||
#include <deque>
|
||||
#include <functional>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
#include <rclcpp/any_executable.hpp>
|
||||
#include <rclcpp/node_interfaces/node_base.hpp>
|
||||
#include <rclcpp/callback_group.hpp>
|
||||
#include <rclcpp/executors/executor_notify_waitable.hpp>
|
||||
#include <rclcpp/visibility_control.hpp>
|
||||
#include <rclcpp/wait_result.hpp>
|
||||
#include <rclcpp/wait_set.hpp>
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace executors
|
||||
{
|
||||
|
||||
/// Structure to represent a single entity's entry in a collection
|
||||
template<typename EntityValueType>
|
||||
struct CollectionEntry
|
||||
{
|
||||
/// Weak pointer to entity type
|
||||
using EntityWeakPtr = typename EntityValueType::WeakPtr;
|
||||
/// Shared pointer to entity type
|
||||
using EntitySharedPtr = typename EntityValueType::SharedPtr;
|
||||
|
||||
/// The entity
|
||||
EntityWeakPtr entity;
|
||||
|
||||
/// If relevant, the entity's corresponding callback_group
|
||||
rclcpp::CallbackGroup::WeakPtr callback_group;
|
||||
};
|
||||
|
||||
/// Update a collection based on another collection
|
||||
/*
|
||||
* Iterates update_from and update_to to see which entities have been added/removed between
|
||||
* the two collections.
|
||||
*
|
||||
* For each new entry (in update_from, but not in update_to),
|
||||
* add the entity and fire the on_added callback
|
||||
* For each removed entry (in update_to, but not in update_from),
|
||||
* remove the entity and fire the on_removed callback.
|
||||
*
|
||||
* \param[in] update_from The collection representing the next iteration's state
|
||||
* \param[inout] update_to The collection representing the current iteration's state
|
||||
* \param[in] on_added Callback fired when a new entity is detected
|
||||
* \param[in] on_removed Callback fired when an entity is removed
|
||||
*/
|
||||
template<typename CollectionType>
|
||||
void update_entities(
|
||||
const CollectionType & update_from,
|
||||
CollectionType & update_to,
|
||||
std::function<void(const typename CollectionType::EntitySharedPtr &)> on_added,
|
||||
std::function<void(const typename CollectionType::EntitySharedPtr &)> on_removed
|
||||
)
|
||||
{
|
||||
for (auto it = update_to.begin(); it != update_to.end(); ) {
|
||||
if (update_from.count(it->first) == 0) {
|
||||
auto entity = it->second.entity.lock();
|
||||
if (entity) {
|
||||
on_removed(entity);
|
||||
}
|
||||
it = update_to.erase(it);
|
||||
} else {
|
||||
++it;
|
||||
}
|
||||
}
|
||||
for (auto it = update_from.begin(); it != update_from.end(); ++it) {
|
||||
if (update_to.count(it->first) == 0) {
|
||||
auto entity = it->second.entity.lock();
|
||||
if (entity) {
|
||||
on_added(entity);
|
||||
}
|
||||
update_to.insert(*it);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// A collection of entities, indexed by their corresponding handles
|
||||
template<typename EntityKeyType, typename EntityValueType>
|
||||
class EntityCollection
|
||||
: public std::unordered_map<const EntityKeyType *, CollectionEntry<EntityValueType>>
|
||||
{
|
||||
public:
|
||||
/// Key type of the map
|
||||
using Key = const EntityKeyType *;
|
||||
|
||||
/// Weak pointer to entity type
|
||||
using EntityWeakPtr = typename EntityValueType::WeakPtr;
|
||||
|
||||
/// Shared pointer to entity type
|
||||
using EntitySharedPtr = typename EntityValueType::SharedPtr;
|
||||
|
||||
/// Update this collection based on the contents of another collection
|
||||
/**
|
||||
* Update the internal state of this collection, firing callbacks when entities have been
|
||||
* added or removed.
|
||||
*
|
||||
* \param[in] other Collection to compare to
|
||||
* \param[in] on_added Callback for when entities have been added
|
||||
* \param[in] on_removed Callback for when entities have been removed
|
||||
*/
|
||||
void update(
|
||||
const EntityCollection<EntityKeyType, EntityValueType> & other,
|
||||
std::function<void(const EntitySharedPtr &)> on_added,
|
||||
std::function<void(const EntitySharedPtr &)> on_removed)
|
||||
{
|
||||
update_entities(other, *this, on_added, on_removed);
|
||||
}
|
||||
};
|
||||
|
||||
/// Represent the total set of entities for a single executor
|
||||
/**
|
||||
* This allows the entities to be stored from ExecutorEntitiesCollector.
|
||||
* The structure also makes in convenient to re-evaluate when entities have been added or removed.
|
||||
*/
|
||||
struct ExecutorEntitiesCollection
|
||||
{
|
||||
/// Collection type for timer entities
|
||||
using TimerCollection = EntityCollection<rcl_timer_t, rclcpp::TimerBase>;
|
||||
|
||||
/// Collection type for subscription entities
|
||||
using SubscriptionCollection = EntityCollection<rcl_subscription_t, rclcpp::SubscriptionBase>;
|
||||
|
||||
/// Collection type for client entities
|
||||
using ClientCollection = EntityCollection<rcl_client_t, rclcpp::ClientBase>;
|
||||
|
||||
/// Collection type for service entities
|
||||
using ServiceCollection = EntityCollection<rcl_service_t, rclcpp::ServiceBase>;
|
||||
|
||||
/// Collection type for waitable entities
|
||||
using WaitableCollection = EntityCollection<rclcpp::Waitable, rclcpp::Waitable>;
|
||||
|
||||
/// Collection type for guard condition entities
|
||||
using GuardConditionCollection = EntityCollection<rcl_guard_condition_t, rclcpp::GuardCondition>;
|
||||
|
||||
/// Collection of timers currently in use by the executor.
|
||||
TimerCollection timers;
|
||||
|
||||
/// Collection of subscriptions currently in use by the executor.
|
||||
SubscriptionCollection subscriptions;
|
||||
|
||||
/// Collection of clients currently in use by the executor.
|
||||
ClientCollection clients;
|
||||
|
||||
/// Collection of services currently in use by the executor.
|
||||
ServiceCollection services;
|
||||
|
||||
/// Collection of guard conditions currently in use by the executor.
|
||||
GuardConditionCollection guard_conditions;
|
||||
|
||||
/// Collection of waitables currently in use by the executor.
|
||||
WaitableCollection waitables;
|
||||
|
||||
/// Check if the entities collection is empty
|
||||
/**
|
||||
* \return true if all member collections are empty, false otherwise
|
||||
*/
|
||||
bool empty() const;
|
||||
|
||||
/// Clear the entities collection
|
||||
void clear();
|
||||
};
|
||||
|
||||
/// Build an entities collection from callback groups
|
||||
/**
|
||||
* Iterates a list of callback groups and adds entities from each valid group
|
||||
*
|
||||
* \param[in] callback_groups List of callback groups to check for entities
|
||||
* \param[inout] colletion Entities collection to populate with found entities
|
||||
*/
|
||||
void
|
||||
build_entities_collection(
|
||||
const std::vector<rclcpp::CallbackGroup::WeakPtr> & callback_groups,
|
||||
ExecutorEntitiesCollection & collection);
|
||||
|
||||
/// Build a queue of executables ready to be executed
|
||||
/**
|
||||
* Iterates a list of entities and adds them to a queue if they are ready.
|
||||
*
|
||||
* \param[in] collection Collection of entities corresponding to the current wait set.
|
||||
* \param[in] wait_result Result of rclcpp::WaitSet::wait corresponding to the collection.
|
||||
* \param[inout] queue of executables to append new ready executables to
|
||||
* \return number of new ready executables
|
||||
*/
|
||||
size_t
|
||||
ready_executables(
|
||||
const ExecutorEntitiesCollection & collection,
|
||||
rclcpp::WaitResult<rclcpp::WaitSet> & wait_result,
|
||||
std::deque<rclcpp::AnyExecutable> & executables
|
||||
);
|
||||
} // namespace executors
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_
|
||||
@@ -1,270 +0,0 @@
|
||||
// Copyright 2023 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__EXECUTOR_ENTITIES_COLLECTOR_HPP_
|
||||
#define RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTOR_HPP_
|
||||
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <set>
|
||||
#include <vector>
|
||||
|
||||
#include "rcpputils/thread_safety_annotations.hpp"
|
||||
|
||||
#include <rclcpp/any_executable.hpp>
|
||||
#include <rclcpp/node_interfaces/node_base.hpp>
|
||||
#include <rclcpp/callback_group.hpp>
|
||||
#include <rclcpp/executors/executor_notify_waitable.hpp>
|
||||
#include <rclcpp/visibility_control.hpp>
|
||||
#include <rclcpp/wait_set.hpp>
|
||||
#include <rclcpp/wait_result.hpp>
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace executors
|
||||
{
|
||||
|
||||
/// Class to monitor a set of nodes and callback groups for changes in entity membership
|
||||
/**
|
||||
* This is to be used with an executor to track the membership of various nodes, groups,
|
||||
* and entities (timers, subscriptions, clients, services, etc) and report status to the
|
||||
* executor.
|
||||
*
|
||||
* In general, users will add either nodes or callback groups to an executor.
|
||||
* Each node may have callback groups that are automatically associated with executors,
|
||||
* or callback groups that must be manually associated with an executor.
|
||||
*
|
||||
* This object tracks both types of callback groups as well as nodes that have been
|
||||
* previously added to the executor.
|
||||
* When a new callback group is added/removed or new entities are added/removed, the
|
||||
* corresponding node or callback group will signal this to the executor so that the
|
||||
* entity collection may be rebuilt according to that executor's implementation.
|
||||
*
|
||||
*/
|
||||
class ExecutorEntitiesCollector
|
||||
{
|
||||
public:
|
||||
/// Constructor
|
||||
/**
|
||||
* \param[in] notify_waitable Waitable that is used to signal to the executor
|
||||
* when nodes or callback groups have been added or removed.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
explicit ExecutorEntitiesCollector(
|
||||
std::shared_ptr<ExecutorNotifyWaitable> notify_waitable);
|
||||
|
||||
/// Destructor
|
||||
RCLCPP_PUBLIC
|
||||
~ExecutorEntitiesCollector();
|
||||
|
||||
/// Indicate if the entities collector has pending additions or removals.
|
||||
/**
|
||||
* \return true if there are pending additions or removals
|
||||
*/
|
||||
bool has_pending() const;
|
||||
|
||||
/// Add a node to the entity collector
|
||||
/**
|
||||
* \param[in] node_ptr a shared pointer that points to a node base interface
|
||||
* \throw std::runtime_error if the node is associated with an executor
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
|
||||
|
||||
/// Remove a node from the entity collector
|
||||
/**
|
||||
* \param[in] node_ptr a shared pointer that points to a node base interface
|
||||
* \throw std::runtime_error if the node is associated with an executor
|
||||
* \throw std::runtime_error if the node is associated with this executor
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
|
||||
|
||||
/// Add a callback group to the entity collector
|
||||
/**
|
||||
* \param[in] group_ptr a shared pointer that points to a callback group
|
||||
* \throw std::runtime_error if the callback_group is associated with an executor
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_callback_group(rclcpp::CallbackGroup::SharedPtr group_ptr);
|
||||
|
||||
/// Remove a callback group from the entity collector
|
||||
/**
|
||||
* \param[in] group_ptr a shared pointer that points to a callback group
|
||||
* \throw std::runtime_error if the callback_group is not associated with an executor
|
||||
* \throw std::runtime_error if the callback_group is not associated with this executor
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
remove_callback_group(rclcpp::CallbackGroup::SharedPtr group_ptr);
|
||||
|
||||
/// Get all callback groups known to this entity collector
|
||||
/**
|
||||
* This will include manually added and automatically added (node associated) groups
|
||||
* \return vector of all callback groups
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr>
|
||||
get_all_callback_groups() const;
|
||||
|
||||
/// Get manually-added callback groups known to this entity collector
|
||||
/**
|
||||
* This will include callback groups that have been added via add_callback_group
|
||||
* \return vector of manually-added callback groups
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr>
|
||||
get_manually_added_callback_groups() const;
|
||||
|
||||
/// Get automatically-added callback groups known to this entity collector
|
||||
/**
|
||||
* This will include callback groups that are associated with nodes added via add_node
|
||||
* \return vector of automatically-added callback groups
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr>
|
||||
get_automatically_added_callback_groups() const;
|
||||
|
||||
/// Update the underlying collections
|
||||
/**
|
||||
* This will prune nodes and callback groups that are no longer valid as well
|
||||
* as add new callback groups from any associated nodes.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
update_collections();
|
||||
|
||||
protected:
|
||||
using NodeCollection = std::set<
|
||||
rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
|
||||
std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>;
|
||||
|
||||
using CallbackGroupCollection = std::set<
|
||||
rclcpp::CallbackGroup::WeakPtr,
|
||||
std::owner_less<rclcpp::CallbackGroup::WeakPtr>>;
|
||||
|
||||
using WeakNodesToGuardConditionsMap = std::map<
|
||||
rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
|
||||
rclcpp::GuardCondition::WeakPtr,
|
||||
std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>;
|
||||
|
||||
using WeakGroupsToGuardConditionsMap = std::map<
|
||||
rclcpp::CallbackGroup::WeakPtr,
|
||||
rclcpp::GuardCondition::WeakPtr,
|
||||
std::owner_less<rclcpp::CallbackGroup::WeakPtr>>;
|
||||
|
||||
/// Implementation of removing a node from the collector.
|
||||
/**
|
||||
* This will disassociate the node from the collector and remove any
|
||||
* automatically-added callback groups
|
||||
*
|
||||
* This takes and returns an iterator so it may be used as:
|
||||
*
|
||||
* it = remove_weak_node(it);
|
||||
*
|
||||
* \param[in] weak_node iterator to the weak node to be removed
|
||||
* \return Valid updated iterator in the same collection
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
NodeCollection::iterator
|
||||
remove_weak_node(NodeCollection::iterator weak_node) RCPPUTILS_TSA_REQUIRES(mutex_);
|
||||
|
||||
/// Implementation of removing a callback group from the collector.
|
||||
/**
|
||||
* This will disassociate the callback group from the collector
|
||||
*
|
||||
* This takes and returns an iterator so it may be used as:
|
||||
*
|
||||
* it = remove_weak_callback_group(it);
|
||||
*
|
||||
* \param[in] weak_group_it iterator to the weak group to be removed
|
||||
* \param[in] collection the collection to remove the group from
|
||||
* (manually or automatically added)
|
||||
* \return Valid updated iterator in the same collection
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
CallbackGroupCollection::iterator
|
||||
remove_weak_callback_group(
|
||||
CallbackGroupCollection::iterator weak_group_it,
|
||||
CallbackGroupCollection & collection) RCPPUTILS_TSA_REQUIRES(mutex_);
|
||||
|
||||
/// Implementation of adding a callback group
|
||||
/**
|
||||
* \param[in] group_ptr the group to add
|
||||
* \param[in] collection the collection to add the group to
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_callback_group_to_collection(
|
||||
rclcpp::CallbackGroup::SharedPtr group_ptr,
|
||||
CallbackGroupCollection & collection) RCPPUTILS_TSA_REQUIRES(mutex_);
|
||||
|
||||
/// Iterate over queued added/remove nodes and callback_groups
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
process_queues() RCPPUTILS_TSA_REQUIRES(mutex_);
|
||||
|
||||
/// Check a collection of nodes and add any new callback_groups that
|
||||
/// are set to be automatically associated via the node.
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_automatically_associated_callback_groups(
|
||||
const NodeCollection & nodes_to_check) RCPPUTILS_TSA_REQUIRES(mutex_);
|
||||
|
||||
/// Check all nodes and group for expired weak pointers and remove them.
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
prune_invalid_nodes_and_groups() RCPPUTILS_TSA_REQUIRES(mutex_);
|
||||
|
||||
/// mutex to protect collections and pending queues
|
||||
mutable std::mutex mutex_;
|
||||
|
||||
/// Callback groups that were added via `add_callback_group`
|
||||
CallbackGroupCollection manually_added_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
|
||||
|
||||
/// Callback groups that were added by their association with added nodes
|
||||
CallbackGroupCollection automatically_added_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
|
||||
|
||||
/// nodes that are associated with the executor
|
||||
NodeCollection weak_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
|
||||
|
||||
/// Track guard conditions associated with added nodes
|
||||
WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
|
||||
|
||||
/// Track guard conditions associated with added callback groups
|
||||
WeakGroupsToGuardConditionsMap weak_groups_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
|
||||
|
||||
/// nodes that have been added since the last update.
|
||||
NodeCollection pending_added_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
|
||||
|
||||
/// nodes that have been removed since the last update.
|
||||
NodeCollection pending_removed_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
|
||||
|
||||
/// callback groups that have been added since the last update.
|
||||
CallbackGroupCollection pending_manually_added_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
|
||||
|
||||
/// callback groups that have been removed since the last update.
|
||||
CallbackGroupCollection pending_manually_removed_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
|
||||
|
||||
/// Waitable to add guard conditions to
|
||||
std::shared_ptr<ExecutorNotifyWaitable> notify_waitable_;
|
||||
};
|
||||
} // namespace executors
|
||||
} // namespace rclcpp
|
||||
//
|
||||
#endif // RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTOR_HPP_
|
||||
@@ -1,158 +0,0 @@
|
||||
// Copyright 2023 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__EXECUTOR_NOTIFY_WAITABLE_HPP_
|
||||
#define RCLCPP__EXECUTORS__EXECUTOR_NOTIFY_WAITABLE_HPP_
|
||||
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <set>
|
||||
|
||||
#include "rclcpp/guard_condition.hpp"
|
||||
#include "rclcpp/waitable.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace executors
|
||||
{
|
||||
|
||||
/// Maintain a collection of guard conditions from associated nodes and callback groups
|
||||
/// to signal to the executor when associated entities have changed.
|
||||
class ExecutorNotifyWaitable : public rclcpp::Waitable
|
||||
{
|
||||
public:
|
||||
RCLCPP_SMART_PTR_DEFINITIONS(ExecutorNotifyWaitable)
|
||||
|
||||
// Constructor
|
||||
/**
|
||||
* \param[in] on_execute_callback Callback to execute when one of the conditions
|
||||
* of this waitable has signaled the wait_set.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
explicit ExecutorNotifyWaitable(std::function<void(void)> on_execute_callback = {});
|
||||
|
||||
// Destructor
|
||||
RCLCPP_PUBLIC
|
||||
~ExecutorNotifyWaitable() override = default;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
ExecutorNotifyWaitable(const ExecutorNotifyWaitable & other);
|
||||
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
ExecutorNotifyWaitable & operator=(const ExecutorNotifyWaitable & other);
|
||||
|
||||
/// Add conditions to the wait set
|
||||
/**
|
||||
* \param[inout] wait_set structure that conditions will be added to
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_to_wait_set(rcl_wait_set_t * wait_set) override;
|
||||
|
||||
/// Check conditions against the wait set
|
||||
/**
|
||||
* \param[inout] wait_set structure that internal elements will be checked against.
|
||||
* \return true if this waitable is ready to be executed, false otherwise.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
is_ready(rcl_wait_set_t * wait_set) override;
|
||||
|
||||
/// Perform work associated with the waitable.
|
||||
/**
|
||||
* This will call the callback provided in the constructor.
|
||||
* \param[in] data Data to be use for the execute, if available, else nullptr.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
execute(std::shared_ptr<void> & data) override;
|
||||
|
||||
/// Retrieve data to be used in the next execute call.
|
||||
/**
|
||||
* \return If available, data to be used, otherwise nullptr
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
std::shared_ptr<void>
|
||||
take_data() override;
|
||||
|
||||
/// Take the data from an entity ID so that it can be consumed with `execute`.
|
||||
/**
|
||||
* \param[in] id ID of the entity to take data from.
|
||||
* \return If available, data to be used, otherwise nullptr
|
||||
* \sa rclcpp::Waitable::take_data_by_entity_id
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
std::shared_ptr<void>
|
||||
take_data_by_entity_id(size_t id) override;
|
||||
|
||||
/// Set a callback to be called whenever the waitable becomes ready.
|
||||
/**
|
||||
* \param[in] callback callback to set
|
||||
* \sa rclcpp::Waitable::set_on_ready_callback
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
set_on_ready_callback(std::function<void(size_t, int)> callback) override;
|
||||
|
||||
/// Add a guard condition to be waited on.
|
||||
/**
|
||||
* \param[in] guard_condition The guard condition to add.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_guard_condition(rclcpp::GuardCondition::WeakPtr guard_condition);
|
||||
|
||||
/// Unset any callback registered via set_on_ready_callback.
|
||||
/**
|
||||
* \sa rclcpp::Waitable::clear_on_ready_callback
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
clear_on_ready_callback() override;
|
||||
|
||||
/// Remove a guard condition from being waited on.
|
||||
/**
|
||||
* \param[in] weak_guard_condition The guard condition to remove.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
remove_guard_condition(rclcpp::GuardCondition::WeakPtr weak_guard_condition);
|
||||
|
||||
/// Get the number of ready guard_conditions
|
||||
/**
|
||||
* \return The number of guard_conditions associated with the Waitable.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
size_t
|
||||
get_number_of_ready_guard_conditions() override;
|
||||
|
||||
private:
|
||||
/// Callback to run when waitable executes
|
||||
std::function<void(void)> execute_callback_;
|
||||
|
||||
std::mutex guard_condition_mutex_;
|
||||
|
||||
std::function<void(size_t)> on_ready_callback_;
|
||||
|
||||
/// The collection of guard conditions to be waited on.
|
||||
std::set<rclcpp::GuardCondition::WeakPtr,
|
||||
std::owner_less<rclcpp::GuardCondition::WeakPtr>> notify_guard_conditions_;
|
||||
};
|
||||
|
||||
} // namespace executors
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__EXECUTORS__EXECUTOR_NOTIFY_WAITABLE_HPP_
|
||||
@@ -0,0 +1,357 @@
|
||||
// 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();
|
||||
|
||||
/// Initialize StaticExecutorEntitiesCollector
|
||||
/**
|
||||
* \param p_wait_set A reference to the wait set to be used in the executor
|
||||
* \param memory_strategy Shared pointer to the memory strategy to set.
|
||||
* \throws std::runtime_error if memory strategy is null
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
init(
|
||||
rcl_wait_set_t * p_wait_set,
|
||||
rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy);
|
||||
|
||||
/// Finalize StaticExecutorEntitiesCollector to clear resources
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
is_init() {return initialized_;}
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
fini();
|
||||
|
||||
/// Execute the waitable.
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
execute(std::shared_ptr<void> & data) override;
|
||||
|
||||
/// Take the data so that it can be consumed with `execute`.
|
||||
/**
|
||||
* For `StaticExecutorEntitiesCollector`, this always return `nullptr`.
|
||||
* \sa rclcpp::Waitable::take_data()
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
std::shared_ptr<void>
|
||||
take_data() override;
|
||||
|
||||
/// Function to add_handles_to_wait_set and wait for work and
|
||||
/**
|
||||
* block until the wait set is ready or until the timeout has been exceeded.
|
||||
* \throws std::runtime_error if wait set couldn't be cleared or filled.
|
||||
* \throws any rcl errors from rcl_wait, \see rclcpp::exceptions::throw_from_rcl_error()
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
refresh_wait_set(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
|
||||
|
||||
/**
|
||||
* \throws std::runtime_error if it couldn't add guard condition to wait set
|
||||
*/
|
||||
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;
|
||||
|
||||
/// Add a callback group to an executor.
|
||||
/**
|
||||
* \see rclcpp::Executor::add_callback_group
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
add_callback_group(
|
||||
rclcpp::CallbackGroup::SharedPtr group_ptr,
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
|
||||
|
||||
/// Add a callback group to an executor.
|
||||
/**
|
||||
* \see rclcpp::Executor::add_callback_group
|
||||
* \return boolean whether the node from the callback group is new
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
add_callback_group(
|
||||
rclcpp::CallbackGroup::SharedPtr group_ptr,
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
|
||||
WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
|
||||
|
||||
/// Remove a callback group from the executor.
|
||||
/**
|
||||
* \see rclcpp::Executor::remove_callback_group
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
remove_callback_group(
|
||||
rclcpp::CallbackGroup::SharedPtr group_ptr);
|
||||
|
||||
/// Remove a callback group from the executor.
|
||||
/**
|
||||
* \see rclcpp::Executor::remove_callback_group_from_map
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
remove_callback_group_from_map(
|
||||
rclcpp::CallbackGroup::SharedPtr group_ptr,
|
||||
WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
|
||||
|
||||
/**
|
||||
* \see rclcpp::Executor::add_node()
|
||||
* \throw std::runtime_error if node was already added
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
add_node(
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
|
||||
|
||||
/**
|
||||
* \see rclcpp::Executor::remove_node()
|
||||
* \throw std::runtime_error if no guard condition is associated with node.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
remove_node(
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr>
|
||||
get_all_callback_groups();
|
||||
|
||||
/// Get callback groups that belong to executor.
|
||||
/**
|
||||
* \see rclcpp::Executor::get_manually_added_callback_groups()
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr>
|
||||
get_manually_added_callback_groups();
|
||||
|
||||
/// Get callback groups that belong to executor.
|
||||
/**
|
||||
* \see rclcpp::Executor::get_automatically_added_callback_groups_from_nodes()
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr>
|
||||
get_automatically_added_callback_groups_from_nodes();
|
||||
|
||||
/// Complete all available queued work without blocking.
|
||||
/**
|
||||
* This function checks if after the guard condition was triggered
|
||||
* (or a spurious wakeup happened) we are really ready to execute
|
||||
* i.e. re-collect entities
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
is_ready(rcl_wait_set_t * wait_set) override;
|
||||
|
||||
/// Return number of timers
|
||||
/**
|
||||
* \return number of timers
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
size_t
|
||||
get_number_of_timers() {return exec_list_.number_of_timers;}
|
||||
|
||||
/// Return number of subscriptions
|
||||
/**
|
||||
* \return number of subscriptions
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
size_t
|
||||
get_number_of_subscriptions() {return exec_list_.number_of_subscriptions;}
|
||||
|
||||
/// Return number of services
|
||||
/**
|
||||
* \return number of services
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
size_t
|
||||
get_number_of_services() {return exec_list_.number_of_services;}
|
||||
|
||||
/// Return number of clients
|
||||
/**
|
||||
* \return number of clients
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
size_t
|
||||
get_number_of_clients() {return exec_list_.number_of_clients;}
|
||||
|
||||
/// Return number of waitables
|
||||
/**
|
||||
* \return number of waitables
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
size_t
|
||||
get_number_of_waitables() {return exec_list_.number_of_waitables;}
|
||||
|
||||
/** Return a SubscritionBase Sharedptr by index.
|
||||
* \param[in] i The index of the SubscritionBase
|
||||
* \return a SubscritionBase shared pointer
|
||||
* \throws std::out_of_range if the argument is higher than the size of the structrue.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
rclcpp::SubscriptionBase::SharedPtr
|
||||
get_subscription(size_t i) {return exec_list_.subscription[i];}
|
||||
|
||||
/** Return a TimerBase Sharedptr by index.
|
||||
* \param[in] i The index of the TimerBase
|
||||
* \return a TimerBase shared pointer
|
||||
* \throws std::out_of_range if the argument is higher than the size.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
rclcpp::TimerBase::SharedPtr
|
||||
get_timer(size_t i) {return exec_list_.timer[i];}
|
||||
|
||||
/** Return a ServiceBase Sharedptr by index.
|
||||
* \param[in] i The index of the ServiceBase
|
||||
* \return a ServiceBase shared pointer
|
||||
* \throws std::out_of_range if the argument is higher than the size.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
rclcpp::ServiceBase::SharedPtr
|
||||
get_service(size_t i) {return exec_list_.service[i];}
|
||||
|
||||
/** Return a ClientBase Sharedptr by index
|
||||
* \param[in] i The index of the ClientBase
|
||||
* \return a ClientBase shared pointer
|
||||
* \throws std::out_of_range if the argument is higher than the size.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
rclcpp::ClientBase::SharedPtr
|
||||
get_client(size_t i) {return exec_list_.client[i];}
|
||||
|
||||
/** Return a Waitable Sharedptr by index
|
||||
* \param[in] i The index of the Waitable
|
||||
* \return a Waitable shared pointer
|
||||
* \throws std::out_of_range if the argument is higher than the size.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
rclcpp::Waitable::SharedPtr
|
||||
get_waitable(size_t i) {return exec_list_.waitable[i];}
|
||||
|
||||
private:
|
||||
/// Function to reallocate space for entities in the wait set.
|
||||
/**
|
||||
* \throws std::runtime_error if wait set couldn't be cleared or resized.
|
||||
*/
|
||||
void
|
||||
prepare_wait_set();
|
||||
|
||||
void
|
||||
fill_executable_list();
|
||||
|
||||
void
|
||||
fill_memory_strategy();
|
||||
|
||||
/// Return true if the node belongs to the collector
|
||||
/**
|
||||
* \param[in] node_ptr a node base interface shared pointer
|
||||
* \param[in] weak_groups_to_nodes map to nodes to lookup
|
||||
* \return boolean whether a node belongs the collector
|
||||
*/
|
||||
bool
|
||||
has_node(
|
||||
const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
|
||||
const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) const;
|
||||
|
||||
/// Add all callback groups that can be automatically added by any executor
|
||||
/// and is not already associated with an executor from nodes
|
||||
/// that are associated with executor
|
||||
/**
|
||||
* \see rclcpp::Executor::add_callback_groups_from_nodes_associated_to_executor()
|
||||
*/
|
||||
void
|
||||
add_callback_groups_from_nodes_associated_to_executor();
|
||||
|
||||
void
|
||||
fill_executable_list_from_map(const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
|
||||
|
||||
/// Memory strategy: an interface for handling user-defined memory allocation strategies.
|
||||
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_;
|
||||
|
||||
/// List of weak nodes registered in the static executor
|
||||
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_;
|
||||
|
||||
/// Wait set for managing entities that the rmw layer waits on.
|
||||
rcl_wait_set_t * p_wait_set_ = nullptr;
|
||||
|
||||
/// Executable list: timers, subscribers, clients, services and waitables
|
||||
rclcpp::experimental::ExecutableList exec_list_;
|
||||
|
||||
/// Bool to check if the entities collector has been initialized
|
||||
bool initialized_ = false;
|
||||
};
|
||||
|
||||
} // namespace executors
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__EXECUTORS__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_
|
||||
@@ -15,13 +15,24 @@
|
||||
#ifndef RCLCPP__EXECUTORS__STATIC_SINGLE_THREADED_EXECUTOR_HPP_
|
||||
#define RCLCPP__EXECUTORS__STATIC_SINGLE_THREADED_EXECUTOR_HPP_
|
||||
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <cassert>
|
||||
#include <cstdlib>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
|
||||
#include "rmw/rmw.h"
|
||||
|
||||
#include "rclcpp/executor.hpp"
|
||||
#include "rclcpp/executors/executor_entities_collection.hpp"
|
||||
#include "rclcpp/executors/single_threaded_executor.hpp"
|
||||
#include "rclcpp/executors/static_executor_entities_collector.hpp"
|
||||
#include "rclcpp/experimental/executable_list.hpp"
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/memory_strategies.hpp"
|
||||
#include "rclcpp/node.hpp"
|
||||
#include "rclcpp/rate.hpp"
|
||||
#include "rclcpp/utilities.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
@@ -54,7 +65,7 @@ public:
|
||||
explicit StaticSingleThreadedExecutor(
|
||||
const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions());
|
||||
|
||||
/// Default destructor.
|
||||
/// Default destrcutor.
|
||||
RCLCPP_PUBLIC
|
||||
virtual ~StaticSingleThreadedExecutor();
|
||||
|
||||
@@ -105,20 +116,92 @@ public:
|
||||
void
|
||||
spin_all(std::chrono::nanoseconds max_duration) override;
|
||||
|
||||
/// Add a callback group to an executor.
|
||||
/**
|
||||
* \sa rclcpp::Executor::add_callback_group
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_callback_group(
|
||||
rclcpp::CallbackGroup::SharedPtr group_ptr,
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
|
||||
bool notify = true) override;
|
||||
|
||||
/// Remove callback group from the executor
|
||||
/**
|
||||
* \sa rclcpp::Executor::remove_callback_group
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
remove_callback_group(
|
||||
rclcpp::CallbackGroup::SharedPtr group_ptr,
|
||||
bool notify = true) override;
|
||||
|
||||
/// Add a node to the executor.
|
||||
/**
|
||||
* \sa rclcpp::Executor::add_node
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_node(
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
|
||||
bool notify = true) override;
|
||||
|
||||
/// Convenience function which takes Node and forwards NodeBaseInterface.
|
||||
/**
|
||||
* \sa rclcpp::StaticSingleThreadedExecutor::add_node
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true) override;
|
||||
|
||||
/// Remove a node from the executor.
|
||||
/**
|
||||
* \sa rclcpp::Executor::remove_node
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
remove_node(
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
|
||||
bool notify = true) override;
|
||||
|
||||
/// Convenience function which takes Node and forwards NodeBaseInterface.
|
||||
/**
|
||||
* \sa rclcpp::Executor::remove_node
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
remove_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true) override;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr>
|
||||
get_all_callback_groups() override;
|
||||
|
||||
/// Get callback groups that belong to executor.
|
||||
/**
|
||||
* \sa rclcpp::Executor::get_manually_added_callback_groups()
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr>
|
||||
get_manually_added_callback_groups() override;
|
||||
|
||||
/// Get callback groups that belong to executor.
|
||||
/**
|
||||
* \sa rclcpp::Executor::get_automatically_added_callback_groups_from_nodes()
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr>
|
||||
get_automatically_added_callback_groups_from_nodes() override;
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Executes ready executables from wait set.
|
||||
* @param collection entities to evaluate for ready executables.
|
||||
* @param wait_result result to check for ready executables.
|
||||
* @param spin_once if true executes only the first ready executable.
|
||||
* @return true if any executable was ready.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
execute_ready_executables(
|
||||
const rclcpp::executors::ExecutorEntitiesCollection & collection,
|
||||
rclcpp::WaitResult<rclcpp::WaitSet> & wait_result,
|
||||
bool spin_once);
|
||||
execute_ready_executables(bool spin_once = false);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
@@ -130,6 +213,8 @@ protected:
|
||||
|
||||
private:
|
||||
RCLCPP_DISABLE_COPY(StaticSingleThreadedExecutor)
|
||||
|
||||
StaticExecutorEntitiesCollector::SharedPtr entities_collector_;
|
||||
};
|
||||
|
||||
} // namespace executors
|
||||
|
||||
@@ -24,7 +24,6 @@
|
||||
#include "rclcpp/allocator/allocator_deleter.hpp"
|
||||
#include "rclcpp/experimental/buffers/buffer_implementation_base.hpp"
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "tracetools/tracetools.h"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
@@ -95,10 +94,6 @@ public:
|
||||
|
||||
buffer_ = std::move(buffer_impl);
|
||||
|
||||
TRACEPOINT(
|
||||
rclcpp_buffer_to_ipb,
|
||||
static_cast<const void *>(buffer_.get()),
|
||||
static_cast<const void *>(this));
|
||||
if (!allocator) {
|
||||
message_allocator_ = std::make_shared<MessageAlloc>();
|
||||
} else {
|
||||
|
||||
@@ -25,7 +25,6 @@
|
||||
#include "rclcpp/logging.hpp"
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
#include "tracetools/tracetools.h"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
@@ -52,7 +51,6 @@ public:
|
||||
if (capacity == 0) {
|
||||
throw std::invalid_argument("capacity must be a positive, non-zero value");
|
||||
}
|
||||
TRACEPOINT(rclcpp_construct_ring_buffer, static_cast<const void *>(this), capacity_);
|
||||
}
|
||||
|
||||
virtual ~RingBufferImplementation() {}
|
||||
@@ -69,12 +67,6 @@ public:
|
||||
|
||||
write_index_ = next_(write_index_);
|
||||
ring_buffer_[write_index_] = std::move(request);
|
||||
TRACEPOINT(
|
||||
rclcpp_ring_buffer_enqueue,
|
||||
static_cast<const void *>(this),
|
||||
write_index_,
|
||||
size_ + 1,
|
||||
is_full_());
|
||||
|
||||
if (is_full_()) {
|
||||
read_index_ = next_(read_index_);
|
||||
@@ -94,15 +86,11 @@ public:
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
|
||||
if (!has_data_()) {
|
||||
return BufferT();
|
||||
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Calling dequeue on empty intra-process buffer");
|
||||
throw std::runtime_error("Calling dequeue on empty intra-process buffer");
|
||||
}
|
||||
|
||||
auto request = std::move(ring_buffer_[read_index_]);
|
||||
TRACEPOINT(
|
||||
rclcpp_ring_buffer_dequeue,
|
||||
static_cast<const void *>(this),
|
||||
read_index_,
|
||||
size_ - 1);
|
||||
read_index_ = next_(read_index_);
|
||||
|
||||
size_--;
|
||||
@@ -148,10 +136,7 @@ public:
|
||||
return is_full_();
|
||||
}
|
||||
|
||||
void clear()
|
||||
{
|
||||
TRACEPOINT(rclcpp_ring_buffer_clear, static_cast<const void *>(this));
|
||||
}
|
||||
void clear() {}
|
||||
|
||||
private:
|
||||
/// Get the next index value for the ring buffer
|
||||
|
||||
@@ -1,286 +0,0 @@
|
||||
// Copyright 2023 iRobot Corporation.
|
||||
//
|
||||
// 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__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_HPP_
|
||||
#define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_HPP_
|
||||
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
#include "rclcpp/executor.hpp"
|
||||
#include "rclcpp/executors/executor_entities_collection.hpp"
|
||||
#include "rclcpp/executors/executor_entities_collector.hpp"
|
||||
#include "rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp"
|
||||
#include "rclcpp/experimental/executors/events_executor/events_queue.hpp"
|
||||
#include "rclcpp/experimental/executors/events_executor/simple_events_queue.hpp"
|
||||
#include "rclcpp/experimental/timers_manager.hpp"
|
||||
#include "rclcpp/node.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace experimental
|
||||
{
|
||||
namespace executors
|
||||
{
|
||||
|
||||
/// Events executor implementation
|
||||
/**
|
||||
* This executor uses an events queue and a timers manager to execute entities from its
|
||||
* associated nodes and callback groups.
|
||||
* ROS 2 entities allow to set callback functions that are invoked when the entity is triggered
|
||||
* or has work to do. The events-executor sets these callbacks such that they push an
|
||||
* event into its queue.
|
||||
*
|
||||
* This executor tries to reduce as much as possible the amount of maintenance operations.
|
||||
* This allows to use customized `EventsQueue` classes to achieve different goals such
|
||||
* as very low CPU usage, bounded memory requirement, determinism, etc.
|
||||
*
|
||||
* The executor uses a weak ownership model and it locks entities only while executing
|
||||
* their related events.
|
||||
*
|
||||
* To run this executor:
|
||||
* rclcpp::experimental::executors::EventsExecutor executor;
|
||||
* executor.add_node(node);
|
||||
* executor.spin();
|
||||
* executor.remove_node(node);
|
||||
*/
|
||||
class EventsExecutor : public rclcpp::Executor
|
||||
{
|
||||
friend class EventsExecutorEntitiesCollector;
|
||||
|
||||
public:
|
||||
RCLCPP_SMART_PTR_DEFINITIONS(EventsExecutor)
|
||||
|
||||
/// Default constructor. See the default constructor for Executor.
|
||||
/**
|
||||
* \param[in] events_queue The queue used to store events.
|
||||
* \param[in] execute_timers_separate_thread If true, timers are executed in a separate
|
||||
* thread. If false, timers are executed in the same thread as all other entities.
|
||||
* \param[in] options Options used to configure the executor.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
explicit EventsExecutor(
|
||||
rclcpp::experimental::executors::EventsQueue::UniquePtr events_queue = std::make_unique<
|
||||
rclcpp::experimental::executors::SimpleEventsQueue>(),
|
||||
bool execute_timers_separate_thread = false,
|
||||
const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions());
|
||||
|
||||
/// Default destructor.
|
||||
RCLCPP_PUBLIC
|
||||
virtual ~EventsExecutor();
|
||||
|
||||
/// Events executor implementation of spin.
|
||||
/**
|
||||
* This function will block until work comes in, execute it, and keep blocking.
|
||||
* It will only be interrupted by a CTRL-C (managed by the global signal handler).
|
||||
* \throws std::runtime_error when spin() called while already spinning
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
spin() override;
|
||||
|
||||
/// Events executor implementation of spin some
|
||||
/**
|
||||
* This non-blocking function will execute the timers and events
|
||||
* that were ready when this API was called, until timeout or no
|
||||
* more work available. New ready-timers/events arrived while
|
||||
* executing work, won't be taken into account here.
|
||||
*
|
||||
* Example:
|
||||
* while(condition) {
|
||||
* spin_some();
|
||||
* sleep(); // User should have some sync work or
|
||||
* // sleep to avoid a 100% CPU usage
|
||||
* }
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0)) override;
|
||||
|
||||
/// Events executor implementation of spin all
|
||||
/**
|
||||
* This non-blocking function will execute timers and events
|
||||
* until timeout or no more work available. If new ready-timers/events
|
||||
* arrive while executing work available, they will be executed
|
||||
* as long as the timeout hasn't expired.
|
||||
*
|
||||
* Example:
|
||||
* while(condition) {
|
||||
* spin_all();
|
||||
* sleep(); // User should have some sync work or
|
||||
* // sleep to avoid a 100% CPU usage
|
||||
* }
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
spin_all(std::chrono::nanoseconds max_duration) override;
|
||||
|
||||
/// Add a node to the executor.
|
||||
/**
|
||||
* \sa rclcpp::Executor::add_node
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_node(
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
|
||||
bool notify = true) override;
|
||||
|
||||
/// Convenience function which takes Node and forwards NodeBaseInterface.
|
||||
/**
|
||||
* \sa rclcpp::EventsExecutor::add_node
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true) override;
|
||||
|
||||
/// Remove a node from the executor.
|
||||
/**
|
||||
* \sa rclcpp::Executor::remove_node
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
remove_node(
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
|
||||
bool notify = true) override;
|
||||
|
||||
/// Convenience function which takes Node and forwards NodeBaseInterface.
|
||||
/**
|
||||
* \sa rclcpp::Executor::remove_node
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
remove_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true) override;
|
||||
|
||||
/// Add a callback group to an executor.
|
||||
/**
|
||||
* \sa rclcpp::Executor::add_callback_group
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_callback_group(
|
||||
rclcpp::CallbackGroup::SharedPtr group_ptr,
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
|
||||
bool notify = true) override;
|
||||
|
||||
/// Remove callback group from the executor
|
||||
/**
|
||||
* \sa rclcpp::Executor::remove_callback_group
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
remove_callback_group(
|
||||
rclcpp::CallbackGroup::SharedPtr group_ptr,
|
||||
bool notify = true) override;
|
||||
|
||||
/// Get callback groups that belong to executor.
|
||||
/**
|
||||
* \sa rclcpp::Executor::get_all_callback_groups()
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr>
|
||||
get_all_callback_groups() override;
|
||||
|
||||
/// Get callback groups that belong to executor.
|
||||
/**
|
||||
* \sa rclcpp::Executor::get_manually_added_callback_groups()
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr>
|
||||
get_manually_added_callback_groups() override;
|
||||
|
||||
/// Get callback groups that belong to executor.
|
||||
/**
|
||||
* \sa rclcpp::Executor::get_automatically_added_callback_groups_from_nodes()
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr>
|
||||
get_automatically_added_callback_groups_from_nodes() override;
|
||||
|
||||
protected:
|
||||
/// Internal implementation of spin_once
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
spin_once_impl(std::chrono::nanoseconds timeout) override;
|
||||
|
||||
/// Internal implementation of spin_some
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive);
|
||||
|
||||
private:
|
||||
RCLCPP_DISABLE_COPY(EventsExecutor)
|
||||
|
||||
/// Execute a provided executor event if its associated entities are available
|
||||
void
|
||||
execute_event(const ExecutorEvent & event);
|
||||
|
||||
/// Collect entities from callback groups and refresh the current collection with them
|
||||
void
|
||||
refresh_current_collection_from_callback_groups();
|
||||
|
||||
/// Refresh the current collection using the provided new_collection
|
||||
void
|
||||
refresh_current_collection(const rclcpp::executors::ExecutorEntitiesCollection & new_collection);
|
||||
|
||||
/// Create a listener callback function for the provided entity
|
||||
std::function<void(size_t)>
|
||||
create_entity_callback(void * entity_key, ExecutorEventType type);
|
||||
|
||||
/// Create a listener callback function for the provided waitable entity
|
||||
std::function<void(size_t, int)>
|
||||
create_waitable_callback(const rclcpp::Waitable * waitable_id);
|
||||
|
||||
/// Searches for the provided entity_id in the collection and returns the entity if valid
|
||||
template<typename CollectionType>
|
||||
typename CollectionType::EntitySharedPtr
|
||||
retrieve_entity(typename CollectionType::Key entity_id, CollectionType & collection)
|
||||
{
|
||||
// Check if the entity_id is in the collection
|
||||
auto it = collection.find(entity_id);
|
||||
if (it == collection.end()) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
// Check if the entity associated with the entity_id is valid
|
||||
// and remove it from the collection if it isn't
|
||||
auto entity = it->second.entity.lock();
|
||||
if (!entity) {
|
||||
collection.erase(it);
|
||||
}
|
||||
|
||||
// Return the retrieved entity (this can be a nullptr if the entity was not valid)
|
||||
return entity;
|
||||
}
|
||||
|
||||
/// Queue where entities can push events
|
||||
rclcpp::experimental::executors::EventsQueue::UniquePtr events_queue_;
|
||||
|
||||
std::shared_ptr<rclcpp::executors::ExecutorEntitiesCollector> entities_collector_;
|
||||
std::shared_ptr<rclcpp::executors::ExecutorEntitiesCollection> current_entities_collection_;
|
||||
std::shared_ptr<rclcpp::executors::ExecutorNotifyWaitable> notify_waitable_;
|
||||
|
||||
/// Flag used to reduce the number of unnecessary waitable events
|
||||
std::atomic<bool> notify_waitable_event_pushed_ {false};
|
||||
|
||||
/// Timers manager used to track and/or execute associated timers
|
||||
std::shared_ptr<rclcpp::experimental::TimersManager> timers_manager_;
|
||||
};
|
||||
|
||||
} // namespace executors
|
||||
} // namespace experimental
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_HPP_
|
||||
@@ -1,46 +0,0 @@
|
||||
// Copyright 2023 iRobot Corporation.
|
||||
//
|
||||
// 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__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_EVENT_TYPES_HPP_
|
||||
#define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_EVENT_TYPES_HPP_
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace experimental
|
||||
{
|
||||
namespace executors
|
||||
{
|
||||
|
||||
enum ExecutorEventType
|
||||
{
|
||||
CLIENT_EVENT,
|
||||
SUBSCRIPTION_EVENT,
|
||||
SERVICE_EVENT,
|
||||
TIMER_EVENT,
|
||||
WAITABLE_EVENT
|
||||
};
|
||||
|
||||
struct ExecutorEvent
|
||||
{
|
||||
const void * entity_key;
|
||||
int waitable_data;
|
||||
ExecutorEventType type;
|
||||
size_t num_events;
|
||||
};
|
||||
|
||||
} // namespace executors
|
||||
} // namespace experimental
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_EVENT_TYPES_HPP_
|
||||
@@ -1,100 +0,0 @@
|
||||
// Copyright 2023 iRobot Corporation.
|
||||
//
|
||||
// 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__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_QUEUE_HPP_
|
||||
#define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_QUEUE_HPP_
|
||||
|
||||
#include <queue>
|
||||
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
#include "rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace experimental
|
||||
{
|
||||
namespace executors
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief This abstract class can be used to implement different types of queues
|
||||
* where `ExecutorEvent` can be stored.
|
||||
* The derived classes should choose which underlying container to use and
|
||||
* the strategy for pushing and popping events.
|
||||
* For example a queue implementation may be bounded or unbounded and have
|
||||
* different pruning strategies.
|
||||
* Implementations may or may not check the validity of events and decide how to handle
|
||||
* the situation where an event is not valid anymore (e.g. a subscription history cache overruns)
|
||||
*/
|
||||
class EventsQueue
|
||||
{
|
||||
public:
|
||||
RCLCPP_SMART_PTR_ALIASES_ONLY(EventsQueue)
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
EventsQueue() = default;
|
||||
|
||||
/**
|
||||
* @brief Destruct the object.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
virtual ~EventsQueue() = default;
|
||||
|
||||
/**
|
||||
* @brief push event into the queue
|
||||
* @param event The event to push into the queue
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
virtual
|
||||
void
|
||||
enqueue(const rclcpp::experimental::executors::ExecutorEvent & event) = 0;
|
||||
|
||||
/**
|
||||
* @brief Extracts an event from the queue, eventually waiting until timeout
|
||||
* if none is available.
|
||||
* @return true if event has been found, false if timeout
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
virtual
|
||||
bool
|
||||
dequeue(
|
||||
rclcpp::experimental::executors::ExecutorEvent & event,
|
||||
std::chrono::nanoseconds timeout = std::chrono::nanoseconds::max()) = 0;
|
||||
|
||||
/**
|
||||
* @brief Test whether queue is empty
|
||||
* @return true if the queue's size is 0, false otherwise.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
virtual
|
||||
bool
|
||||
empty() const = 0;
|
||||
|
||||
/**
|
||||
* @brief Returns the number of elements in the queue.
|
||||
* @return the number of elements in the queue.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
virtual
|
||||
size_t
|
||||
size() const = 0;
|
||||
};
|
||||
|
||||
} // namespace executors
|
||||
} // namespace experimental
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_QUEUE_HPP_
|
||||
@@ -1,134 +0,0 @@
|
||||
// Copyright 2023 iRobot Corporation.
|
||||
//
|
||||
// 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__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__SIMPLE_EVENTS_QUEUE_HPP_
|
||||
#define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__SIMPLE_EVENTS_QUEUE_HPP_
|
||||
|
||||
#include <condition_variable>
|
||||
#include <mutex>
|
||||
#include <queue>
|
||||
#include <utility>
|
||||
|
||||
#include "rclcpp/experimental/executors/events_executor/events_queue.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace experimental
|
||||
{
|
||||
namespace executors
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief This class implements an EventsQueue as a simple wrapper around a std::queue.
|
||||
* It does not perform any checks about the size of queue, which can grow
|
||||
* unbounded without being pruned.
|
||||
* The simplicity of this implementation makes it suitable for optimizing CPU usage.
|
||||
*/
|
||||
class SimpleEventsQueue : public EventsQueue
|
||||
{
|
||||
public:
|
||||
RCLCPP_PUBLIC
|
||||
~SimpleEventsQueue() override = default;
|
||||
|
||||
/**
|
||||
* @brief enqueue event into the queue
|
||||
* Thread safe
|
||||
* @param event The event to enqueue into the queue
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
enqueue(const rclcpp::experimental::executors::ExecutorEvent & event) override
|
||||
{
|
||||
rclcpp::experimental::executors::ExecutorEvent single_event = event;
|
||||
single_event.num_events = 1;
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
for (size_t ev = 0; ev < event.num_events; ev++) {
|
||||
event_queue_.push(single_event);
|
||||
}
|
||||
}
|
||||
events_queue_cv_.notify_one();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief waits for an event until timeout, gets a single event
|
||||
* Thread safe
|
||||
* @return true if event, false if timeout
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
dequeue(
|
||||
rclcpp::experimental::executors::ExecutorEvent & event,
|
||||
std::chrono::nanoseconds timeout = std::chrono::nanoseconds::max()) override
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
|
||||
// Initialize to true because it's only needed if we have a valid timeout
|
||||
bool has_data = true;
|
||||
if (timeout != std::chrono::nanoseconds::max()) {
|
||||
has_data =
|
||||
events_queue_cv_.wait_for(lock, timeout, [this]() {return !event_queue_.empty();});
|
||||
} else {
|
||||
events_queue_cv_.wait(lock, [this]() {return !event_queue_.empty();});
|
||||
}
|
||||
|
||||
if (has_data) {
|
||||
event = event_queue_.front();
|
||||
event_queue_.pop();
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Test whether queue is empty
|
||||
* Thread safe
|
||||
* @return true if the queue's size is 0, false otherwise.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
empty() const override
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
return event_queue_.empty();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Returns the number of elements in the queue.
|
||||
* Thread safe
|
||||
* @return the number of elements in the queue.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
size_t
|
||||
size() const override
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
return event_queue_.size();
|
||||
}
|
||||
|
||||
private:
|
||||
// The underlying queue implementation
|
||||
std::queue<rclcpp::experimental::executors::ExecutorEvent> event_queue_;
|
||||
// Mutex to protect read/write access to the queue
|
||||
mutable std::mutex mutex_;
|
||||
// Variable used to notify when an event is added to the queue
|
||||
std::condition_variable events_queue_cv_;
|
||||
};
|
||||
|
||||
} // namespace executors
|
||||
} // namespace experimental
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__SIMPLE_EVENTS_QUEUE_HPP_
|
||||
@@ -454,8 +454,6 @@ private:
|
||||
if (std::next(it) == subscription_ids.end()) {
|
||||
// If this is the last subscription, give up ownership
|
||||
subscription->provide_intra_process_data(std::move(message));
|
||||
// Last message delivered, break from for loop
|
||||
break;
|
||||
} else {
|
||||
// Copy the message since we have additional subscriptions to serve
|
||||
Deleter deleter = message.get_deleter();
|
||||
@@ -495,8 +493,6 @@ private:
|
||||
if (std::next(it) == subscription_ids.end()) {
|
||||
// If this is the last subscription, give up ownership
|
||||
ros_message_subscription->provide_intra_process_message(std::move(message));
|
||||
// Last message delivered, break from for loop
|
||||
break;
|
||||
} else {
|
||||
// Copy the message since we have additional subscriptions to serve
|
||||
Deleter deleter = message.get_deleter();
|
||||
|
||||
@@ -109,22 +109,9 @@ public:
|
||||
|
||||
if (any_callback_.use_take_shared_method()) {
|
||||
shared_msg = this->buffer_->consume_shared();
|
||||
if (!shared_msg) {
|
||||
return nullptr;
|
||||
}
|
||||
} else {
|
||||
unique_msg = this->buffer_->consume_unique();
|
||||
if (!unique_msg) {
|
||||
return nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
if (this->buffer_->has_data()) {
|
||||
// If there is data still to be processed, indicate to the
|
||||
// executor or waitset by triggering the guard condition.
|
||||
this->trigger_guard_condition();
|
||||
}
|
||||
|
||||
return std::static_pointer_cast<void>(
|
||||
std::make_shared<std::pair<ConstMessageSharedPtr, MessageUniquePtr>>(
|
||||
std::pair<ConstMessageSharedPtr, MessageUniquePtr>(
|
||||
@@ -151,7 +138,7 @@ protected:
|
||||
execute_impl(std::shared_ptr<void> & data)
|
||||
{
|
||||
if (!data) {
|
||||
return;
|
||||
throw std::runtime_error("'data' is empty");
|
||||
}
|
||||
|
||||
rmw_message_info_t msg_info;
|
||||
|
||||
@@ -31,8 +31,6 @@
|
||||
#include "rclcpp/qos.hpp"
|
||||
#include "rclcpp/type_support_decl.hpp"
|
||||
|
||||
#include "tracetools/tracetools.h"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace experimental
|
||||
@@ -93,10 +91,6 @@ public:
|
||||
buffer_type,
|
||||
qos_profile,
|
||||
std::make_shared<Alloc>(subscribed_type_allocator_));
|
||||
TRACEPOINT(
|
||||
rclcpp_ipb_to_subscription,
|
||||
static_cast<const void *>(buffer_.get()),
|
||||
static_cast<const void *>(this));
|
||||
}
|
||||
|
||||
bool
|
||||
|
||||
@@ -1,553 +0,0 @@
|
||||
// Copyright 2023 iRobot Corporation.
|
||||
//
|
||||
// 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__EXPERIMENTAL__TIMERS_MANAGER_HPP_
|
||||
#define RCLCPP__EXPERIMENTAL__TIMERS_MANAGER_HPP_
|
||||
|
||||
#include <algorithm>
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <condition_variable>
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <thread>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "rclcpp/context.hpp"
|
||||
#include "rclcpp/timer.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace experimental
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief This class provides a way for storing and executing timer objects.
|
||||
* It provides APIs to suit the needs of different applications and execution models.
|
||||
* All public APIs provided by this class are thread-safe.
|
||||
*
|
||||
* Timers management
|
||||
* This class provides APIs to add/remove timers to/from an internal storage.
|
||||
* It keeps a list of weak pointers from added timers, and locks them only when
|
||||
* they need to be executed or modified.
|
||||
* Timers are kept ordered in a binary-heap priority queue.
|
||||
* Calls to add/remove APIs will temporarily block the execution of the timers and
|
||||
* will require to reorder the internal priority queue.
|
||||
* Because of this, they have a not-negligible impact on the performance.
|
||||
*
|
||||
* Timers execution
|
||||
* The most efficient use of this class consists in letting a TimersManager object
|
||||
* to spawn a thread where timers are monitored and optionally executed.
|
||||
* This can be controlled via the `start` and `stop` methods.
|
||||
* Ready timers can either be executed or an on_ready_callback can be used to notify
|
||||
* other entities that they are ready and need to be executed.
|
||||
* Other APIs allow to directly execute a given timer.
|
||||
*
|
||||
* This class assumes that the `execute_callback()` API of the stored timers is never
|
||||
* called by other entities, but it can only be called from here.
|
||||
* If this assumption is not respected, the heap property may be invalidated,
|
||||
* so timers may be executed out of order, without this object noticing it.
|
||||
*
|
||||
*/
|
||||
class TimersManager
|
||||
{
|
||||
public:
|
||||
RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(TimersManager)
|
||||
|
||||
/**
|
||||
* @brief Construct a new TimersManager object
|
||||
*
|
||||
* @param context custom context to be used.
|
||||
* Shared ownership of the context is held until destruction.
|
||||
* @param on_ready_callback The timers on ready callback. The presence of this function
|
||||
* indicates what to do when the TimersManager is running and a timer becomes ready.
|
||||
* The TimersManager is considered "running" when the `start` method has been called.
|
||||
* If it's callable, it will be invoked instead of the timer callback.
|
||||
* If it's not callable, then the TimersManager will
|
||||
* directly execute timers when they are ready.
|
||||
* All the methods that execute a given timer (e.g. `execute_head_timer`
|
||||
* or `execute_ready_timer`) without the TimersManager being `running`, i.e.
|
||||
* without actually explicitly waiting for the timer to become ready, will ignore this
|
||||
* callback.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
TimersManager(
|
||||
std::shared_ptr<rclcpp::Context> context,
|
||||
std::function<void(const rclcpp::TimerBase *)> on_ready_callback = nullptr);
|
||||
|
||||
/**
|
||||
* @brief Destruct the TimersManager object making sure to stop thread and release memory.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
~TimersManager();
|
||||
|
||||
/**
|
||||
* @brief Adds a new timer to the storage, maintaining weak ownership of it.
|
||||
* Function is thread safe and it can be called regardless of the state of the timers thread.
|
||||
*
|
||||
* @param timer the timer to add.
|
||||
* @throws std::invalid_argument if timer is a nullptr.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void add_timer(rclcpp::TimerBase::SharedPtr timer);
|
||||
|
||||
/**
|
||||
* @brief Remove a single timer from the object storage.
|
||||
* Will do nothing if the timer was not being stored here.
|
||||
* Function is thread safe and it can be called regardless of the state of the timers thread.
|
||||
*
|
||||
* @param timer the timer to remove.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void remove_timer(rclcpp::TimerBase::SharedPtr timer);
|
||||
|
||||
/**
|
||||
* @brief Remove all the timers stored in the object.
|
||||
* Function is thread safe and it can be called regardless of the state of the timers thread.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void clear();
|
||||
|
||||
/**
|
||||
* @brief Starts a thread that takes care of executing the timers stored in this object.
|
||||
* Function will throw an error if the timers thread was already running.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void start();
|
||||
|
||||
/**
|
||||
* @brief Stops the timers thread.
|
||||
* Will do nothing if the timer thread was not running.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void stop();
|
||||
|
||||
/**
|
||||
* @brief Get the number of timers that are currently ready.
|
||||
* This function is thread safe.
|
||||
*
|
||||
* @return size_t number of ready timers.
|
||||
* @throws std::runtime_error if the timers thread was already running.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
size_t get_number_ready_timers();
|
||||
|
||||
/**
|
||||
* @brief Executes head timer if ready.
|
||||
* This function is thread safe.
|
||||
* This function will try to execute the timer callback regardless of whether
|
||||
* the TimersManager on_ready_callback was passed during construction.
|
||||
*
|
||||
* @return true if head timer was ready.
|
||||
* @throws std::runtime_error if the timers thread was already running.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
bool execute_head_timer();
|
||||
|
||||
/**
|
||||
* @brief Executes timer identified by its ID.
|
||||
* This function is thread safe.
|
||||
* This function will try to execute the timer callback regardless of whether
|
||||
* the TimersManager on_ready_callback was passed during construction.
|
||||
*
|
||||
* @param timer_id the timer ID of the timer to execute
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void execute_ready_timer(const rclcpp::TimerBase * timer_id);
|
||||
|
||||
/**
|
||||
* @brief Get the amount of time before the next timer triggers.
|
||||
* This function is thread safe.
|
||||
*
|
||||
* @return std::chrono::nanoseconds to wait,
|
||||
* the returned value could be negative if the timer is already expired
|
||||
* or std::chrono::nanoseconds::max() if there are no timers stored in the object.
|
||||
* @throws std::runtime_error if the timers thread was already running.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
std::chrono::nanoseconds get_head_timeout();
|
||||
|
||||
private:
|
||||
RCLCPP_DISABLE_COPY(TimersManager)
|
||||
|
||||
using TimerPtr = rclcpp::TimerBase::SharedPtr;
|
||||
using WeakTimerPtr = rclcpp::TimerBase::WeakPtr;
|
||||
|
||||
// Forward declaration
|
||||
class TimersHeap;
|
||||
|
||||
/**
|
||||
* @brief This class allows to store weak pointers to timers in a heap-like data structure.
|
||||
* The root of the heap is the timer that triggers first.
|
||||
* Since this class uses weak ownership, it is not guaranteed that it represents a valid heap
|
||||
* at any point in time as timers could go out of scope, thus invalidating it.
|
||||
* The "validate_and_lock" API allows to restore the heap property and also returns a locked version
|
||||
* of the timers heap.
|
||||
* This class is not thread safe and requires external mutexes to protect its usage.
|
||||
*/
|
||||
class WeakTimersHeap
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Add a new timer to the heap. After the addition, the heap property is enforced.
|
||||
*
|
||||
* @param timer new timer to add.
|
||||
* @return true if timer has been added, false if it was already there.
|
||||
*/
|
||||
bool add_timer(TimerPtr timer)
|
||||
{
|
||||
TimersHeap locked_heap = this->validate_and_lock();
|
||||
bool added = locked_heap.add_timer(std::move(timer));
|
||||
|
||||
if (added) {
|
||||
// Re-create the weak heap with the new timer added
|
||||
this->store(locked_heap);
|
||||
}
|
||||
|
||||
return added;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Remove a timer from the heap. After the removal, the heap property is enforced.
|
||||
*
|
||||
* @param timer timer to remove.
|
||||
* @return true if timer has been removed, false if it was not there.
|
||||
*/
|
||||
bool remove_timer(TimerPtr timer)
|
||||
{
|
||||
TimersHeap locked_heap = this->validate_and_lock();
|
||||
bool removed = locked_heap.remove_timer(std::move(timer));
|
||||
|
||||
if (removed) {
|
||||
// Re-create the weak heap with the timer removed
|
||||
this->store(locked_heap);
|
||||
}
|
||||
|
||||
return removed;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Retrieve the timer identified by the key
|
||||
* @param timer_id The ID of the timer to retrieve.
|
||||
* @return TimerPtr if there's a timer associated with the ID, nullptr otherwise
|
||||
*/
|
||||
TimerPtr get_timer(const rclcpp::TimerBase * timer_id)
|
||||
{
|
||||
for (auto & weak_timer : weak_heap_) {
|
||||
auto timer = weak_timer.lock();
|
||||
if (timer.get() == timer_id) {
|
||||
return timer;
|
||||
}
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Returns a const reference to the front element.
|
||||
*/
|
||||
const WeakTimerPtr & front() const
|
||||
{
|
||||
return weak_heap_.front();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Returns whether the heap is empty or not.
|
||||
*/
|
||||
bool empty() const
|
||||
{
|
||||
return weak_heap_.empty();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief This function restores the current object as a valid heap
|
||||
* and it returns a locked version of it.
|
||||
* Timers that went out of scope are removed from the container.
|
||||
* It is the only public API to access and manipulate the stored timers.
|
||||
*
|
||||
* @return TimersHeap owned timers corresponding to the current object
|
||||
*/
|
||||
TimersHeap validate_and_lock()
|
||||
{
|
||||
TimersHeap locked_heap;
|
||||
bool any_timer_destroyed = false;
|
||||
|
||||
for (auto weak_timer : weak_heap_) {
|
||||
auto timer = weak_timer.lock();
|
||||
if (timer) {
|
||||
// This timer is valid, so add it to the locked heap
|
||||
// Note: we access friend private `owned_heap_` member field.
|
||||
locked_heap.owned_heap_.push_back(std::move(timer));
|
||||
} else {
|
||||
// This timer went out of scope, so we don't add it to locked heap
|
||||
// and we mark the corresponding flag.
|
||||
// It's not needed to erase it from weak heap, as we are going to re-heapify.
|
||||
// Note: we can't exit from the loop here, as we need to find all valid timers.
|
||||
any_timer_destroyed = true;
|
||||
}
|
||||
}
|
||||
|
||||
// If a timer has gone out of scope, then the remaining elements do not represent
|
||||
// a valid heap anymore. We need to re-heapify the timers heap.
|
||||
if (any_timer_destroyed) {
|
||||
locked_heap.heapify();
|
||||
// Re-create the weak heap now that elements have been heapified again
|
||||
this->store(locked_heap);
|
||||
}
|
||||
|
||||
return locked_heap;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief This function allows to recreate the heap of weak pointers
|
||||
* from an heap of owned pointers.
|
||||
* It is required to be called after a locked TimersHeap generated from this object
|
||||
* has been modified in any way (e.g. timers triggered, added, removed).
|
||||
*
|
||||
* @param heap timers heap to store as weak pointers
|
||||
*/
|
||||
void store(const TimersHeap & heap)
|
||||
{
|
||||
weak_heap_.clear();
|
||||
// Note: we access friend private `owned_heap_` member field.
|
||||
for (auto t : heap.owned_heap_) {
|
||||
weak_heap_.push_back(t);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Remove all timers from the heap.
|
||||
*/
|
||||
void clear()
|
||||
{
|
||||
weak_heap_.clear();
|
||||
}
|
||||
|
||||
private:
|
||||
std::vector<WeakTimerPtr> weak_heap_;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief This class is the equivalent of WeakTimersHeap but with ownership of the timers.
|
||||
* It can be generated by locking the weak version.
|
||||
* It provides operations to manipulate the heap.
|
||||
* This class is not thread safe and requires external mutexes to protect its usage.
|
||||
*/
|
||||
class TimersHeap
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Try to add a new timer to the heap.
|
||||
* After the addition, the heap property is preserved.
|
||||
* @param timer new timer to add.
|
||||
* @return true if timer has been added, false if it was already there.
|
||||
*/
|
||||
bool add_timer(TimerPtr timer)
|
||||
{
|
||||
// Nothing to do if the timer is already stored here
|
||||
auto it = std::find(owned_heap_.begin(), owned_heap_.end(), timer);
|
||||
if (it != owned_heap_.end()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
owned_heap_.push_back(std::move(timer));
|
||||
std::push_heap(owned_heap_.begin(), owned_heap_.end(), timer_greater);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Try to remove a timer from the heap.
|
||||
* After the removal, the heap property is preserved.
|
||||
* @param timer timer to remove.
|
||||
* @return true if timer has been removed, false if it was not there.
|
||||
*/
|
||||
bool remove_timer(TimerPtr timer)
|
||||
{
|
||||
// Nothing to do if the timer is not stored here
|
||||
auto it = std::find(owned_heap_.begin(), owned_heap_.end(), timer);
|
||||
if (it == owned_heap_.end()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
owned_heap_.erase(it);
|
||||
this->heapify();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Returns a reference to the front element.
|
||||
* @return reference to front element.
|
||||
*/
|
||||
TimerPtr & front()
|
||||
{
|
||||
return owned_heap_.front();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Returns a const reference to the front element.
|
||||
* @return const reference to front element.
|
||||
*/
|
||||
const TimerPtr & front() const
|
||||
{
|
||||
return owned_heap_.front();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Returns whether the heap is empty or not.
|
||||
* @return true if the heap is empty.
|
||||
*/
|
||||
bool empty() const
|
||||
{
|
||||
return owned_heap_.empty();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Returns the size of the heap.
|
||||
* @return the number of valid timers in the heap.
|
||||
*/
|
||||
size_t size() const
|
||||
{
|
||||
return owned_heap_.size();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get the number of timers that are currently ready.
|
||||
* @return size_t number of ready timers.
|
||||
*/
|
||||
size_t get_number_ready_timers() const
|
||||
{
|
||||
size_t ready_timers = 0;
|
||||
|
||||
for (TimerPtr t : owned_heap_) {
|
||||
if (t->is_ready()) {
|
||||
ready_timers++;
|
||||
}
|
||||
}
|
||||
|
||||
return ready_timers;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Restore a valid heap after the root value has been replaced (e.g. timer triggered).
|
||||
*/
|
||||
void heapify_root()
|
||||
{
|
||||
// The following code is a more efficient version than doing
|
||||
// pop_heap, pop_back, push_back, push_heap
|
||||
// as it removes the need for the last push_heap
|
||||
|
||||
// Push the modified element (i.e. the current root) at the bottom of the heap
|
||||
owned_heap_.push_back(owned_heap_[0]);
|
||||
// Exchange first and last-1 elements and reheapify
|
||||
std::pop_heap(owned_heap_.begin(), owned_heap_.end(), timer_greater);
|
||||
// Remove last element
|
||||
owned_heap_.pop_back();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Completely restores the structure to a valid heap
|
||||
*/
|
||||
void heapify()
|
||||
{
|
||||
std::make_heap(owned_heap_.begin(), owned_heap_.end(), timer_greater);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Helper function to clear the "on_reset_callback" on all associated timers.
|
||||
*/
|
||||
void clear_timers_on_reset_callbacks()
|
||||
{
|
||||
for (TimerPtr & t : owned_heap_) {
|
||||
t->clear_on_reset_callback();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Friend declaration to allow the `validate_and_lock()` function to access the
|
||||
* underlying heap container
|
||||
*/
|
||||
friend TimersHeap WeakTimersHeap::validate_and_lock();
|
||||
|
||||
/**
|
||||
* @brief Friend declaration to allow the `store()` function to access the
|
||||
* underlying heap container
|
||||
*/
|
||||
friend void WeakTimersHeap::store(const TimersHeap & heap);
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Comparison function between timers.
|
||||
* @return true if `a` triggers after `b`.
|
||||
*/
|
||||
static bool timer_greater(TimerPtr a, TimerPtr b)
|
||||
{
|
||||
// TODO(alsora): this can cause an error if timers are using different clocks
|
||||
return a->time_until_trigger() > b->time_until_trigger();
|
||||
}
|
||||
|
||||
std::vector<TimerPtr> owned_heap_;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Implements a loop that keeps executing ready timers.
|
||||
* This function is executed in the timers thread.
|
||||
*/
|
||||
void run_timers();
|
||||
|
||||
/**
|
||||
* @brief Get the amount of time before the next timer triggers.
|
||||
* This function is not thread safe, acquire a mutex before calling it.
|
||||
*
|
||||
* @return std::chrono::nanoseconds to wait,
|
||||
* the returned value could be negative if the timer is already expired
|
||||
* or std::chrono::nanoseconds::max() if the heap is empty.
|
||||
* This function is not thread safe, acquire the timers_mutex_ before calling it.
|
||||
*/
|
||||
std::chrono::nanoseconds get_head_timeout_unsafe();
|
||||
|
||||
/**
|
||||
* @brief Executes all the timers currently ready when the function is invoked
|
||||
* while keeping the heap correctly sorted.
|
||||
* This function is not thread safe, acquire the timers_mutex_ before calling it.
|
||||
*/
|
||||
void execute_ready_timers_unsafe();
|
||||
|
||||
// Callback to be called when timer is ready
|
||||
std::function<void(const rclcpp::TimerBase *)> on_ready_callback_ = nullptr;
|
||||
|
||||
// Thread used to run the timers execution task
|
||||
std::thread timers_thread_;
|
||||
// Protects access to timers
|
||||
std::mutex timers_mutex_;
|
||||
// Protects access to stop()
|
||||
std::mutex stop_mutex_;
|
||||
// Notifies the timers thread whenever timers are added/removed
|
||||
std::condition_variable timers_cv_;
|
||||
// Flag used as predicate by timers_cv_ that denotes one or more timers being added/removed
|
||||
bool timers_updated_ {false};
|
||||
// Indicates whether the timers thread is currently running or not
|
||||
std::atomic<bool> running_ {false};
|
||||
// Parent context used to understand if ROS is still active
|
||||
std::shared_ptr<rclcpp::Context> context_;
|
||||
// Timers heap storage with weak ownership
|
||||
WeakTimersHeap weak_timers_heap_;
|
||||
};
|
||||
|
||||
} // namespace experimental
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__EXPERIMENTAL__TIMERS_MANAGER_HPP_
|
||||
@@ -78,12 +78,38 @@ public:
|
||||
node_base,
|
||||
topic_name,
|
||||
*rclcpp::get_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib),
|
||||
options.template to_rcl_publisher_options<rclcpp::SerializedMessage>(qos),
|
||||
// NOTE(methylDragon): Passing these args separately is necessary for event binding
|
||||
options.event_callbacks,
|
||||
options.use_default_callbacks),
|
||||
options.template to_rcl_publisher_options<rclcpp::SerializedMessage>(qos)),
|
||||
ts_lib_(ts_lib)
|
||||
{}
|
||||
{
|
||||
// This is unfortunately duplicated with the code in publisher.hpp.
|
||||
// TODO(nnmm): Deduplicate by moving this into PublisherBase.
|
||||
if (options.event_callbacks.deadline_callback) {
|
||||
this->add_event_handler(
|
||||
options.event_callbacks.deadline_callback,
|
||||
RCL_PUBLISHER_OFFERED_DEADLINE_MISSED);
|
||||
}
|
||||
if (options.event_callbacks.liveliness_callback) {
|
||||
this->add_event_handler(
|
||||
options.event_callbacks.liveliness_callback,
|
||||
RCL_PUBLISHER_LIVELINESS_LOST);
|
||||
}
|
||||
if (options.event_callbacks.incompatible_qos_callback) {
|
||||
this->add_event_handler(
|
||||
options.event_callbacks.incompatible_qos_callback,
|
||||
RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
|
||||
} else if (options.use_default_callbacks) {
|
||||
// Register default callback when not specified
|
||||
try {
|
||||
this->add_event_handler(
|
||||
[this](QOSOfferedIncompatibleQoSInfo & info) {
|
||||
this->default_incompatible_qos_callback(info);
|
||||
},
|
||||
RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
|
||||
} catch (UnsupportedEventTypeException & /*exc*/) {
|
||||
// pass
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual ~GenericPublisher() = default;
|
||||
|
||||
@@ -81,13 +81,45 @@ public:
|
||||
node_base,
|
||||
*rclcpp::get_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib),
|
||||
topic_name,
|
||||
options.to_rcl_subscription_options(qos),
|
||||
options.event_callbacks,
|
||||
options.use_default_callbacks,
|
||||
DeliveredMessageKind::SERIALIZED_MESSAGE),
|
||||
options.template to_rcl_subscription_options<rclcpp::SerializedMessage>(qos),
|
||||
true),
|
||||
callback_(callback),
|
||||
ts_lib_(ts_lib)
|
||||
{}
|
||||
{
|
||||
// This is unfortunately duplicated with the code in subscription.hpp.
|
||||
// TODO(nnmm): Deduplicate by moving this into SubscriptionBase.
|
||||
if (options.event_callbacks.deadline_callback) {
|
||||
this->add_event_handler(
|
||||
options.event_callbacks.deadline_callback,
|
||||
RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED);
|
||||
}
|
||||
if (options.event_callbacks.liveliness_callback) {
|
||||
this->add_event_handler(
|
||||
options.event_callbacks.liveliness_callback,
|
||||
RCL_SUBSCRIPTION_LIVELINESS_CHANGED);
|
||||
}
|
||||
if (options.event_callbacks.incompatible_qos_callback) {
|
||||
this->add_event_handler(
|
||||
options.event_callbacks.incompatible_qos_callback,
|
||||
RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS);
|
||||
} else if (options.use_default_callbacks) {
|
||||
// Register default callback when not specified
|
||||
try {
|
||||
this->add_event_handler(
|
||||
[this](QOSRequestedIncompatibleQoSInfo & info) {
|
||||
this->default_incompatible_qos_callback(info);
|
||||
},
|
||||
RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS);
|
||||
} catch (UnsupportedEventTypeException & /*exc*/) {
|
||||
// pass
|
||||
}
|
||||
}
|
||||
if (options.event_callbacks.message_lost_callback) {
|
||||
this->add_event_handler(
|
||||
options.event_callbacks.message_lost_callback,
|
||||
RCL_SUBSCRIPTION_MESSAGE_LOST);
|
||||
}
|
||||
}
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual ~GenericSubscription() = default;
|
||||
@@ -123,31 +155,6 @@ public:
|
||||
RCLCPP_PUBLIC
|
||||
void return_serialized_message(std::shared_ptr<rclcpp::SerializedMessage> & message) override;
|
||||
|
||||
|
||||
// DYNAMIC TYPE ==================================================================================
|
||||
RCLCPP_PUBLIC
|
||||
rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr get_shared_dynamic_message_type()
|
||||
override;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr get_shared_dynamic_message() override;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr
|
||||
get_shared_dynamic_serialization_support() override;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr create_dynamic_message() override;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void return_dynamic_message(
|
||||
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message) override;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void handle_dynamic_message(
|
||||
const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message,
|
||||
const rclcpp::MessageInfo & message_info) override;
|
||||
|
||||
private:
|
||||
RCLCPP_DISABLE_COPY(GenericSubscription)
|
||||
|
||||
|
||||
@@ -17,7 +17,6 @@
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
@@ -123,7 +122,6 @@ private:
|
||||
: name_(new std::string(name)) {}
|
||||
|
||||
std::shared_ptr<const std::string> name_;
|
||||
std::shared_ptr<std::pair<std::string, std::string>> logger_sublogger_pairname_ = nullptr;
|
||||
|
||||
public:
|
||||
RCLCPP_PUBLIC
|
||||
@@ -159,7 +157,13 @@ public:
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
Logger
|
||||
get_child(const std::string & suffix);
|
||||
get_child(const std::string & suffix)
|
||||
{
|
||||
if (!name_) {
|
||||
return Logger();
|
||||
}
|
||||
return Logger(*name_ + "." + suffix);
|
||||
}
|
||||
|
||||
/// Set level for current logger.
|
||||
/**
|
||||
@@ -170,24 +174,6 @@ public:
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
set_level(Level level);
|
||||
|
||||
/// Get effective level for current logger.
|
||||
/**
|
||||
* The effective level is determined as the severity level of
|
||||
* the logger if it is set, otherwise it is the first specified severity
|
||||
* level of the logger's ancestors, starting with its closest ancestor.
|
||||
* The ancestor hierarchy is signified by logger names being separated by dots:
|
||||
* a logger named `x` is an ancestor of `x.y`, and both `x` and `x.y` are
|
||||
* ancestors of `x.y.z`, etc.
|
||||
* If the level has not been set for the logger nor any of its
|
||||
* ancestors, the default level is used.
|
||||
*
|
||||
* \throws rclcpp::exceptions::RCLError if any error happens.
|
||||
* \return Level for the current logger.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
Level
|
||||
get_effective_level() const;
|
||||
};
|
||||
|
||||
} // namespace rclcpp
|
||||
|
||||
@@ -777,7 +777,7 @@ public:
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
get_parameter(const std::string & name, rclcpp::Parameter & parameter) const;
|
||||
get_parameter(const std::string & name, rclcpp::Parameter & parameter) const noexcept;
|
||||
|
||||
/// Get the value of a parameter by the given name, and return true if it was set.
|
||||
/**
|
||||
|
||||
@@ -1,208 +0,0 @@
|
||||
// Copyright 2022 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__DETAIL__NODE_INTERFACES_HELPERS_HPP_
|
||||
#define RCLCPP__NODE_INTERFACES__DETAIL__NODE_INTERFACES_HELPERS_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <tuple>
|
||||
#include <type_traits>
|
||||
#include <utility>
|
||||
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace node_interfaces
|
||||
{
|
||||
namespace detail
|
||||
{
|
||||
|
||||
// Support and Helper template classes for the NodeInterfaces class.
|
||||
|
||||
template<typename NodeT, typename ... Ts>
|
||||
std::tuple<std::shared_ptr<Ts>...>
|
||||
init_tuple(NodeT & n);
|
||||
|
||||
/// Stores the interfaces in a tuple, provides constructors, and getters.
|
||||
template<typename ... InterfaceTs>
|
||||
struct NodeInterfacesStorage
|
||||
{
|
||||
template<typename NodeT>
|
||||
NodeInterfacesStorage(NodeT & node) // NOLINT(runtime/explicit)
|
||||
: interfaces_(init_tuple<decltype(node), InterfaceTs ...>(node))
|
||||
{}
|
||||
|
||||
NodeInterfacesStorage()
|
||||
: interfaces_()
|
||||
{}
|
||||
|
||||
explicit NodeInterfacesStorage(std::shared_ptr<InterfaceTs>... args)
|
||||
: interfaces_(args ...)
|
||||
{}
|
||||
|
||||
/// Individual Node Interface non-const getter.
|
||||
template<typename NodeInterfaceT>
|
||||
std::shared_ptr<NodeInterfaceT>
|
||||
get()
|
||||
{
|
||||
static_assert(
|
||||
(std::is_same_v<NodeInterfaceT, InterfaceTs>|| ...),
|
||||
"NodeInterfaces class does not contain given NodeInterfaceT");
|
||||
return std::get<std::shared_ptr<NodeInterfaceT>>(interfaces_);
|
||||
}
|
||||
|
||||
/// Individual Node Interface const getter.
|
||||
template<typename NodeInterfaceT>
|
||||
std::shared_ptr<const NodeInterfaceT>
|
||||
get() const
|
||||
{
|
||||
static_assert(
|
||||
(std::is_same_v<NodeInterfaceT, InterfaceTs>|| ...),
|
||||
"NodeInterfaces class does not contain given NodeInterfaceT");
|
||||
return std::get<std::shared_ptr<NodeInterfaceT>>(interfaces_);
|
||||
}
|
||||
|
||||
protected:
|
||||
std::tuple<std::shared_ptr<InterfaceTs>...> interfaces_;
|
||||
};
|
||||
|
||||
/// Prototype of NodeInterfacesSupports.
|
||||
/**
|
||||
* Should read NodeInterfacesSupports<..., T, ...> as "NodeInterfaces supports T", and
|
||||
* if NodeInterfacesSupport is specialized for T, the is_supported should be
|
||||
* set to std::true_type, but by default it is std::false_type, which will
|
||||
* lead to a compiler error when trying to use T with NodeInterfaces.
|
||||
*/
|
||||
template<typename StorageClassT, typename ... Ts>
|
||||
struct NodeInterfacesSupports;
|
||||
|
||||
/// Prototype of NodeInterfacesSupportCheck template meta-function.
|
||||
/**
|
||||
* This meta-function checks that all the types given are supported,
|
||||
* throwing a more human-readable error if an unsupported type is used.
|
||||
*/
|
||||
template<typename StorageClassT, typename ... InterfaceTs>
|
||||
struct NodeInterfacesSupportCheck;
|
||||
|
||||
/// Iterating specialization that ensures classes are supported and inherited.
|
||||
template<typename StorageClassT, typename NextInterfaceT, typename ... RemainingInterfaceTs>
|
||||
struct NodeInterfacesSupportCheck<StorageClassT, NextInterfaceT, RemainingInterfaceTs ...>
|
||||
: public NodeInterfacesSupportCheck<StorageClassT, RemainingInterfaceTs ...>
|
||||
{
|
||||
static_assert(
|
||||
NodeInterfacesSupports<StorageClassT, NextInterfaceT>::is_supported::value,
|
||||
"given NodeInterfaceT is not supported by rclcpp::node_interfaces::NodeInterfaces");
|
||||
};
|
||||
|
||||
/// Terminating case when there are no more "RemainingInterfaceTs".
|
||||
template<typename StorageClassT>
|
||||
struct NodeInterfacesSupportCheck<StorageClassT>
|
||||
{};
|
||||
|
||||
/// Default specialization, needs to be specialized for each supported interface.
|
||||
template<typename StorageClassT, typename ... RemainingInterfaceTs>
|
||||
struct NodeInterfacesSupports
|
||||
{
|
||||
// Specializations need to set this to std::true_type in addition to other interfaces.
|
||||
using is_supported = std::false_type;
|
||||
};
|
||||
|
||||
/// Terminating specialization of NodeInterfacesSupports.
|
||||
template<typename StorageClassT>
|
||||
struct NodeInterfacesSupports<StorageClassT>
|
||||
: public StorageClassT
|
||||
{
|
||||
/// Perfect forwarding constructor to get arguments down to StorageClassT.
|
||||
template<typename ... ArgsT>
|
||||
explicit NodeInterfacesSupports(ArgsT && ... args)
|
||||
: StorageClassT(std::forward<ArgsT>(args) ...)
|
||||
{}
|
||||
};
|
||||
|
||||
// Helper functions to initialize the tuple in NodeInterfaces.
|
||||
|
||||
template<typename StorageClassT, typename ElementT, typename TupleT, typename NodeT>
|
||||
void
|
||||
init_element(TupleT & t, NodeT & n)
|
||||
{
|
||||
std::get<std::shared_ptr<ElementT>>(t) =
|
||||
NodeInterfacesSupports<StorageClassT, ElementT>::get_from_node_like(n);
|
||||
}
|
||||
|
||||
template<typename NodeT, typename ... Ts>
|
||||
std::tuple<std::shared_ptr<Ts>...>
|
||||
init_tuple(NodeT & n)
|
||||
{
|
||||
using StorageClassT = NodeInterfacesStorage<Ts ...>;
|
||||
std::tuple<std::shared_ptr<Ts>...> t;
|
||||
(init_element<StorageClassT, Ts>(t, n), ...);
|
||||
return t;
|
||||
}
|
||||
|
||||
/// Macro for creating specializations with less boilerplate.
|
||||
/**
|
||||
* You can use this macro to add support for your interface class if:
|
||||
*
|
||||
* - The standard getter is get_node_{NodeInterfaceName}_interface(), and
|
||||
* - the getter returns a non-const shared_ptr<{NodeInterfaceType}>
|
||||
*
|
||||
* Examples of using this can be seen in the standard node interface headers
|
||||
* in rclcpp, e.g. rclcpp/node_interfaces/node_base_interface.hpp has:
|
||||
*
|
||||
* RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base)
|
||||
*
|
||||
* If your interface has a non-standard getter, or you want to instrument it or
|
||||
* something like that, then you'll need to create your own specialization of
|
||||
* the NodeInterfacesSupports struct without this macro.
|
||||
*/
|
||||
#define RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(NodeInterfaceType, NodeInterfaceName) \
|
||||
namespace rclcpp::node_interfaces::detail { \
|
||||
template<typename StorageClassT, typename ... RemainingInterfaceTs> \
|
||||
struct NodeInterfacesSupports< \
|
||||
StorageClassT, \
|
||||
NodeInterfaceType, \
|
||||
RemainingInterfaceTs ...> \
|
||||
: public NodeInterfacesSupports<StorageClassT, RemainingInterfaceTs ...> \
|
||||
{ \
|
||||
using is_supported = std::true_type; \
|
||||
\
|
||||
template<typename NodeT> \
|
||||
static \
|
||||
std::shared_ptr<NodeInterfaceType> \
|
||||
get_from_node_like(NodeT & node_like) \
|
||||
{ \
|
||||
return node_like.get_node_ ## NodeInterfaceName ## _interface(); \
|
||||
} \
|
||||
\
|
||||
/* Perfect forwarding constructor to get arguments down to StorageClassT (eventually). */ \
|
||||
template<typename ... ArgsT> \
|
||||
explicit NodeInterfacesSupports(ArgsT && ... args) \
|
||||
: NodeInterfacesSupports<StorageClassT, RemainingInterfaceTs ...>( \
|
||||
std::forward<ArgsT>(args) ...) \
|
||||
{} \
|
||||
\
|
||||
std::shared_ptr<NodeInterfaceType> \
|
||||
get_node_ ## NodeInterfaceName ## _interface() \
|
||||
{ \
|
||||
return StorageClassT::template get<NodeInterfaceType>(); \
|
||||
} \
|
||||
}; \
|
||||
} // namespace rclcpp::node_interfaces::detail
|
||||
|
||||
} // namespace detail
|
||||
} // namespace node_interfaces
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__NODE_INTERFACES__DETAIL__NODE_INTERFACES_HELPERS_HPP_
|
||||
@@ -121,19 +121,10 @@ public:
|
||||
std::atomic_bool &
|
||||
get_associated_with_executor_atomic() override;
|
||||
|
||||
[[deprecated("Use get_shared_notify_guard_condition or trigger_notify_guard_condition instead")]]
|
||||
RCLCPP_PUBLIC
|
||||
rclcpp::GuardCondition &
|
||||
get_notify_guard_condition() override;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
rclcpp::GuardCondition::SharedPtr
|
||||
get_shared_notify_guard_condition() override;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
trigger_notify_guard_condition() override;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
get_use_intra_process_default() const override;
|
||||
@@ -162,7 +153,7 @@ private:
|
||||
|
||||
/// Guard condition for notifying the Executor of changes to this node.
|
||||
mutable std::recursive_mutex notify_guard_condition_mutex_;
|
||||
std::shared_ptr<rclcpp::GuardCondition> notify_guard_condition_;
|
||||
rclcpp::GuardCondition notify_guard_condition_;
|
||||
bool notify_guard_condition_is_valid_;
|
||||
};
|
||||
|
||||
|
||||
@@ -26,7 +26,6 @@
|
||||
#include "rclcpp/context.hpp"
|
||||
#include "rclcpp/guard_condition.hpp"
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
@@ -148,33 +147,13 @@ public:
|
||||
/**
|
||||
* For example, this should be notified when a publisher is added or removed.
|
||||
*
|
||||
* \return the GuardCondition if it is valid, else throw runtime error
|
||||
* \return the GuardCondition if it is valid, else thow runtime error
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
virtual
|
||||
rclcpp::GuardCondition &
|
||||
get_notify_guard_condition() = 0;
|
||||
|
||||
/// Return a guard condition that should be notified when the internal node state changes.
|
||||
/**
|
||||
* For example, this should be notified when a publisher is added or removed.
|
||||
*
|
||||
* \return the GuardCondition if it is valid, else nullptr
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
virtual
|
||||
rclcpp::GuardCondition::SharedPtr
|
||||
get_shared_notify_guard_condition() = 0;
|
||||
|
||||
/// Trigger the guard condition that notifies of internal node state changes.
|
||||
/**
|
||||
* For example, this should be notified when a publisher is added or removed.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
virtual
|
||||
void
|
||||
trigger_notify_guard_condition() = 0;
|
||||
|
||||
/// Return the default preference for using intra process communication.
|
||||
RCLCPP_PUBLIC
|
||||
virtual
|
||||
@@ -198,6 +177,4 @@ public:
|
||||
} // namespace node_interfaces
|
||||
} // namespace rclcpp
|
||||
|
||||
RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base)
|
||||
|
||||
#endif // RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_
|
||||
|
||||
@@ -15,7 +15,6 @@
|
||||
#ifndef RCLCPP__NODE_INTERFACES__NODE_CLOCK_HPP_
|
||||
#define RCLCPP__NODE_INTERFACES__NODE_CLOCK_HPP_
|
||||
|
||||
#include "rcl/time.h"
|
||||
#include "rclcpp/clock.hpp"
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/node_interfaces/node_base_interface.hpp"
|
||||
@@ -43,8 +42,7 @@ public:
|
||||
rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
|
||||
rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
|
||||
rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services,
|
||||
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
|
||||
rcl_clock_type_t clock_type);
|
||||
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual
|
||||
@@ -69,7 +67,7 @@ private:
|
||||
rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
|
||||
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
|
||||
|
||||
rclcpp::Clock::SharedPtr clock_;
|
||||
rclcpp::Clock::SharedPtr ros_clock_;
|
||||
};
|
||||
|
||||
} // namespace node_interfaces
|
||||
|
||||
@@ -17,7 +17,6 @@
|
||||
|
||||
#include "rclcpp/clock.hpp"
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
@@ -51,6 +50,4 @@ public:
|
||||
} // namespace node_interfaces
|
||||
} // namespace rclcpp
|
||||
|
||||
RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeClockInterface, clock)
|
||||
|
||||
#endif // RCLCPP__NODE_INTERFACES__NODE_CLOCK_INTERFACE_HPP_
|
||||
|
||||
@@ -29,7 +29,6 @@
|
||||
|
||||
#include "rclcpp/event.hpp"
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
|
||||
#include "rclcpp/qos.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
@@ -57,8 +56,7 @@ public:
|
||||
node_namespace_(info.node_namespace),
|
||||
topic_type_(info.topic_type),
|
||||
endpoint_type_(static_cast<rclcpp::EndpointType>(info.endpoint_type)),
|
||||
qos_profile_({info.qos_profile.history, info.qos_profile.depth}, info.qos_profile),
|
||||
topic_type_hash_(info.topic_type_hash)
|
||||
qos_profile_({info.qos_profile.history, info.qos_profile.depth}, info.qos_profile)
|
||||
{
|
||||
std::copy(info.endpoint_gid, info.endpoint_gid + RMW_GID_STORAGE_SIZE, endpoint_gid_.begin());
|
||||
}
|
||||
@@ -123,16 +121,6 @@ public:
|
||||
const rclcpp::QoS &
|
||||
qos_profile() const;
|
||||
|
||||
/// Get a mutable reference to the type hash of the topic endpoint.
|
||||
RCLCPP_PUBLIC
|
||||
rosidl_type_hash_t &
|
||||
topic_type_hash();
|
||||
|
||||
/// Get a const reference to the type hash of the topic endpoint.
|
||||
RCLCPP_PUBLIC
|
||||
const rosidl_type_hash_t &
|
||||
topic_type_hash() const;
|
||||
|
||||
private:
|
||||
std::string node_name_;
|
||||
std::string node_namespace_;
|
||||
@@ -140,7 +128,6 @@ private:
|
||||
rclcpp::EndpointType endpoint_type_;
|
||||
std::array<uint8_t, RMW_GID_STORAGE_SIZE> endpoint_gid_;
|
||||
rclcpp::QoS qos_profile_;
|
||||
rosidl_type_hash_t topic_type_hash_;
|
||||
};
|
||||
|
||||
namespace node_interfaces
|
||||
@@ -395,6 +382,4 @@ public:
|
||||
} // namespace node_interfaces
|
||||
} // namespace rclcpp
|
||||
|
||||
RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeGraphInterface, graph)
|
||||
|
||||
#endif // RCLCPP__NODE_INTERFACES__NODE_GRAPH_INTERFACE_HPP_
|
||||
|
||||
@@ -1,164 +0,0 @@
|
||||
// Copyright 2022 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_INTERFACES_HPP_
|
||||
#define RCLCPP__NODE_INTERFACES__NODE_INTERFACES_HPP_
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include "rclcpp/detail/template_unique.hpp"
|
||||
#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
|
||||
|
||||
#define ALL_RCLCPP_NODE_INTERFACES \
|
||||
rclcpp::node_interfaces::NodeBaseInterface, \
|
||||
rclcpp::node_interfaces::NodeClockInterface, \
|
||||
rclcpp::node_interfaces::NodeGraphInterface, \
|
||||
rclcpp::node_interfaces::NodeLoggingInterface, \
|
||||
rclcpp::node_interfaces::NodeParametersInterface, \
|
||||
rclcpp::node_interfaces::NodeServicesInterface, \
|
||||
rclcpp::node_interfaces::NodeTimeSourceInterface, \
|
||||
rclcpp::node_interfaces::NodeTimersInterface, \
|
||||
rclcpp::node_interfaces::NodeTopicsInterface, \
|
||||
rclcpp::node_interfaces::NodeWaitablesInterface
|
||||
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace node_interfaces
|
||||
{
|
||||
|
||||
|
||||
/// A helper class for aggregating node interfaces.
|
||||
template<typename ... InterfaceTs>
|
||||
class NodeInterfaces
|
||||
: public detail::NodeInterfacesSupportCheck<
|
||||
detail::NodeInterfacesStorage<InterfaceTs ...>,
|
||||
InterfaceTs ...
|
||||
>,
|
||||
public detail::NodeInterfacesSupports<
|
||||
detail::NodeInterfacesStorage<InterfaceTs ...>,
|
||||
InterfaceTs ...
|
||||
>
|
||||
{
|
||||
static_assert(
|
||||
0 != sizeof ...(InterfaceTs),
|
||||
"must provide at least one interface as a template argument");
|
||||
static_assert(
|
||||
rclcpp::detail::template_unique_v<InterfaceTs ...>,
|
||||
"must provide unique template parameters");
|
||||
|
||||
using NodeInterfacesSupportsT = detail::NodeInterfacesSupports<
|
||||
detail::NodeInterfacesStorage<InterfaceTs ...>,
|
||||
InterfaceTs ...
|
||||
>;
|
||||
|
||||
public:
|
||||
/// Create a new NodeInterfaces object using the given node-like object's interfaces.
|
||||
/**
|
||||
* Specify which interfaces you need by passing them as template parameters.
|
||||
*
|
||||
* This allows you to aggregate interfaces from different sources together to pass as a single
|
||||
* aggregate object to any functions that take node interfaces or node-likes, without needing to
|
||||
* templatize that function.
|
||||
*
|
||||
* You may also use this constructor to create a NodeInterfaces that contains a subset of
|
||||
* another NodeInterfaces' interfaces.
|
||||
*
|
||||
* Finally, this class supports implicit conversion from node-like objects, allowing you to
|
||||
* directly pass a node-like to a function that takes a NodeInterfaces object.
|
||||
*
|
||||
* Usage examples:
|
||||
* ```cpp
|
||||
* // Suppose we have some function:
|
||||
* void fn(NodeInterfaces<NodeBaseInterface, NodeClockInterface> interfaces);
|
||||
*
|
||||
* // Then we can, explicitly:
|
||||
* rclcpp::Node node("some_node");
|
||||
* auto ni = NodeInterfaces<NodeBaseInterface, NodeClockInterface>(node);
|
||||
* fn(ni);
|
||||
*
|
||||
* // But also:
|
||||
* fn(node);
|
||||
*
|
||||
* // Subsetting a NodeInterfaces object also works!
|
||||
* auto ni_base = NodeInterfaces<NodeBaseInterface>(ni);
|
||||
*
|
||||
* // Or aggregate them (you could aggregate interfaces from disparate node-likes)
|
||||
* auto ni_aggregated = NodeInterfaces<NodeBaseInterface, NodeClockInterface>(
|
||||
* node->get_node_base_interface(),
|
||||
* node->get_node_clock_interface()
|
||||
* )
|
||||
*
|
||||
* // And then to access the interfaces:
|
||||
* // Get with get<>
|
||||
* auto base = ni.get<NodeBaseInterface>();
|
||||
*
|
||||
* // Or the appropriate getter
|
||||
* auto clock = ni.get_clock_interface();
|
||||
* ```
|
||||
*
|
||||
* You may use any of the standard node interfaces that come with rclcpp:
|
||||
* - rclcpp::node_interfaces::NodeBaseInterface
|
||||
* - rclcpp::node_interfaces::NodeClockInterface
|
||||
* - rclcpp::node_interfaces::NodeGraphInterface
|
||||
* - rclcpp::node_interfaces::NodeLoggingInterface
|
||||
* - rclcpp::node_interfaces::NodeParametersInterface
|
||||
* - rclcpp::node_interfaces::NodeServicesInterface
|
||||
* - rclcpp::node_interfaces::NodeTimeSourceInterface
|
||||
* - rclcpp::node_interfaces::NodeTimersInterface
|
||||
* - rclcpp::node_interfaces::NodeTopicsInterface
|
||||
* - rclcpp::node_interfaces::NodeWaitablesInterface
|
||||
*
|
||||
* Or you use custom interfaces as long as you make a template specialization
|
||||
* of the rclcpp::node_interfaces::detail::NodeInterfacesSupport struct using
|
||||
* the RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT macro.
|
||||
*
|
||||
* Usage example:
|
||||
* ```RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base)```
|
||||
*
|
||||
* If you choose not to use the helper macro, then you can specialize the
|
||||
* template yourself, but you must:
|
||||
*
|
||||
* - Provide a template specialization of the get_from_node_like method that gets the interface
|
||||
* from any node-like that stores the interface, using the node-like's getter
|
||||
* - Designate the is_supported type as std::true_type using a using directive
|
||||
* - Provide any number of getter methods to be used to obtain the interface with the
|
||||
* NodeInterface object, noting that the getters of the storage class will apply to all
|
||||
* supported interfaces.
|
||||
* - The getter method names should not clash in name with any other interface getter
|
||||
* specializations if those other interfaces are meant to be aggregated in the same
|
||||
* NodeInterfaces object.
|
||||
*
|
||||
* \param[in] node Node-like object from which to get the node interfaces
|
||||
*/
|
||||
template<typename NodeT>
|
||||
NodeInterfaces(NodeT & node) // NOLINT(runtime/explicit)
|
||||
: NodeInterfacesSupportsT(node)
|
||||
{}
|
||||
|
||||
// Create a NodeInterfaces object with no bound interfaces
|
||||
NodeInterfaces()
|
||||
: NodeInterfacesSupportsT()
|
||||
{}
|
||||
|
||||
explicit NodeInterfaces(std::shared_ptr<InterfaceTs>... args)
|
||||
: NodeInterfacesSupportsT(args ...)
|
||||
{}
|
||||
};
|
||||
|
||||
|
||||
} // namespace node_interfaces
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__NODE_INTERFACES__NODE_INTERFACES_HPP_
|
||||
@@ -19,7 +19,6 @@
|
||||
|
||||
#include "rclcpp/logger.hpp"
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
@@ -59,6 +58,4 @@ public:
|
||||
} // namespace node_interfaces
|
||||
} // namespace rclcpp
|
||||
|
||||
RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeLoggingInterface, logging)
|
||||
|
||||
#endif // RCLCPP__NODE_INTERFACES__NODE_LOGGING_INTERFACE_HPP_
|
||||
|
||||
@@ -162,7 +162,7 @@ public:
|
||||
bool
|
||||
get_parameter(
|
||||
const std::string & name,
|
||||
rclcpp::Parameter & parameter) const override;
|
||||
rclcpp::Parameter & parameter) const noexcept override;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
|
||||
@@ -25,7 +25,6 @@
|
||||
#include "rcl_interfaces/msg/set_parameters_result.hpp"
|
||||
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
|
||||
#include "rclcpp/parameter.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
@@ -177,7 +176,7 @@ public:
|
||||
bool
|
||||
get_parameter(
|
||||
const std::string & name,
|
||||
rclcpp::Parameter & parameter) const = 0;
|
||||
rclcpp::Parameter & parameter) const noexcept = 0;
|
||||
|
||||
/// Get all parameters that have the specified prefix into the parameters map.
|
||||
/*
|
||||
@@ -277,6 +276,4 @@ public:
|
||||
} // namespace node_interfaces
|
||||
} // namespace rclcpp
|
||||
|
||||
RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeParametersInterface, parameters)
|
||||
|
||||
#endif // RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_
|
||||
|
||||
@@ -20,7 +20,6 @@
|
||||
#include "rclcpp/callback_group.hpp"
|
||||
#include "rclcpp/client.hpp"
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
|
||||
#include "rclcpp/service.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
@@ -63,6 +62,4 @@ public:
|
||||
} // namespace node_interfaces
|
||||
} // namespace rclcpp
|
||||
|
||||
RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeServicesInterface, services)
|
||||
|
||||
#endif // RCLCPP__NODE_INTERFACES__NODE_SERVICES_INTERFACE_HPP_
|
||||
|
||||
@@ -16,7 +16,6 @@
|
||||
#define RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_
|
||||
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
@@ -38,6 +37,4 @@ public:
|
||||
} // namespace node_interfaces
|
||||
} // namespace rclcpp
|
||||
|
||||
RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeTimeSourceInterface, time_source)
|
||||
|
||||
#endif // RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_
|
||||
|
||||
@@ -17,7 +17,6 @@
|
||||
|
||||
#include "rclcpp/callback_group.hpp"
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
|
||||
#include "rclcpp/timer.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
@@ -48,6 +47,4 @@ public:
|
||||
} // namespace node_interfaces
|
||||
} // namespace rclcpp
|
||||
|
||||
RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeTimersInterface, timers)
|
||||
|
||||
#endif // RCLCPP__NODE_INTERFACES__NODE_TIMERS_INTERFACE_HPP_
|
||||
|
||||
@@ -22,7 +22,6 @@
|
||||
|
||||
#include "rclcpp/callback_group.hpp"
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
|
||||
#include "rclcpp/node_interfaces/node_base_interface.hpp"
|
||||
#include "rclcpp/node_interfaces/node_timers_interface.hpp"
|
||||
#include "rclcpp/publisher.hpp"
|
||||
@@ -96,6 +95,4 @@ public:
|
||||
} // namespace node_interfaces
|
||||
} // namespace rclcpp
|
||||
|
||||
RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeTopicsInterface, topics)
|
||||
|
||||
#endif // RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_
|
||||
|
||||
@@ -17,7 +17,6 @@
|
||||
|
||||
#include "rclcpp/callback_group.hpp"
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
#include "rclcpp/waitable.hpp"
|
||||
|
||||
@@ -55,6 +54,4 @@ public:
|
||||
} // namespace node_interfaces
|
||||
} // namespace rclcpp
|
||||
|
||||
RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeWaitablesInterface, waitables)
|
||||
|
||||
#endif // RCLCPP__NODE_INTERFACES__NODE_WAITABLES_INTERFACE_HPP_
|
||||
|
||||
@@ -19,7 +19,6 @@
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "rcl/time.h"
|
||||
#include "rcl/node_options.h"
|
||||
#include "rclcpp/context.hpp"
|
||||
#include "rclcpp/contexts/default_context.hpp"
|
||||
@@ -47,7 +46,6 @@ public:
|
||||
* - enable_topic_statistics = false
|
||||
* - start_parameter_services = true
|
||||
* - start_parameter_event_publisher = true
|
||||
* - clock_type = RCL_ROS_TIME
|
||||
* - clock_qos = rclcpp::ClockQoS()
|
||||
* - use_clock_thread = true
|
||||
* - rosout_qos = rclcpp::RosoutQoS()
|
||||
@@ -248,19 +246,6 @@ public:
|
||||
NodeOptions &
|
||||
start_parameter_event_publisher(bool start_parameter_event_publisher);
|
||||
|
||||
/// Return a reference to the clock type.
|
||||
RCLCPP_PUBLIC
|
||||
const rcl_clock_type_t &
|
||||
clock_type() const;
|
||||
|
||||
/// Set the clock type.
|
||||
/**
|
||||
* The clock type to be used by the node.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
NodeOptions &
|
||||
clock_type(const rcl_clock_type_t & clock_type);
|
||||
|
||||
/// Return a reference to the clock QoS.
|
||||
RCLCPP_PUBLIC
|
||||
const rclcpp::QoS &
|
||||
@@ -415,8 +400,6 @@ private:
|
||||
|
||||
bool start_parameter_event_publisher_ {true};
|
||||
|
||||
rcl_clock_type_t clock_type_ {RCL_ROS_TIME};
|
||||
|
||||
rclcpp::QoS clock_qos_ = rclcpp::ClockQoS();
|
||||
|
||||
bool use_clock_thread_ {true};
|
||||
|
||||
@@ -131,16 +131,40 @@ public:
|
||||
node_base,
|
||||
topic,
|
||||
rclcpp::get_message_type_support_handle<MessageT>(),
|
||||
options.template to_rcl_publisher_options<MessageT>(qos),
|
||||
// NOTE(methylDragon): Passing these args separately is necessary for event binding
|
||||
options.event_callbacks,
|
||||
options.use_default_callbacks),
|
||||
options.template to_rcl_publisher_options<MessageT>(qos)),
|
||||
options_(options),
|
||||
published_type_allocator_(*options.get_allocator()),
|
||||
ros_message_type_allocator_(*options.get_allocator())
|
||||
{
|
||||
allocator::set_allocator_for_deleter(&published_type_deleter_, &published_type_allocator_);
|
||||
allocator::set_allocator_for_deleter(&ros_message_type_deleter_, &ros_message_type_allocator_);
|
||||
|
||||
if (options_.event_callbacks.deadline_callback) {
|
||||
this->add_event_handler(
|
||||
options_.event_callbacks.deadline_callback,
|
||||
RCL_PUBLISHER_OFFERED_DEADLINE_MISSED);
|
||||
}
|
||||
if (options_.event_callbacks.liveliness_callback) {
|
||||
this->add_event_handler(
|
||||
options_.event_callbacks.liveliness_callback,
|
||||
RCL_PUBLISHER_LIVELINESS_LOST);
|
||||
}
|
||||
if (options_.event_callbacks.incompatible_qos_callback) {
|
||||
this->add_event_handler(
|
||||
options_.event_callbacks.incompatible_qos_callback,
|
||||
RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
|
||||
} else if (options_.use_default_callbacks) {
|
||||
// Register default callback when not specified
|
||||
try {
|
||||
this->add_event_handler(
|
||||
[this](QOSOfferedIncompatibleQoSInfo & info) {
|
||||
this->default_incompatible_qos_callback(info);
|
||||
},
|
||||
RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
|
||||
} catch (UnsupportedEventTypeException & /*exc*/) {
|
||||
// pass
|
||||
}
|
||||
}
|
||||
// Setup continues in the post construction method, post_init_setup().
|
||||
}
|
||||
|
||||
@@ -381,6 +405,10 @@ public:
|
||||
if (!loaned_msg.is_valid()) {
|
||||
throw std::runtime_error("loaned message is not valid");
|
||||
}
|
||||
if (intra_process_is_enabled_) {
|
||||
// TODO(Karsten1987): support loaned message passed by intraprocess
|
||||
throw std::runtime_error("storing loaned messages in intra process is not supported yet");
|
||||
}
|
||||
|
||||
// verify that publisher supports loaned messages
|
||||
// TODO(Karsten1987): This case separation has to be done in rclcpp
|
||||
@@ -394,7 +422,7 @@ public:
|
||||
} else {
|
||||
// we don't release the ownership, let the middleware copy the ros message
|
||||
// and thus the destructor of rclcpp::LoanedMessage cleans up the memory.
|
||||
this->publish(loaned_msg.get());
|
||||
this->do_inter_process_publish(loaned_msg.get());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -484,10 +512,6 @@ protected:
|
||||
if (!msg) {
|
||||
throw std::runtime_error("cannot publish msg which is a null pointer");
|
||||
}
|
||||
TRACEPOINT(
|
||||
rclcpp_intra_publish,
|
||||
static_cast<const void *>(publisher_handle_.get()),
|
||||
msg.get());
|
||||
|
||||
ipm->template do_intra_process_publish<PublishedType, ROSMessageType, AllocatorT>(
|
||||
intra_process_publisher_id_,
|
||||
@@ -506,10 +530,6 @@ protected:
|
||||
if (!msg) {
|
||||
throw std::runtime_error("cannot publish msg which is a null pointer");
|
||||
}
|
||||
TRACEPOINT(
|
||||
rclcpp_intra_publish,
|
||||
static_cast<const void *>(publisher_handle_.get()),
|
||||
msg.get());
|
||||
|
||||
ipm->template do_intra_process_publish<ROSMessageType, ROSMessageType, AllocatorT>(
|
||||
intra_process_publisher_id_,
|
||||
@@ -529,10 +549,6 @@ protected:
|
||||
if (!msg) {
|
||||
throw std::runtime_error("cannot publish msg which is a null pointer");
|
||||
}
|
||||
TRACEPOINT(
|
||||
rclcpp_intra_publish,
|
||||
static_cast<const void *>(publisher_handle_.get()),
|
||||
msg.get());
|
||||
|
||||
return ipm->template do_intra_process_publish_and_return_shared<ROSMessageType, ROSMessageType,
|
||||
AllocatorT>(
|
||||
|
||||
@@ -33,7 +33,7 @@
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/network_flow_endpoint.hpp"
|
||||
#include "rclcpp/qos.hpp"
|
||||
#include "rclcpp/event_handler.hpp"
|
||||
#include "rclcpp/qos_event.hpp"
|
||||
#include "rclcpp/type_support_decl.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
#include "rcpputils/time.hpp"
|
||||
@@ -78,18 +78,11 @@ public:
|
||||
rclcpp::node_interfaces::NodeBaseInterface * node_base,
|
||||
const std::string & topic,
|
||||
const rosidl_message_type_support_t & type_support,
|
||||
const rcl_publisher_options_t & publisher_options,
|
||||
const PublisherEventCallbacks & event_callbacks,
|
||||
bool use_default_callbacks);
|
||||
const rcl_publisher_options_t & publisher_options);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual ~PublisherBase();
|
||||
|
||||
/// Add event handlers for passed in event_callbacks.
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
bind_event_callbacks(const PublisherEventCallbacks & event_callbacks, bool use_default_callbacks);
|
||||
|
||||
/// Get the topic that this publisher publishes on.
|
||||
/** \return The topic name. */
|
||||
RCLCPP_PUBLIC
|
||||
@@ -124,7 +117,7 @@ public:
|
||||
/** \return The map of QoS event handlers. */
|
||||
RCLCPP_PUBLIC
|
||||
const
|
||||
std::unordered_map<rcl_publisher_event_type_t, std::shared_ptr<rclcpp::EventHandlerBase>> &
|
||||
std::unordered_map<rcl_publisher_event_type_t, std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
|
||||
get_event_handlers() const;
|
||||
|
||||
/// Get subscription count
|
||||
@@ -276,7 +269,7 @@ public:
|
||||
* If you want more information available in the callback, like the qos event
|
||||
* or other information, you may use a lambda with captures or std::bind.
|
||||
*
|
||||
* \sa rclcpp::EventHandlerBase::set_on_ready_callback
|
||||
* \sa rclcpp::QOSEventHandlerBase::set_on_ready_callback
|
||||
*
|
||||
* \param[in] callback functor to be called when a new event occurs
|
||||
* \param[in] event_type identifier for the qos event we want to attach the callback to
|
||||
@@ -327,7 +320,7 @@ protected:
|
||||
const EventCallbackT & callback,
|
||||
const rcl_publisher_event_type_t event_type)
|
||||
{
|
||||
auto handler = std::make_shared<EventHandler<EventCallbackT,
|
||||
auto handler = std::make_shared<QOSEventHandler<EventCallbackT,
|
||||
std::shared_ptr<rcl_publisher_t>>>(
|
||||
callback,
|
||||
rcl_publisher_event_init,
|
||||
@@ -339,15 +332,12 @@ protected:
|
||||
RCLCPP_PUBLIC
|
||||
void default_incompatible_qos_callback(QOSOfferedIncompatibleQoSInfo & info) const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void default_incompatible_type_callback(IncompatibleTypeInfo & info) const;
|
||||
|
||||
std::shared_ptr<rcl_node_t> rcl_node_handle_;
|
||||
|
||||
std::shared_ptr<rcl_publisher_t> publisher_handle_;
|
||||
|
||||
std::unordered_map<rcl_publisher_event_type_t,
|
||||
std::shared_ptr<rclcpp::EventHandlerBase>> event_handlers_;
|
||||
std::shared_ptr<rclcpp::QOSEventHandlerBase>> event_handlers_;
|
||||
|
||||
using IntraProcessManagerWeakPtr =
|
||||
std::weak_ptr<rclcpp::experimental::IntraProcessManager>;
|
||||
@@ -358,8 +348,6 @@ protected:
|
||||
rmw_gid_t rmw_gid_;
|
||||
|
||||
const rosidl_message_type_support_t type_support_;
|
||||
|
||||
const PublisherEventCallbacks event_callbacks_;
|
||||
};
|
||||
|
||||
} // namespace rclcpp
|
||||
|
||||
@@ -26,7 +26,7 @@
|
||||
#include "rclcpp/detail/rmw_implementation_specific_publisher_payload.hpp"
|
||||
#include "rclcpp/intra_process_setting.hpp"
|
||||
#include "rclcpp/qos.hpp"
|
||||
#include "rclcpp/event_handler.hpp"
|
||||
#include "rclcpp/qos_event.hpp"
|
||||
#include "rclcpp/qos_overriding_options.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
|
||||
@@ -80,9 +80,7 @@ struct RCLCPP_PUBLIC QoSInitialization
|
||||
size_t depth;
|
||||
|
||||
/// Constructor which takes both a history policy and a depth (even if it would be unused).
|
||||
QoSInitialization(
|
||||
rmw_qos_history_policy_t history_policy_arg, size_t depth_arg,
|
||||
bool print_depth_warning = true);
|
||||
QoSInitialization(rmw_qos_history_policy_t history_policy_arg, size_t depth_arg);
|
||||
|
||||
/// Create a QoSInitialization from an existing rmw_qos_profile_t, using its history and depth.
|
||||
static
|
||||
@@ -99,7 +97,7 @@ struct RCLCPP_PUBLIC KeepAll : public rclcpp::QoSInitialization
|
||||
/// Use to initialize the QoS with the keep_last history setting and the given depth.
|
||||
struct RCLCPP_PUBLIC KeepLast : public rclcpp::QoSInitialization
|
||||
{
|
||||
explicit KeepLast(size_t depth, bool print_depth_warning = true);
|
||||
explicit KeepLast(size_t depth);
|
||||
};
|
||||
|
||||
/// Encapsulation of Quality of Service settings.
|
||||
|
||||
@@ -15,8 +15,277 @@
|
||||
#ifndef RCLCPP__QOS_EVENT_HPP_
|
||||
#define RCLCPP__QOS_EVENT_HPP_
|
||||
|
||||
#warning This header is obsolete, please include rclcpp/event_handler.hpp instead
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
|
||||
#include "rclcpp/event_handler.hpp"
|
||||
#include "rcl/error_handling.h"
|
||||
#include "rcl/event_callback.h"
|
||||
#include "rmw/impl/cpp/demangle.hpp"
|
||||
#include "rmw/incompatible_qos_events_statuses.h"
|
||||
|
||||
#include "rcutils/logging_macros.h"
|
||||
|
||||
#include "rclcpp/detail/cpp_callback_trampoline.hpp"
|
||||
#include "rclcpp/exceptions.hpp"
|
||||
#include "rclcpp/function_traits.hpp"
|
||||
#include "rclcpp/logging.hpp"
|
||||
#include "rclcpp/waitable.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
|
||||
using QOSDeadlineRequestedInfo = rmw_requested_deadline_missed_status_t;
|
||||
using QOSDeadlineOfferedInfo = rmw_offered_deadline_missed_status_t;
|
||||
using QOSLivelinessChangedInfo = rmw_liveliness_changed_status_t;
|
||||
using QOSLivelinessLostInfo = rmw_liveliness_lost_status_t;
|
||||
using QOSMessageLostInfo = rmw_message_lost_status_t;
|
||||
using QOSOfferedIncompatibleQoSInfo = rmw_offered_qos_incompatible_event_status_t;
|
||||
using QOSRequestedIncompatibleQoSInfo = rmw_requested_qos_incompatible_event_status_t;
|
||||
|
||||
using QOSDeadlineRequestedCallbackType = std::function<void (QOSDeadlineRequestedInfo &)>;
|
||||
using QOSDeadlineOfferedCallbackType = std::function<void (QOSDeadlineOfferedInfo &)>;
|
||||
using QOSLivelinessChangedCallbackType = std::function<void (QOSLivelinessChangedInfo &)>;
|
||||
using QOSLivelinessLostCallbackType = std::function<void (QOSLivelinessLostInfo &)>;
|
||||
using QOSMessageLostCallbackType = std::function<void (QOSMessageLostInfo &)>;
|
||||
using QOSOfferedIncompatibleQoSCallbackType = std::function<void (QOSOfferedIncompatibleQoSInfo &)>;
|
||||
using QOSRequestedIncompatibleQoSCallbackType =
|
||||
std::function<void (QOSRequestedIncompatibleQoSInfo &)>;
|
||||
|
||||
/// Contains callbacks for various types of events a Publisher can receive from the middleware.
|
||||
struct PublisherEventCallbacks
|
||||
{
|
||||
QOSDeadlineOfferedCallbackType deadline_callback;
|
||||
QOSLivelinessLostCallbackType liveliness_callback;
|
||||
QOSOfferedIncompatibleQoSCallbackType incompatible_qos_callback;
|
||||
};
|
||||
|
||||
/// Contains callbacks for non-message events that a Subscription can receive from the middleware.
|
||||
struct SubscriptionEventCallbacks
|
||||
{
|
||||
QOSDeadlineRequestedCallbackType deadline_callback;
|
||||
QOSLivelinessChangedCallbackType liveliness_callback;
|
||||
QOSRequestedIncompatibleQoSCallbackType incompatible_qos_callback;
|
||||
QOSMessageLostCallbackType message_lost_callback;
|
||||
};
|
||||
|
||||
class UnsupportedEventTypeException : public exceptions::RCLErrorBase, public std::runtime_error
|
||||
{
|
||||
public:
|
||||
RCLCPP_PUBLIC
|
||||
UnsupportedEventTypeException(
|
||||
rcl_ret_t ret,
|
||||
const rcl_error_state_t * error_state,
|
||||
const std::string & prefix);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
UnsupportedEventTypeException(
|
||||
const exceptions::RCLErrorBase & base_exc,
|
||||
const std::string & prefix);
|
||||
};
|
||||
|
||||
class QOSEventHandlerBase : public Waitable
|
||||
{
|
||||
public:
|
||||
enum class EntityType : std::size_t
|
||||
{
|
||||
Event,
|
||||
};
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual ~QOSEventHandlerBase();
|
||||
|
||||
/// Get the number of ready events
|
||||
RCLCPP_PUBLIC
|
||||
size_t
|
||||
get_number_of_ready_events() override;
|
||||
|
||||
/// Add the Waitable to a wait set.
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
add_to_wait_set(rcl_wait_set_t * wait_set) override;
|
||||
|
||||
/// Check if the Waitable is ready.
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
is_ready(rcl_wait_set_t * wait_set) override;
|
||||
|
||||
/// Set a callback to be called when each new event instance occurs.
|
||||
/**
|
||||
* The callback receives a size_t which is the number of events that occurred
|
||||
* since the last time this callback was called.
|
||||
* Normally this is 1, but can be > 1 if events occurred before any
|
||||
* callback was set.
|
||||
*
|
||||
* The callback also receives an int identifier argument.
|
||||
* This is needed because a Waitable may be composed of several distinct entities,
|
||||
* such as subscriptions, services, etc.
|
||||
* The application should provide a generic callback function that will be then
|
||||
* forwarded by the waitable to all of its entities.
|
||||
* Before forwarding, a different value for the identifier argument will be
|
||||
* bond to the function.
|
||||
* This implies that the provided callback can use the identifier to behave
|
||||
* differently depending on which entity triggered the waitable to become ready.
|
||||
*
|
||||
* Since this callback is called from the middleware, you should aim to make
|
||||
* it fast and not blocking.
|
||||
* If you need to do a lot of work or wait for some other event, you should
|
||||
* spin it off to another thread, otherwise you risk blocking the middleware.
|
||||
*
|
||||
* Calling it again will clear any previously set callback.
|
||||
*
|
||||
* An exception will be thrown if the callback is not callable.
|
||||
*
|
||||
* This function is thread-safe.
|
||||
*
|
||||
* If you want more information available in the callback, like the qos event
|
||||
* or other information, you may use a lambda with captures or std::bind.
|
||||
*
|
||||
* \sa rmw_event_set_callback
|
||||
* \sa rcl_event_set_callback
|
||||
*
|
||||
* \param[in] callback functor to be called when a new event occurs
|
||||
*/
|
||||
void
|
||||
set_on_ready_callback(std::function<void(size_t, int)> callback) override
|
||||
{
|
||||
if (!callback) {
|
||||
throw std::invalid_argument(
|
||||
"The callback passed to set_on_ready_callback "
|
||||
"is not callable.");
|
||||
}
|
||||
|
||||
// Note: we bind the int identifier argument to this waitable's entity types
|
||||
auto new_callback =
|
||||
[callback, this](size_t number_of_events) {
|
||||
try {
|
||||
callback(number_of_events, static_cast<int>(EntityType::Event));
|
||||
} catch (const std::exception & exception) {
|
||||
RCLCPP_ERROR_STREAM(
|
||||
// TODO(wjwwood): get this class access to the node logger it is associated with
|
||||
rclcpp::get_logger("rclcpp"),
|
||||
"rclcpp::QOSEventHandlerBase@" << this <<
|
||||
" caught " << rmw::impl::cpp::demangle(exception) <<
|
||||
" exception in user-provided callback for the 'on ready' callback: " <<
|
||||
exception.what());
|
||||
} catch (...) {
|
||||
RCLCPP_ERROR_STREAM(
|
||||
rclcpp::get_logger("rclcpp"),
|
||||
"rclcpp::QOSEventHandlerBase@" << this <<
|
||||
" caught unhandled exception in user-provided callback " <<
|
||||
"for the 'on ready' callback");
|
||||
}
|
||||
};
|
||||
|
||||
std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
|
||||
|
||||
// Set it temporarily to the new callback, while we replace the old one.
|
||||
// This two-step setting, prevents a gap where the old std::function has
|
||||
// been replaced but the middleware hasn't been told about the new one yet.
|
||||
set_on_new_event_callback(
|
||||
rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
|
||||
static_cast<const void *>(&new_callback));
|
||||
|
||||
// Store the std::function to keep it in scope, also overwrites the existing one.
|
||||
on_new_event_callback_ = new_callback;
|
||||
|
||||
// Set it again, now using the permanent storage.
|
||||
set_on_new_event_callback(
|
||||
rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
|
||||
static_cast<const void *>(&on_new_event_callback_));
|
||||
}
|
||||
|
||||
/// Unset the callback registered for new events, if any.
|
||||
void
|
||||
clear_on_ready_callback() override
|
||||
{
|
||||
std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
|
||||
if (on_new_event_callback_) {
|
||||
set_on_new_event_callback(nullptr, nullptr);
|
||||
on_new_event_callback_ = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
protected:
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
set_on_new_event_callback(rcl_event_callback_t callback, const void * user_data);
|
||||
|
||||
rcl_event_t event_handle_;
|
||||
size_t wait_set_event_index_;
|
||||
std::recursive_mutex callback_mutex_;
|
||||
std::function<void(size_t)> on_new_event_callback_{nullptr};
|
||||
};
|
||||
|
||||
template<typename EventCallbackT, typename ParentHandleT>
|
||||
class QOSEventHandler : public QOSEventHandlerBase
|
||||
{
|
||||
public:
|
||||
template<typename InitFuncT, typename EventTypeEnum>
|
||||
QOSEventHandler(
|
||||
const EventCallbackT & callback,
|
||||
InitFuncT init_func,
|
||||
ParentHandleT parent_handle,
|
||||
EventTypeEnum event_type)
|
||||
: parent_handle_(parent_handle), event_callback_(callback)
|
||||
{
|
||||
event_handle_ = rcl_get_zero_initialized_event();
|
||||
rcl_ret_t ret = init_func(&event_handle_, parent_handle.get(), event_type);
|
||||
if (ret != RCL_RET_OK) {
|
||||
if (ret == RCL_RET_UNSUPPORTED) {
|
||||
UnsupportedEventTypeException exc(ret, rcl_get_error_state(), "Failed to initialize event");
|
||||
rcl_reset_error();
|
||||
throw exc;
|
||||
} else {
|
||||
rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to initialize event");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Take data so that the callback cannot be scheduled again
|
||||
std::shared_ptr<void>
|
||||
take_data() override
|
||||
{
|
||||
EventCallbackInfoT callback_info;
|
||||
rcl_ret_t ret = rcl_take_event(&event_handle_, &callback_info);
|
||||
if (ret != RCL_RET_OK) {
|
||||
RCUTILS_LOG_ERROR_NAMED(
|
||||
"rclcpp",
|
||||
"Couldn't take event info: %s", rcl_get_error_string().str);
|
||||
return nullptr;
|
||||
}
|
||||
return std::static_pointer_cast<void>(std::make_shared<EventCallbackInfoT>(callback_info));
|
||||
}
|
||||
|
||||
std::shared_ptr<void>
|
||||
take_data_by_entity_id(size_t id) override
|
||||
{
|
||||
(void)id;
|
||||
return take_data();
|
||||
}
|
||||
|
||||
/// Execute any entities of the Waitable that are ready.
|
||||
void
|
||||
execute(std::shared_ptr<void> & data) override
|
||||
{
|
||||
if (!data) {
|
||||
throw std::runtime_error("'data' is empty");
|
||||
}
|
||||
auto callback_ptr = std::static_pointer_cast<EventCallbackInfoT>(data);
|
||||
event_callback_(*callback_ptr);
|
||||
callback_ptr.reset();
|
||||
}
|
||||
|
||||
private:
|
||||
using EventCallbackInfoT = typename std::remove_reference<typename
|
||||
rclcpp::function_traits::function_traits<EventCallbackT>::template argument_type<0>>::type;
|
||||
|
||||
ParentHandleT parent_handle_;
|
||||
EventCallbackT event_callback_;
|
||||
};
|
||||
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__QOS_EVENT_HPP_
|
||||
|
||||
@@ -117,18 +117,6 @@
|
||||
* - Allocator related items:
|
||||
* - rclcpp/allocator/allocator_common.hpp
|
||||
* - rclcpp/allocator/allocator_deleter.hpp
|
||||
* - Dynamic typesupport wrappers
|
||||
* - rclcpp::dynamic_typesupport::DynamicMessage
|
||||
* - rclcpp::dynamic_typesupport::DynamicMessageType
|
||||
* - rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder
|
||||
* - rclcpp::dynamic_typesupport::DynamicSerializationSupport
|
||||
* - rclcpp/dynamic_typesupport/dynamic_message.hpp
|
||||
* - rclcpp/dynamic_typesupport/dynamic_message_type.hpp
|
||||
* - rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp
|
||||
* - rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp
|
||||
* - Dynamic typesupport
|
||||
* - rclcpp::dynamic_typesupport::DynamicMessageTypeSupport
|
||||
* - rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp
|
||||
* - Generic publisher
|
||||
* - rclcpp::Node::create_generic_publisher()
|
||||
* - rclcpp::GenericPublisher
|
||||
|
||||
56
rclcpp/include/rclcpp/scope_exit.hpp
Normal file
56
rclcpp/include/rclcpp/scope_exit.hpp
Normal file
@@ -0,0 +1,56 @@
|
||||
// Copyright 2015 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.
|
||||
|
||||
// Based on: http://the-witness.net/news/2012/11/scopeexit-in-c11/
|
||||
// But I changed the lambda to include by reference rather than value, see:
|
||||
// http://the-witness.net/news/2012/11/scopeexit-in-c11/comment-page-1/#comment-86873
|
||||
|
||||
#ifndef RCLCPP__SCOPE_EXIT_HPP_
|
||||
#define RCLCPP__SCOPE_EXIT_HPP_
|
||||
|
||||
// TODO(christophebedard) remove this header completely in I-turtle
|
||||
|
||||
#warning rclcpp/scope_exit.hpp has been deprecated, please use rcpputils/scope_exit.hpp instead
|
||||
|
||||
#include <functional>
|
||||
|
||||
#include "rclcpp/macros.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
|
||||
template<typename Callable>
|
||||
struct ScopeExit
|
||||
{
|
||||
explicit ScopeExit(Callable callable)
|
||||
: callable_(callable) {}
|
||||
~ScopeExit() {callable_();}
|
||||
|
||||
private:
|
||||
Callable callable_;
|
||||
};
|
||||
|
||||
template<typename Callable>
|
||||
ScopeExit<Callable>
|
||||
make_scope_exit(Callable callable)
|
||||
{
|
||||
return ScopeExit<Callable>(callable);
|
||||
}
|
||||
|
||||
} // namespace rclcpp
|
||||
|
||||
#define RCLCPP_SCOPE_EXIT(code) \
|
||||
auto RCLCPP_STRING_JOIN(scope_exit_, __LINE__) = rclcpp::make_scope_exit([&]() {code;})
|
||||
|
||||
#endif // RCLCPP__SCOPE_EXIT_HPP_
|
||||
@@ -26,7 +26,6 @@
|
||||
#include "rcl/error_handling.h"
|
||||
#include "rcl/event_callback.h"
|
||||
#include "rcl/service.h"
|
||||
#include "rcl/service_introspection.h"
|
||||
|
||||
#include "rmw/error_handling.h"
|
||||
#include "rmw/impl/cpp/demangle.hpp"
|
||||
@@ -35,7 +34,6 @@
|
||||
#include "tracetools/tracetools.h"
|
||||
|
||||
#include "rclcpp/any_service_callback.hpp"
|
||||
#include "rclcpp/clock.hpp"
|
||||
#include "rclcpp/detail/cpp_callback_trampoline.hpp"
|
||||
#include "rclcpp/exceptions.hpp"
|
||||
#include "rclcpp/expand_topic_or_service_name.hpp"
|
||||
@@ -224,7 +222,7 @@ public:
|
||||
// This two-step setting, prevents a gap where the old std::function has
|
||||
// been replaced but the middleware hasn't been told about the new one yet.
|
||||
set_on_new_request_callback(
|
||||
rclcpp::detail::cpp_callback_trampoline<decltype(new_callback), const void *, size_t>,
|
||||
rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
|
||||
static_cast<const void *>(&new_callback));
|
||||
|
||||
// Store the std::function to keep it in scope, also overwrites the existing one.
|
||||
@@ -232,8 +230,7 @@ public:
|
||||
|
||||
// Set it again, now using the permanent storage.
|
||||
set_on_new_request_callback(
|
||||
rclcpp::detail::cpp_callback_trampoline<
|
||||
decltype(on_new_request_callback_), const void *, size_t>,
|
||||
rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
|
||||
static_cast<const void *>(&on_new_request_callback_));
|
||||
}
|
||||
|
||||
@@ -310,9 +307,11 @@ public:
|
||||
const std::string & service_name,
|
||||
AnyServiceCallback<ServiceT> any_callback,
|
||||
rcl_service_options_t & service_options)
|
||||
: ServiceBase(node_handle), any_callback_(any_callback),
|
||||
srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle<ServiceT>())
|
||||
: ServiceBase(node_handle), any_callback_(any_callback)
|
||||
{
|
||||
using rosidl_typesupport_cpp::get_service_type_support_handle;
|
||||
auto service_type_support_handle = get_service_type_support_handle<ServiceT>();
|
||||
|
||||
// rcl does the static memory allocation here
|
||||
service_handle_ = std::shared_ptr<rcl_service_t>(
|
||||
new rcl_service_t, [handle = node_handle_, service_name](rcl_service_t * service)
|
||||
@@ -331,7 +330,7 @@ public:
|
||||
rcl_ret_t ret = rcl_service_init(
|
||||
service_handle_.get(),
|
||||
node_handle.get(),
|
||||
srv_type_support_handle_,
|
||||
service_type_support_handle,
|
||||
service_name.c_str(),
|
||||
&service_options);
|
||||
if (ret != RCL_RET_OK) {
|
||||
@@ -371,8 +370,8 @@ public:
|
||||
std::shared_ptr<rcl_node_t> node_handle,
|
||||
std::shared_ptr<rcl_service_t> service_handle,
|
||||
AnyServiceCallback<ServiceT> any_callback)
|
||||
: ServiceBase(node_handle), any_callback_(any_callback),
|
||||
srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle<ServiceT>())
|
||||
: ServiceBase(node_handle),
|
||||
any_callback_(any_callback)
|
||||
{
|
||||
// check if service handle was initialized
|
||||
if (!rcl_service_is_valid(service_handle.get())) {
|
||||
@@ -406,8 +405,8 @@ public:
|
||||
std::shared_ptr<rcl_node_t> node_handle,
|
||||
rcl_service_t * service_handle,
|
||||
AnyServiceCallback<ServiceT> any_callback)
|
||||
: ServiceBase(node_handle), any_callback_(any_callback),
|
||||
srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle<ServiceT>())
|
||||
: ServiceBase(node_handle),
|
||||
any_callback_(any_callback)
|
||||
{
|
||||
// check if service handle was initialized
|
||||
if (!rcl_service_is_valid(service_handle)) {
|
||||
@@ -487,39 +486,10 @@ public:
|
||||
}
|
||||
}
|
||||
|
||||
/// Configure client introspection.
|
||||
/**
|
||||
* \param[in] clock clock to use to generate introspection timestamps
|
||||
* \param[in] qos_service_event_pub QoS settings to use when creating the introspection publisher
|
||||
* \param[in] introspection_state the state to set introspection to
|
||||
*/
|
||||
void
|
||||
configure_introspection(
|
||||
Clock::SharedPtr clock, const QoS & qos_service_event_pub,
|
||||
rcl_service_introspection_state_t introspection_state)
|
||||
{
|
||||
rcl_publisher_options_t pub_opts = rcl_publisher_get_default_options();
|
||||
pub_opts.qos = qos_service_event_pub.get_rmw_qos_profile();
|
||||
|
||||
rcl_ret_t ret = rcl_service_configure_service_introspection(
|
||||
service_handle_.get(),
|
||||
node_handle_.get(),
|
||||
clock->get_clock_handle(),
|
||||
srv_type_support_handle_,
|
||||
pub_opts,
|
||||
introspection_state);
|
||||
|
||||
if (RCL_RET_OK != ret) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(ret, "failed to configure service introspection");
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
RCLCPP_DISABLE_COPY(Service)
|
||||
|
||||
AnyServiceCallback<ServiceT> any_callback_;
|
||||
|
||||
const rosidl_service_type_support_t * srv_type_support_handle_;
|
||||
};
|
||||
|
||||
} // namespace rclcpp
|
||||
|
||||
@@ -17,7 +17,6 @@
|
||||
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
#include <utility>
|
||||
|
||||
#include "rcl/allocator.h"
|
||||
|
||||
@@ -121,8 +120,8 @@ public:
|
||||
}
|
||||
}
|
||||
for (size_t i = 0; i < waitable_handles_.size(); ++i) {
|
||||
if (waitable_handles_[i]->is_ready(wait_set)) {
|
||||
waitable_triggered_handles_.emplace_back(std::move(waitable_handles_[i]));
|
||||
if (!waitable_handles_[i]->is_ready(wait_set)) {
|
||||
waitable_handles_[i].reset();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -146,7 +145,10 @@ public:
|
||||
timer_handles_.end()
|
||||
);
|
||||
|
||||
waitable_handles_.clear();
|
||||
waitable_handles_.erase(
|
||||
std::remove(waitable_handles_.begin(), waitable_handles_.end(), nullptr),
|
||||
waitable_handles_.end()
|
||||
);
|
||||
}
|
||||
|
||||
bool collect_entities(const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override
|
||||
@@ -390,9 +392,8 @@ public:
|
||||
rclcpp::AnyExecutable & any_exec,
|
||||
const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override
|
||||
{
|
||||
auto & waitable_handles = waitable_triggered_handles_;
|
||||
auto it = waitable_handles.begin();
|
||||
while (it != waitable_handles.end()) {
|
||||
auto it = waitable_handles_.begin();
|
||||
while (it != waitable_handles_.end()) {
|
||||
std::shared_ptr<Waitable> & waitable = *it;
|
||||
if (waitable) {
|
||||
// Find the group for this handle and see if it can be serviced
|
||||
@@ -400,7 +401,7 @@ public:
|
||||
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);
|
||||
it = waitable_handles_.erase(it);
|
||||
continue;
|
||||
}
|
||||
if (!group->can_be_taken_from().load()) {
|
||||
@@ -413,11 +414,11 @@ public:
|
||||
any_exec.waitable = waitable;
|
||||
any_exec.callback_group = group;
|
||||
any_exec.node_base = get_node_by_group(group, weak_groups_to_nodes);
|
||||
waitable_handles.erase(it);
|
||||
waitable_handles_.erase(it);
|
||||
return;
|
||||
}
|
||||
// Else, the waitable is no longer valid, remove it and continue
|
||||
it = waitable_handles.erase(it);
|
||||
it = waitable_handles_.erase(it);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -498,8 +499,6 @@ private:
|
||||
VectorRebind<std::shared_ptr<const rcl_timer_t>> timer_handles_;
|
||||
VectorRebind<std::shared_ptr<Waitable>> waitable_handles_;
|
||||
|
||||
VectorRebind<std::shared_ptr<Waitable>> waitable_triggered_handles_;
|
||||
|
||||
std::shared_ptr<VoidAlloc> allocator_;
|
||||
};
|
||||
|
||||
|
||||
@@ -140,15 +140,44 @@ public:
|
||||
node_base,
|
||||
type_support_handle,
|
||||
topic_name,
|
||||
options.to_rcl_subscription_options(qos),
|
||||
// NOTE(methylDragon): Passing these args separately is necessary for event binding
|
||||
options.event_callbacks,
|
||||
options.use_default_callbacks,
|
||||
callback.is_serialized_message_callback() ? DeliveredMessageKind::SERIALIZED_MESSAGE : DeliveredMessageKind::ROS_MESSAGE), // NOLINT
|
||||
options.template to_rcl_subscription_options<ROSMessageType>(qos),
|
||||
callback.is_serialized_message_callback()),
|
||||
any_callback_(callback),
|
||||
options_(options),
|
||||
message_memory_strategy_(message_memory_strategy)
|
||||
{
|
||||
if (options_.event_callbacks.deadline_callback) {
|
||||
this->add_event_handler(
|
||||
options_.event_callbacks.deadline_callback,
|
||||
RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED);
|
||||
}
|
||||
if (options_.event_callbacks.liveliness_callback) {
|
||||
this->add_event_handler(
|
||||
options_.event_callbacks.liveliness_callback,
|
||||
RCL_SUBSCRIPTION_LIVELINESS_CHANGED);
|
||||
}
|
||||
if (options_.event_callbacks.incompatible_qos_callback) {
|
||||
this->add_event_handler(
|
||||
options_.event_callbacks.incompatible_qos_callback,
|
||||
RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS);
|
||||
} else if (options_.use_default_callbacks) {
|
||||
// Register default callback when not specified
|
||||
try {
|
||||
this->add_event_handler(
|
||||
[this](QOSRequestedIncompatibleQoSInfo & info) {
|
||||
this->default_incompatible_qos_callback(info);
|
||||
},
|
||||
RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS);
|
||||
} catch (UnsupportedEventTypeException & /*exc*/) {
|
||||
// pass
|
||||
}
|
||||
}
|
||||
if (options_.event_callbacks.message_lost_callback) {
|
||||
this->add_event_handler(
|
||||
options_.event_callbacks.message_lost_callback,
|
||||
RCL_SUBSCRIPTION_MESSAGE_LOST);
|
||||
}
|
||||
|
||||
// Setup intra process publishing if requested.
|
||||
if (rclcpp::detail::resolve_use_intra_process(options_, *node_base)) {
|
||||
using rclcpp::detail::resolve_intra_process_buffer_type;
|
||||
@@ -388,57 +417,6 @@ public:
|
||||
return any_callback_.use_take_shared_method();
|
||||
}
|
||||
|
||||
// DYNAMIC TYPE ==================================================================================
|
||||
// TODO(methylDragon): Reorder later
|
||||
// TODO(methylDragon): Implement later...
|
||||
rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr
|
||||
get_shared_dynamic_message_type() override
|
||||
{
|
||||
throw rclcpp::exceptions::UnimplementedError(
|
||||
"get_shared_dynamic_message_type is not implemented for Subscription");
|
||||
}
|
||||
|
||||
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr
|
||||
get_shared_dynamic_message() override
|
||||
{
|
||||
throw rclcpp::exceptions::UnimplementedError(
|
||||
"get_shared_dynamic_message is not implemented for Subscription");
|
||||
}
|
||||
|
||||
rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr
|
||||
get_shared_dynamic_serialization_support() override
|
||||
{
|
||||
throw rclcpp::exceptions::UnimplementedError(
|
||||
"get_shared_dynamic_serialization_support is not implemented for Subscription");
|
||||
}
|
||||
|
||||
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr
|
||||
create_dynamic_message() override
|
||||
{
|
||||
throw rclcpp::exceptions::UnimplementedError(
|
||||
"create_dynamic_message is not implemented for Subscription");
|
||||
}
|
||||
|
||||
void
|
||||
return_dynamic_message(
|
||||
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message) override
|
||||
{
|
||||
(void) message;
|
||||
throw rclcpp::exceptions::UnimplementedError(
|
||||
"return_dynamic_message is not implemented for Subscription");
|
||||
}
|
||||
|
||||
void
|
||||
handle_dynamic_message(
|
||||
const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message,
|
||||
const rclcpp::MessageInfo & message_info) override
|
||||
{
|
||||
(void) message;
|
||||
(void) message_info;
|
||||
throw rclcpp::exceptions::UnimplementedError(
|
||||
"handle_dynamic_message is not implemented for Subscription");
|
||||
}
|
||||
|
||||
private:
|
||||
RCLCPP_DISABLE_COPY(Subscription)
|
||||
|
||||
|
||||
@@ -31,16 +31,13 @@
|
||||
|
||||
#include "rclcpp/any_subscription_callback.hpp"
|
||||
#include "rclcpp/detail/cpp_callback_trampoline.hpp"
|
||||
#include "rclcpp/dynamic_typesupport/dynamic_message.hpp"
|
||||
#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp"
|
||||
#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
|
||||
#include "rclcpp/experimental/intra_process_manager.hpp"
|
||||
#include "rclcpp/experimental/subscription_intra_process_base.hpp"
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/message_info.hpp"
|
||||
#include "rclcpp/network_flow_endpoint.hpp"
|
||||
#include "rclcpp/qos.hpp"
|
||||
#include "rclcpp/event_handler.hpp"
|
||||
#include "rclcpp/qos_event.hpp"
|
||||
#include "rclcpp/serialized_message.hpp"
|
||||
#include "rclcpp/subscription_content_filter_options.hpp"
|
||||
#include "rclcpp/type_support_decl.hpp"
|
||||
@@ -63,27 +60,6 @@ namespace experimental
|
||||
class IntraProcessManager;
|
||||
} // namespace experimental
|
||||
|
||||
/// The kind of message that the subscription delivers in its callback, used by the executor
|
||||
/**
|
||||
* This enum needs to exist because the callback handle is not accessible to the executor's scope.
|
||||
*
|
||||
* "Kind" is used since what is being delivered is a category of messages, for example, there are
|
||||
* different ROS message types that can be delivered, but they're all ROS messages.
|
||||
*
|
||||
* As a concrete example, all of the following callbacks will be considered ROS_MESSAGE for
|
||||
* DeliveredMessageKind:
|
||||
* - void callback(const std_msgs::msg::String &)
|
||||
* - void callback(const std::string &) // type adaption
|
||||
* - void callback(std::unique_ptr<std_msgs::msg::String>)
|
||||
*/
|
||||
enum class DeliveredMessageKind : uint8_t
|
||||
{
|
||||
INVALID = 0,
|
||||
ROS_MESSAGE = 1, // The subscription delivers a ROS message to its callback
|
||||
SERIALIZED_MESSAGE = 2, // The subscription delivers a serialized message to its callback
|
||||
DYNAMIC_MESSAGE = 3, // The subscription delivers a dynamic message to its callback
|
||||
};
|
||||
|
||||
/// Virtual base class for subscriptions. This pattern allows us to iterate over different template
|
||||
/// specializations of Subscription, among other things.
|
||||
class SubscriptionBase : public std::enable_shared_from_this<SubscriptionBase>
|
||||
@@ -100,8 +76,7 @@ public:
|
||||
* \param[in] type_support_handle rosidl type support struct, for the Message type of the topic.
|
||||
* \param[in] topic_name Name of the topic to subscribe to.
|
||||
* \param[in] subscription_options Options for the subscription.
|
||||
* \param[in] delivered_message_kind Enum flag to change how the message will be received and
|
||||
* delivered
|
||||
* \param[in] is_serialized is true if the message will be delivered still serialized
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
SubscriptionBase(
|
||||
@@ -109,20 +84,12 @@ public:
|
||||
const rosidl_message_type_support_t & type_support_handle,
|
||||
const std::string & topic_name,
|
||||
const rcl_subscription_options_t & subscription_options,
|
||||
const SubscriptionEventCallbacks & event_callbacks,
|
||||
bool use_default_callbacks,
|
||||
DeliveredMessageKind delivered_message_kind = DeliveredMessageKind::ROS_MESSAGE);
|
||||
bool is_serialized = false);
|
||||
|
||||
/// Destructor.
|
||||
RCLCPP_PUBLIC
|
||||
virtual ~SubscriptionBase();
|
||||
|
||||
/// Add event handlers for passed in event_callbacks.
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
bind_event_callbacks(
|
||||
const SubscriptionEventCallbacks & event_callbacks, bool use_default_callbacks);
|
||||
|
||||
/// Get the topic that this subscription is subscribed on.
|
||||
RCLCPP_PUBLIC
|
||||
const char *
|
||||
@@ -140,7 +107,7 @@ public:
|
||||
/** \return The map of QoS event handlers. */
|
||||
RCLCPP_PUBLIC
|
||||
const
|
||||
std::unordered_map<rcl_subscription_event_type_t, std::shared_ptr<rclcpp::EventHandlerBase>> &
|
||||
std::unordered_map<rcl_subscription_event_type_t, std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
|
||||
get_event_handlers() const;
|
||||
|
||||
/// Get the actual QoS settings, after the defaults have been determined.
|
||||
@@ -260,14 +227,6 @@ public:
|
||||
bool
|
||||
is_serialized() const;
|
||||
|
||||
/// Return the type of the subscription.
|
||||
/**
|
||||
* \return `DeliveredMessageKind`, which adjusts how messages are received and delivered.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
DeliveredMessageKind
|
||||
get_subscription_type() const;
|
||||
|
||||
/// Get matching publisher count.
|
||||
/** \return The number of publishers on this topic. */
|
||||
RCLCPP_PUBLIC
|
||||
@@ -389,7 +348,7 @@ public:
|
||||
// This two-step setting, prevents a gap where the old std::function has
|
||||
// been replaced but the middleware hasn't been told about the new one yet.
|
||||
set_on_new_message_callback(
|
||||
rclcpp::detail::cpp_callback_trampoline<decltype(new_callback), const void *, size_t>,
|
||||
rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
|
||||
static_cast<const void *>(&new_callback));
|
||||
|
||||
// Store the std::function to keep it in scope, also overwrites the existing one.
|
||||
@@ -397,8 +356,7 @@ public:
|
||||
|
||||
// Set it again, now using the permanent storage.
|
||||
set_on_new_message_callback(
|
||||
rclcpp::detail::cpp_callback_trampoline<
|
||||
decltype(on_new_message_callback_), const void *, size_t>,
|
||||
rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
|
||||
static_cast<const void *>(&on_new_message_callback_));
|
||||
}
|
||||
|
||||
@@ -490,7 +448,7 @@ public:
|
||||
* If you want more information available in the callback, like the qos event
|
||||
* or other information, you may use a lambda with captures or std::bind.
|
||||
*
|
||||
* \sa rclcpp::EventHandlerBase::set_on_ready_callback
|
||||
* \sa rclcpp::QOSEventHandlerBase::set_on_ready_callback
|
||||
*
|
||||
* \param[in] callback functor to be called when a new event occurs
|
||||
* \param[in] event_type identifier for the qos event we want to attach the callback to
|
||||
@@ -568,49 +526,6 @@ public:
|
||||
rclcpp::ContentFilterOptions
|
||||
get_content_filter() const;
|
||||
|
||||
// DYNAMIC TYPE ==================================================================================
|
||||
// TODO(methylDragon): Reorder later
|
||||
RCLCPP_PUBLIC
|
||||
virtual
|
||||
rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr
|
||||
get_shared_dynamic_message_type() = 0;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual
|
||||
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr
|
||||
get_shared_dynamic_message() = 0;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual
|
||||
rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr
|
||||
get_shared_dynamic_serialization_support() = 0;
|
||||
|
||||
/// Borrow a new serialized message (this clones!)
|
||||
/** \return Shared pointer to a rclcpp::dynamic_typesupport::DynamicMessage. */
|
||||
RCLCPP_PUBLIC
|
||||
virtual
|
||||
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr
|
||||
create_dynamic_message() = 0;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual
|
||||
void
|
||||
return_dynamic_message(rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message) = 0;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual
|
||||
void
|
||||
handle_dynamic_message(
|
||||
const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message,
|
||||
const rclcpp::MessageInfo & message_info) = 0;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
take_dynamic_message(
|
||||
rclcpp::dynamic_typesupport::DynamicMessage & message_out,
|
||||
rclcpp::MessageInfo & message_info_out);
|
||||
// ===============================================================================================
|
||||
|
||||
protected:
|
||||
template<typename EventCallbackT>
|
||||
void
|
||||
@@ -618,7 +533,7 @@ protected:
|
||||
const EventCallbackT & callback,
|
||||
const rcl_subscription_event_type_t event_type)
|
||||
{
|
||||
auto handler = std::make_shared<EventHandler<EventCallbackT,
|
||||
auto handler = std::make_shared<QOSEventHandler<EventCallbackT,
|
||||
std::shared_ptr<rcl_subscription_t>>>(
|
||||
callback,
|
||||
rcl_subscription_event_init,
|
||||
@@ -631,9 +546,6 @@ protected:
|
||||
RCLCPP_PUBLIC
|
||||
void default_incompatible_qos_callback(QOSRequestedIncompatibleQoSInfo & info) const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void default_incompatible_type_callback(IncompatibleTypeInfo & info) const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
matches_any_intra_process_publishers(const rmw_gid_t * sender_gid) const;
|
||||
@@ -650,24 +562,22 @@ protected:
|
||||
rclcpp::Logger node_logger_;
|
||||
|
||||
std::unordered_map<rcl_subscription_event_type_t,
|
||||
std::shared_ptr<rclcpp::EventHandlerBase>> event_handlers_;
|
||||
std::shared_ptr<rclcpp::QOSEventHandlerBase>> event_handlers_;
|
||||
|
||||
bool use_intra_process_;
|
||||
IntraProcessManagerWeakPtr weak_ipm_;
|
||||
uint64_t intra_process_subscription_id_;
|
||||
std::shared_ptr<rclcpp::experimental::SubscriptionIntraProcessBase> subscription_intra_process_;
|
||||
|
||||
const SubscriptionEventCallbacks event_callbacks_;
|
||||
|
||||
private:
|
||||
RCLCPP_DISABLE_COPY(SubscriptionBase)
|
||||
|
||||
rosidl_message_type_support_t type_support_;
|
||||
DeliveredMessageKind delivered_message_type_;
|
||||
bool is_serialized_;
|
||||
|
||||
std::atomic<bool> subscription_in_use_by_wait_set_{false};
|
||||
std::atomic<bool> intra_process_subscription_waitable_in_use_by_wait_set_{false};
|
||||
std::unordered_map<rclcpp::EventHandlerBase *,
|
||||
std::unordered_map<rclcpp::QOSEventHandlerBase *,
|
||||
std::atomic<bool>> qos_events_in_use_by_wait_set_;
|
||||
|
||||
std::recursive_mutex callback_mutex_;
|
||||
|
||||
@@ -26,7 +26,7 @@
|
||||
#include "rclcpp/intra_process_buffer_type.hpp"
|
||||
#include "rclcpp/intra_process_setting.hpp"
|
||||
#include "rclcpp/qos.hpp"
|
||||
#include "rclcpp/event_handler.hpp"
|
||||
#include "rclcpp/qos_event.hpp"
|
||||
#include "rclcpp/qos_overriding_options.hpp"
|
||||
#include "rclcpp/subscription_content_filter_options.hpp"
|
||||
#include "rclcpp/topic_statistics_state.hpp"
|
||||
@@ -110,6 +110,7 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
|
||||
* \param qos QoS profile for subcription.
|
||||
* \return rcl_subscription_options_t structure based on the rclcpp::QoS
|
||||
*/
|
||||
template<typename MessageT>
|
||||
rcl_subscription_options_t
|
||||
to_rcl_subscription_options(const rclcpp::QoS & qos) const
|
||||
{
|
||||
|
||||
@@ -149,48 +149,11 @@ public:
|
||||
bool
|
||||
exchange_in_use_by_wait_set_state(bool in_use_state);
|
||||
|
||||
/// Set a callback to be called when the timer is reset
|
||||
/**
|
||||
* You should aim to make this callback fast and not blocking.
|
||||
* If you need to do a lot of work or wait for some other event, you should
|
||||
* spin it off to another thread.
|
||||
*
|
||||
* Calling it again will override any previously set callback.
|
||||
* An exception will be thrown if the callback is not callable.
|
||||
*
|
||||
* This function is thread-safe.
|
||||
*
|
||||
* If you want more information available in the callback,
|
||||
* you may use a lambda with captures or std::bind.
|
||||
*
|
||||
* \param[in] callback functor to be called whenever timer is reset
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
set_on_reset_callback(std::function<void(size_t)> callback);
|
||||
|
||||
/// Unset the callback registered for reset timer
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
clear_on_reset_callback();
|
||||
|
||||
protected:
|
||||
std::recursive_mutex callback_mutex_;
|
||||
// Declare callback before timer_handle_, so on destruction
|
||||
// the callback is destroyed last. Otherwise, the rcl timer
|
||||
// callback would point briefly to a destroyed function.
|
||||
// Clearing the callback on timer destructor also makes sure
|
||||
// the rcl callback is cleared before on_reset_callback_.
|
||||
std::function<void(size_t)> on_reset_callback_{nullptr};
|
||||
|
||||
Clock::SharedPtr clock_;
|
||||
std::shared_ptr<rcl_timer_t> timer_handle_;
|
||||
|
||||
std::atomic<bool> in_use_by_wait_set_{false};
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
set_on_reset_callback(rcl_event_callback_t callback, const void * user_data);
|
||||
};
|
||||
|
||||
|
||||
@@ -227,16 +190,10 @@ public:
|
||||
rclcpp_timer_callback_added,
|
||||
static_cast<const void *>(get_timer_handle().get()),
|
||||
reinterpret_cast<const void *>(&callback_));
|
||||
#ifndef TRACETOOLS_DISABLED
|
||||
if (TRACEPOINT_ENABLED(rclcpp_callback_register)) {
|
||||
char * symbol = tracetools::get_symbol(callback_);
|
||||
DO_TRACEPOINT(
|
||||
rclcpp_callback_register,
|
||||
reinterpret_cast<const void *>(&callback_),
|
||||
symbol);
|
||||
std::free(symbol);
|
||||
}
|
||||
#endif
|
||||
TRACEPOINT(
|
||||
rclcpp_callback_register,
|
||||
reinterpret_cast<const void *>(&callback_),
|
||||
tracetools::get_symbol(callback_));
|
||||
}
|
||||
|
||||
/// Default destructor.
|
||||
|
||||
@@ -104,7 +104,7 @@ protected:
|
||||
// TODO(wjwwood): support custom allocator, maybe restrict to polymorphic allocator
|
||||
rcl_get_default_allocator());
|
||||
if (RCL_RET_OK != ret) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to create wait set");
|
||||
rclcpp::exceptions::throw_from_rcl_error(ret);
|
||||
}
|
||||
|
||||
// (Re)build the wait set for the first time.
|
||||
@@ -192,7 +192,8 @@ protected:
|
||||
size_t services_from_waitables = 0;
|
||||
size_t events_from_waitables = 0;
|
||||
for (const auto & waitable_entry : waitables) {
|
||||
if (!waitable_entry.waitable) {
|
||||
auto waitable_ptr_pair = get_raw_pointer_from_smart_pointer(waitable_entry.waitable);
|
||||
if (nullptr == waitable_ptr_pair.second) {
|
||||
// In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
|
||||
if (HasStrongOwnership) {
|
||||
// This will not happen in fixed sized storage, as it holds
|
||||
@@ -203,13 +204,13 @@ protected:
|
||||
needs_pruning_ = true;
|
||||
continue;
|
||||
}
|
||||
const auto & waitable = waitable_entry.waitable;
|
||||
subscriptions_from_waitables += waitable->get_number_of_ready_subscriptions();
|
||||
guard_conditions_from_waitables += waitable->get_number_of_ready_guard_conditions();
|
||||
timers_from_waitables += waitable->get_number_of_ready_timers();
|
||||
clients_from_waitables += waitable->get_number_of_ready_clients();
|
||||
services_from_waitables += waitable->get_number_of_ready_services();
|
||||
events_from_waitables += waitable->get_number_of_ready_events();
|
||||
rclcpp::Waitable & waitable = *waitable_ptr_pair.second;
|
||||
subscriptions_from_waitables += waitable.get_number_of_ready_subscriptions();
|
||||
guard_conditions_from_waitables += waitable.get_number_of_ready_guard_conditions();
|
||||
timers_from_waitables += waitable.get_number_of_ready_timers();
|
||||
clients_from_waitables += waitable.get_number_of_ready_clients();
|
||||
services_from_waitables += waitable.get_number_of_ready_services();
|
||||
events_from_waitables += waitable.get_number_of_ready_events();
|
||||
}
|
||||
rcl_ret_t ret = rcl_wait_set_resize(
|
||||
&rcl_wait_set_,
|
||||
@@ -221,7 +222,7 @@ protected:
|
||||
events_from_waitables
|
||||
);
|
||||
if (RCL_RET_OK != ret) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't resize the wait set");
|
||||
rclcpp::exceptions::throw_from_rcl_error(ret);
|
||||
}
|
||||
was_resized = true;
|
||||
// Assumption: the calling code ensures this function is not called
|
||||
@@ -237,13 +238,15 @@ protected:
|
||||
if (!was_resized) {
|
||||
rcl_ret_t ret = rcl_wait_set_clear(&rcl_wait_set_);
|
||||
if (RCL_RET_OK != ret) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't clear the wait set");
|
||||
rclcpp::exceptions::throw_from_rcl_error(ret);
|
||||
}
|
||||
}
|
||||
|
||||
// Add subscriptions.
|
||||
for (const auto & subscription_entry : subscriptions) {
|
||||
if (!subscription_entry.subscription) {
|
||||
auto subscription_ptr_pair =
|
||||
get_raw_pointer_from_smart_pointer(subscription_entry.subscription);
|
||||
if (nullptr == subscription_ptr_pair.second) {
|
||||
// In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
|
||||
if (HasStrongOwnership) {
|
||||
// This will not happen in fixed sized storage, as it holds
|
||||
@@ -254,13 +257,12 @@ protected:
|
||||
needs_pruning_ = true;
|
||||
continue;
|
||||
}
|
||||
|
||||
rcl_ret_t ret = rcl_wait_set_add_subscription(
|
||||
&rcl_wait_set_,
|
||||
subscription_entry.subscription->get_subscription_handle().get(),
|
||||
subscription_ptr_pair.second->get_subscription_handle().get(),
|
||||
nullptr);
|
||||
if (RCL_RET_OK != ret) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't fill wait set");
|
||||
rclcpp::exceptions::throw_from_rcl_error(ret);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -269,7 +271,8 @@ protected:
|
||||
[this](const auto & inner_guard_conditions)
|
||||
{
|
||||
for (const auto & guard_condition : inner_guard_conditions) {
|
||||
if (!guard_condition) {
|
||||
auto guard_condition_ptr_pair = get_raw_pointer_from_smart_pointer(guard_condition);
|
||||
if (nullptr == guard_condition_ptr_pair.second) {
|
||||
// In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
|
||||
if (HasStrongOwnership) {
|
||||
// This will not happen in fixed sized storage, as it holds
|
||||
@@ -282,10 +285,10 @@ protected:
|
||||
}
|
||||
rcl_ret_t ret = rcl_wait_set_add_guard_condition(
|
||||
&rcl_wait_set_,
|
||||
&guard_condition->get_rcl_guard_condition(),
|
||||
&guard_condition_ptr_pair.second->get_rcl_guard_condition(),
|
||||
nullptr);
|
||||
if (RCL_RET_OK != ret) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't fill wait set");
|
||||
rclcpp::exceptions::throw_from_rcl_error(ret);
|
||||
}
|
||||
}
|
||||
};
|
||||
@@ -298,7 +301,8 @@ protected:
|
||||
|
||||
// Add timers.
|
||||
for (const auto & timer : timers) {
|
||||
if (!timer) {
|
||||
auto timer_ptr_pair = get_raw_pointer_from_smart_pointer(timer);
|
||||
if (nullptr == timer_ptr_pair.second) {
|
||||
// In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
|
||||
if (HasStrongOwnership) {
|
||||
// This will not happen in fixed sized storage, as it holds
|
||||
@@ -311,16 +315,17 @@ protected:
|
||||
}
|
||||
rcl_ret_t ret = rcl_wait_set_add_timer(
|
||||
&rcl_wait_set_,
|
||||
timer->get_timer_handle().get(),
|
||||
timer_ptr_pair.second->get_timer_handle().get(),
|
||||
nullptr);
|
||||
if (RCL_RET_OK != ret) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't fill wait set");
|
||||
rclcpp::exceptions::throw_from_rcl_error(ret);
|
||||
}
|
||||
}
|
||||
|
||||
// Add clients.
|
||||
for (const auto & client : clients) {
|
||||
if (!client) {
|
||||
auto client_ptr_pair = get_raw_pointer_from_smart_pointer(client);
|
||||
if (nullptr == client_ptr_pair.second) {
|
||||
// In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
|
||||
if (HasStrongOwnership) {
|
||||
// This will not happen in fixed sized storage, as it holds
|
||||
@@ -333,17 +338,17 @@ protected:
|
||||
}
|
||||
rcl_ret_t ret = rcl_wait_set_add_client(
|
||||
&rcl_wait_set_,
|
||||
client->get_client_handle().get(),
|
||||
client_ptr_pair.second->get_client_handle().get(),
|
||||
nullptr);
|
||||
if (RCL_RET_OK != ret) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(ret);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Add services.
|
||||
for (const auto & service : services) {
|
||||
if (!service) {
|
||||
auto service_ptr_pair = get_raw_pointer_from_smart_pointer(service);
|
||||
if (nullptr == service_ptr_pair.second) {
|
||||
// In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
|
||||
if (HasStrongOwnership) {
|
||||
// This will not happen in fixed sized storage, as it holds
|
||||
@@ -356,16 +361,17 @@ protected:
|
||||
}
|
||||
rcl_ret_t ret = rcl_wait_set_add_service(
|
||||
&rcl_wait_set_,
|
||||
service->get_service_handle().get(),
|
||||
service_ptr_pair.second->get_service_handle().get(),
|
||||
nullptr);
|
||||
if (RCL_RET_OK != ret) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't fill wait set");
|
||||
rclcpp::exceptions::throw_from_rcl_error(ret);
|
||||
}
|
||||
}
|
||||
|
||||
// Add waitables.
|
||||
for (auto & waitable_entry : waitables) {
|
||||
if (!waitable_entry.waitable) {
|
||||
auto waitable_ptr_pair = get_raw_pointer_from_smart_pointer(waitable_entry.waitable);
|
||||
if (nullptr == waitable_ptr_pair.second) {
|
||||
// In this case it was probably stored as a weak_ptr, but is now locking to nullptr.
|
||||
if (HasStrongOwnership) {
|
||||
// This will not happen in fixed sized storage, as it holds
|
||||
@@ -376,7 +382,8 @@ protected:
|
||||
needs_pruning_ = true;
|
||||
continue;
|
||||
}
|
||||
waitable_entry.waitable->add_to_wait_set(&rcl_wait_set_);
|
||||
rclcpp::Waitable & waitable = *waitable_ptr_pair.second;
|
||||
waitable.add_to_wait_set(&rcl_wait_set_);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -204,19 +204,15 @@ public:
|
||||
void
|
||||
storage_rebuild_rcl_wait_set(const ArrayOfExtraGuardConditions & extra_guard_conditions)
|
||||
{
|
||||
this->storage_acquire_ownerships();
|
||||
|
||||
this->storage_rebuild_rcl_wait_set_with_sets(
|
||||
shared_subscriptions_,
|
||||
shared_guard_conditions_,
|
||||
subscriptions_,
|
||||
guard_conditions_,
|
||||
extra_guard_conditions,
|
||||
shared_timers_,
|
||||
shared_clients_,
|
||||
shared_services_,
|
||||
shared_waitables_
|
||||
timers_,
|
||||
clients_,
|
||||
services_,
|
||||
waitables_
|
||||
);
|
||||
|
||||
this->storage_release_ownerships();
|
||||
}
|
||||
|
||||
template<class EntityT, class SequenceOfEntitiesT>
|
||||
@@ -411,7 +407,6 @@ public:
|
||||
}
|
||||
};
|
||||
// Lock all the weak pointers and hold them until released.
|
||||
lock_all(subscriptions_, shared_subscriptions_);
|
||||
lock_all(guard_conditions_, shared_guard_conditions_);
|
||||
lock_all(timers_, shared_timers_);
|
||||
lock_all(clients_, shared_clients_);
|
||||
@@ -443,7 +438,6 @@ public:
|
||||
shared_ptr.reset();
|
||||
}
|
||||
};
|
||||
reset_all(shared_subscriptions_);
|
||||
reset_all(shared_guard_conditions_);
|
||||
reset_all(shared_timers_);
|
||||
reset_all(shared_clients_);
|
||||
|
||||
@@ -290,7 +290,7 @@ protected:
|
||||
return create_wait_result(WaitResultKind::Empty);
|
||||
} else {
|
||||
// Some other error case, throw.
|
||||
rclcpp::exceptions::throw_from_rcl_error(ret, "rcl_wait() failed");
|
||||
rclcpp::exceptions::throw_from_rcl_error(ret);
|
||||
}
|
||||
} while (should_loop());
|
||||
|
||||
|
||||
@@ -516,7 +516,7 @@ public:
|
||||
* the waitable to be removed, but it will cause the associated entity pointer
|
||||
* to be nullptr when introspecting this waitable after waiting.
|
||||
*
|
||||
* Note that rclcpp::EventHandlerBase is just a special case of
|
||||
* Note that rclcpp::QOSEventHandlerBase are just a special case of
|
||||
* rclcpp::Waitable and can be added with this function.
|
||||
*
|
||||
* \param[in] waitable Waitable to be added.
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="2">
|
||||
<name>rclcpp</name>
|
||||
<version>20.0.0</version>
|
||||
<version>17.1.0</version>
|
||||
<description>The ROS client library in C++.</description>
|
||||
|
||||
<maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
|
||||
@@ -39,7 +39,6 @@
|
||||
<depend>rcpputils</depend>
|
||||
<depend>rcutils</depend>
|
||||
<depend>rmw</depend>
|
||||
<depend>rosidl_dynamic_typesupport</depend>
|
||||
<depend>statistics_msgs</depend>
|
||||
<depend>tracetools</depend>
|
||||
|
||||
|
||||
@@ -124,7 +124,7 @@ def get_rclcpp_suffix_from_features(features):
|
||||
) \
|
||||
do { \
|
||||
static_assert( \
|
||||
::std::is_same<typename std::remove_cv_t<typename std::remove_reference_t<decltype(logger)>>, \
|
||||
::std::is_same<typename std::remove_cv<typename std::remove_reference<decltype(logger)>::type>::type, \
|
||||
typename ::rclcpp::Logger>::value, \
|
||||
"First argument to logging macros must be an rclcpp::Logger"); \
|
||||
@[ if 'throttle' in feature_combination]@ \
|
||||
|
||||
@@ -16,16 +16,13 @@
|
||||
|
||||
using rclcpp::AnyExecutable;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
AnyExecutable::AnyExecutable()
|
||||
: subscription(nullptr),
|
||||
timer(nullptr),
|
||||
service(nullptr),
|
||||
client(nullptr),
|
||||
waitable(nullptr),
|
||||
callback_group(nullptr),
|
||||
node_base(nullptr),
|
||||
data(nullptr)
|
||||
node_base(nullptr)
|
||||
{}
|
||||
|
||||
AnyExecutable::~AnyExecutable()
|
||||
|
||||
@@ -31,12 +31,10 @@ using rclcpp::CallbackGroupType;
|
||||
|
||||
CallbackGroup::CallbackGroup(
|
||||
CallbackGroupType group_type,
|
||||
std::function<rclcpp::Context::SharedPtr(void)> get_context,
|
||||
bool automatically_add_to_executor_with_node)
|
||||
: type_(group_type), associated_with_executor_(false),
|
||||
can_be_taken_from_(true),
|
||||
automatically_add_to_executor_with_node_(automatically_add_to_executor_with_node),
|
||||
get_context_(get_context)
|
||||
automatically_add_to_executor_with_node_(automatically_add_to_executor_with_node)
|
||||
{}
|
||||
|
||||
CallbackGroup::~CallbackGroup()
|
||||
@@ -56,17 +54,6 @@ CallbackGroup::type() const
|
||||
return type_;
|
||||
}
|
||||
|
||||
size_t
|
||||
CallbackGroup::size() const
|
||||
{
|
||||
return
|
||||
subscription_ptrs_.size() +
|
||||
service_ptrs_.size() +
|
||||
client_ptrs_.size() +
|
||||
timer_ptrs_.size() +
|
||||
waitable_ptrs_.size();
|
||||
}
|
||||
|
||||
void CallbackGroup::collect_all_ptrs(
|
||||
std::function<void(const rclcpp::SubscriptionBase::SharedPtr &)> sub_func,
|
||||
std::function<void(const rclcpp::ServiceBase::SharedPtr &)> service_func,
|
||||
@@ -124,7 +111,6 @@ CallbackGroup::automatically_add_to_executor_with_node() const
|
||||
return automatically_add_to_executor_with_node_;
|
||||
}
|
||||
|
||||
// \TODO(mjcarroll) Deprecated, remove on tock
|
||||
rclcpp::GuardCondition::SharedPtr
|
||||
CallbackGroup::get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr)
|
||||
{
|
||||
@@ -143,29 +129,6 @@ CallbackGroup::get_notify_guard_condition(const rclcpp::Context::SharedPtr conte
|
||||
return notify_guard_condition_;
|
||||
}
|
||||
|
||||
rclcpp::GuardCondition::SharedPtr
|
||||
CallbackGroup::get_notify_guard_condition()
|
||||
{
|
||||
std::lock_guard<std::recursive_mutex> lock(notify_guard_condition_mutex_);
|
||||
if (!this->get_context_) {
|
||||
throw std::runtime_error("Callback group was created without context and not passed context");
|
||||
}
|
||||
auto context_ptr = this->get_context_();
|
||||
if (context_ptr && context_ptr->is_valid()) {
|
||||
if (notify_guard_condition_ && context_ptr != notify_guard_condition_->get_context()) {
|
||||
if (associated_with_executor_) {
|
||||
trigger_notify_guard_condition();
|
||||
}
|
||||
notify_guard_condition_ = nullptr;
|
||||
}
|
||||
if (!notify_guard_condition_) {
|
||||
notify_guard_condition_ = std::make_shared<rclcpp::GuardCondition>(context_ptr);
|
||||
}
|
||||
return notify_guard_condition_;
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
void
|
||||
CallbackGroup::trigger_notify_guard_condition()
|
||||
{
|
||||
|
||||
@@ -23,11 +23,9 @@
|
||||
#include "rcl/graph.h"
|
||||
#include "rcl/node.h"
|
||||
#include "rcl/wait.h"
|
||||
|
||||
#include "rclcpp/exceptions.hpp"
|
||||
#include "rclcpp/node_interfaces/node_base_interface.hpp"
|
||||
#include "rclcpp/node_interfaces/node_graph_interface.hpp"
|
||||
#include "rclcpp/qos.hpp"
|
||||
#include "rclcpp/utilities.hpp"
|
||||
#include "rclcpp/logging.hpp"
|
||||
|
||||
@@ -243,6 +241,7 @@ ClientBase::set_on_new_response_callback(rcl_event_callback_t callback, const vo
|
||||
user_data);
|
||||
|
||||
if (RCL_RET_OK != ret) {
|
||||
using rclcpp::exceptions::throw_from_rcl_error;
|
||||
throw_from_rcl_error(ret, "failed to set the on new response callback for client");
|
||||
}
|
||||
}
|
||||
|
||||
@@ -182,71 +182,6 @@ Clock::sleep_for(Duration rel_time, Context::SharedPtr context)
|
||||
return sleep_until(now() + rel_time, context);
|
||||
}
|
||||
|
||||
bool
|
||||
Clock::started()
|
||||
{
|
||||
if (!rcl_clock_valid(get_clock_handle())) {
|
||||
throw std::runtime_error("clock is not rcl_clock_valid");
|
||||
}
|
||||
return rcl_clock_time_started(get_clock_handle());
|
||||
}
|
||||
|
||||
bool
|
||||
Clock::wait_until_started(Context::SharedPtr context)
|
||||
{
|
||||
if (!context || !context->is_valid()) {
|
||||
throw std::runtime_error("context cannot be slept with because it's invalid");
|
||||
}
|
||||
if (!rcl_clock_valid(get_clock_handle())) {
|
||||
throw std::runtime_error("clock cannot be waited on as it is not rcl_clock_valid");
|
||||
}
|
||||
|
||||
if (started()) {
|
||||
return true;
|
||||
} else {
|
||||
// Wait until the first non-zero time
|
||||
return sleep_until(rclcpp::Time(0, 1, get_clock_type()), context);
|
||||
}
|
||||
}
|
||||
|
||||
bool
|
||||
Clock::wait_until_started(
|
||||
const Duration & timeout,
|
||||
Context::SharedPtr context,
|
||||
const Duration & wait_tick_ns)
|
||||
{
|
||||
if (!context || !context->is_valid()) {
|
||||
throw std::runtime_error("context cannot be slept with because it's invalid");
|
||||
}
|
||||
if (!rcl_clock_valid(get_clock_handle())) {
|
||||
throw std::runtime_error("clock cannot be waited on as it is not rcl_clock_valid");
|
||||
}
|
||||
|
||||
Clock timeout_clock = Clock(RCL_STEADY_TIME);
|
||||
Time start = timeout_clock.now();
|
||||
|
||||
// Check if the clock has started every wait_tick_ns nanoseconds
|
||||
// Context check checks for rclcpp::shutdown()
|
||||
while (!started() && context->is_valid()) {
|
||||
if (timeout < wait_tick_ns) {
|
||||
timeout_clock.sleep_for(timeout);
|
||||
} else {
|
||||
Duration time_left = start + timeout - timeout_clock.now();
|
||||
if (time_left > wait_tick_ns) {
|
||||
timeout_clock.sleep_for(Duration(wait_tick_ns));
|
||||
} else {
|
||||
timeout_clock.sleep_for(time_left);
|
||||
}
|
||||
}
|
||||
|
||||
if (timeout_clock.now() - start > timeout) {
|
||||
return started();
|
||||
}
|
||||
}
|
||||
return started();
|
||||
}
|
||||
|
||||
|
||||
bool
|
||||
Clock::ros_time_is_active()
|
||||
{
|
||||
|
||||
@@ -219,7 +219,7 @@ Context::init(
|
||||
if (0u == count) {
|
||||
ret = rcl_logging_configure_with_output_handler(
|
||||
&rcl_context_->global_arguments,
|
||||
rcl_init_options_get_allocator(init_options.get_rcl_init_options()),
|
||||
rcl_init_options_get_allocator(init_options_.get_rcl_init_options()),
|
||||
rclcpp_logging_output_handler);
|
||||
if (RCL_RET_OK != ret) {
|
||||
rcl_context_.reset();
|
||||
@@ -365,45 +365,49 @@ Context::on_shutdown(OnShutdownCallback callback)
|
||||
rclcpp::OnShutdownCallbackHandle
|
||||
Context::add_on_shutdown_callback(OnShutdownCallback callback)
|
||||
{
|
||||
return add_shutdown_callback<ShutdownType::on_shutdown>(callback);
|
||||
return add_shutdown_callback(ShutdownType::on_shutdown, callback);
|
||||
}
|
||||
|
||||
bool
|
||||
Context::remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle)
|
||||
{
|
||||
return remove_shutdown_callback<ShutdownType::on_shutdown>(callback_handle);
|
||||
return remove_shutdown_callback(ShutdownType::on_shutdown, callback_handle);
|
||||
}
|
||||
|
||||
rclcpp::PreShutdownCallbackHandle
|
||||
Context::add_pre_shutdown_callback(PreShutdownCallback callback)
|
||||
{
|
||||
return add_shutdown_callback<ShutdownType::pre_shutdown>(callback);
|
||||
return add_shutdown_callback(ShutdownType::pre_shutdown, callback);
|
||||
}
|
||||
|
||||
bool
|
||||
Context::remove_pre_shutdown_callback(
|
||||
const PreShutdownCallbackHandle & callback_handle)
|
||||
{
|
||||
return remove_shutdown_callback<ShutdownType::pre_shutdown>(callback_handle);
|
||||
return remove_shutdown_callback(ShutdownType::pre_shutdown, callback_handle);
|
||||
}
|
||||
|
||||
template<Context::ShutdownType shutdown_type>
|
||||
rclcpp::ShutdownCallbackHandle
|
||||
Context::add_shutdown_callback(
|
||||
ShutdownType shutdown_type,
|
||||
ShutdownCallback callback)
|
||||
{
|
||||
auto callback_shared_ptr =
|
||||
std::make_shared<ShutdownCallbackHandle::ShutdownCallbackType>(callback);
|
||||
|
||||
static_assert(
|
||||
shutdown_type == ShutdownType::pre_shutdown || shutdown_type == ShutdownType::on_shutdown);
|
||||
|
||||
if constexpr (shutdown_type == ShutdownType::pre_shutdown) {
|
||||
std::lock_guard<std::mutex> lock(pre_shutdown_callbacks_mutex_);
|
||||
pre_shutdown_callbacks_.emplace_back(callback_shared_ptr);
|
||||
} else {
|
||||
std::lock_guard<std::mutex> lock(on_shutdown_callbacks_mutex_);
|
||||
on_shutdown_callbacks_.emplace_back(callback_shared_ptr);
|
||||
switch (shutdown_type) {
|
||||
case ShutdownType::pre_shutdown:
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(pre_shutdown_callbacks_mutex_);
|
||||
pre_shutdown_callbacks_.emplace(callback_shared_ptr);
|
||||
}
|
||||
break;
|
||||
case ShutdownType::on_shutdown:
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(on_shutdown_callbacks_mutex_);
|
||||
on_shutdown_callbacks_.emplace(callback_shared_ptr);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
ShutdownCallbackHandle callback_handle;
|
||||
@@ -411,74 +415,73 @@ Context::add_shutdown_callback(
|
||||
return callback_handle;
|
||||
}
|
||||
|
||||
template<Context::ShutdownType shutdown_type>
|
||||
bool
|
||||
Context::remove_shutdown_callback(
|
||||
ShutdownType shutdown_type,
|
||||
const ShutdownCallbackHandle & callback_handle)
|
||||
{
|
||||
const auto callback_shared_ptr = callback_handle.callback.lock();
|
||||
std::mutex * mutex_ptr = nullptr;
|
||||
std::unordered_set<
|
||||
std::shared_ptr<ShutdownCallbackHandle::ShutdownCallbackType>> * callback_list_ptr;
|
||||
|
||||
switch (shutdown_type) {
|
||||
case ShutdownType::pre_shutdown:
|
||||
mutex_ptr = &pre_shutdown_callbacks_mutex_;
|
||||
callback_list_ptr = &pre_shutdown_callbacks_;
|
||||
break;
|
||||
case ShutdownType::on_shutdown:
|
||||
mutex_ptr = &on_shutdown_callbacks_mutex_;
|
||||
callback_list_ptr = &on_shutdown_callbacks_;
|
||||
break;
|
||||
}
|
||||
|
||||
std::lock_guard<std::mutex> lock(*mutex_ptr);
|
||||
auto callback_shared_ptr = callback_handle.callback.lock();
|
||||
if (callback_shared_ptr == nullptr) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const auto remove_callback = [&callback_shared_ptr](auto & mutex, auto & callback_vector) {
|
||||
const std::lock_guard<std::mutex> lock(mutex);
|
||||
auto iter = callback_vector.begin();
|
||||
for (; iter != callback_vector.end(); iter++) {
|
||||
if ((*iter).get() == callback_shared_ptr.get()) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (iter == callback_vector.end()) {
|
||||
return false;
|
||||
}
|
||||
callback_vector.erase(iter);
|
||||
return true;
|
||||
};
|
||||
|
||||
static_assert(
|
||||
shutdown_type == ShutdownType::pre_shutdown || shutdown_type == ShutdownType::on_shutdown);
|
||||
|
||||
if constexpr (shutdown_type == ShutdownType::pre_shutdown) {
|
||||
return remove_callback(pre_shutdown_callbacks_mutex_, pre_shutdown_callbacks_);
|
||||
} else {
|
||||
return remove_callback(on_shutdown_callbacks_mutex_, on_shutdown_callbacks_);
|
||||
}
|
||||
return callback_list_ptr->erase(callback_shared_ptr) == 1;
|
||||
}
|
||||
|
||||
std::vector<rclcpp::Context::OnShutdownCallback>
|
||||
Context::get_on_shutdown_callbacks() const
|
||||
{
|
||||
return get_shutdown_callback<ShutdownType::on_shutdown>();
|
||||
return get_shutdown_callback(ShutdownType::on_shutdown);
|
||||
}
|
||||
|
||||
std::vector<rclcpp::Context::PreShutdownCallback>
|
||||
Context::get_pre_shutdown_callbacks() const
|
||||
{
|
||||
return get_shutdown_callback<ShutdownType::pre_shutdown>();
|
||||
return get_shutdown_callback(ShutdownType::pre_shutdown);
|
||||
}
|
||||
|
||||
template<Context::ShutdownType shutdown_type>
|
||||
std::vector<rclcpp::Context::ShutdownCallback>
|
||||
Context::get_shutdown_callback() const
|
||||
Context::get_shutdown_callback(ShutdownType shutdown_type) const
|
||||
{
|
||||
const auto get_callback_vector = [this](auto & mutex, auto & callback_set) {
|
||||
const std::lock_guard<std::mutex> lock(mutex);
|
||||
std::vector<rclcpp::Context::ShutdownCallback> callbacks;
|
||||
for (auto & callback : callback_set) {
|
||||
callbacks.push_back(*callback);
|
||||
}
|
||||
return callbacks;
|
||||
};
|
||||
std::mutex * mutex_ptr = nullptr;
|
||||
const std::unordered_set<
|
||||
std::shared_ptr<ShutdownCallbackHandle::ShutdownCallbackType>> * callback_list_ptr;
|
||||
|
||||
static_assert(
|
||||
shutdown_type == ShutdownType::pre_shutdown || shutdown_type == ShutdownType::on_shutdown);
|
||||
|
||||
if constexpr (shutdown_type == ShutdownType::pre_shutdown) {
|
||||
return get_callback_vector(pre_shutdown_callbacks_mutex_, pre_shutdown_callbacks_);
|
||||
} else {
|
||||
return get_callback_vector(on_shutdown_callbacks_mutex_, on_shutdown_callbacks_);
|
||||
switch (shutdown_type) {
|
||||
case ShutdownType::pre_shutdown:
|
||||
mutex_ptr = &pre_shutdown_callbacks_mutex_;
|
||||
callback_list_ptr = &pre_shutdown_callbacks_;
|
||||
break;
|
||||
case ShutdownType::on_shutdown:
|
||||
mutex_ptr = &on_shutdown_callbacks_mutex_;
|
||||
callback_list_ptr = &on_shutdown_callbacks_;
|
||||
break;
|
||||
}
|
||||
|
||||
std::vector<rclcpp::Context::ShutdownCallback> callbacks;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(*mutex_ptr);
|
||||
for (auto & iter : *callback_list_ptr) {
|
||||
callbacks.emplace_back(*iter);
|
||||
}
|
||||
}
|
||||
|
||||
return callbacks;
|
||||
}
|
||||
|
||||
std::shared_ptr<rcl_context_t>
|
||||
|
||||
@@ -1,40 +0,0 @@
|
||||
// Copyright 2023 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 <rosidl_dynamic_typesupport/api/dynamic_type.h>
|
||||
#include <rosidl_dynamic_typesupport/api/dynamic_data.h>
|
||||
#include <rosidl_dynamic_typesupport/api/serialization_support.h>
|
||||
#include <rosidl_dynamic_typesupport/types.h>
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "rcl/allocator.h"
|
||||
#include "rcl/types.h"
|
||||
#include "rcutils/logging_macros.h"
|
||||
|
||||
#include "rclcpp/dynamic_typesupport/dynamic_message.hpp"
|
||||
#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp"
|
||||
#include "rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp"
|
||||
#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
|
||||
#include "rclcpp/exceptions.hpp"
|
||||
|
||||
|
||||
using rclcpp::dynamic_typesupport::DynamicMessage;
|
||||
using rclcpp::dynamic_typesupport::DynamicMessageType;
|
||||
using rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder;
|
||||
using rclcpp::dynamic_typesupport::DynamicSerializationSupport;
|
||||
|
||||
DynamicMessage::~DynamicMessage()
|
||||
{} // STUBBED
|
||||
@@ -1,38 +0,0 @@
|
||||
// Copyright 2023 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 <rosidl_dynamic_typesupport/api/dynamic_data.h>
|
||||
#include <rosidl_dynamic_typesupport/api/dynamic_type.h>
|
||||
#include <rosidl_dynamic_typesupport/api/serialization_support.h>
|
||||
#include <rosidl_dynamic_typesupport/types.h>
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "rcutils/logging_macros.h"
|
||||
|
||||
#include "rclcpp/dynamic_typesupport/dynamic_message.hpp"
|
||||
#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp"
|
||||
#include "rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp"
|
||||
#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
|
||||
#include "rclcpp/exceptions.hpp"
|
||||
|
||||
|
||||
using rclcpp::dynamic_typesupport::DynamicMessage;
|
||||
using rclcpp::dynamic_typesupport::DynamicMessageType;
|
||||
using rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder;
|
||||
using rclcpp::dynamic_typesupport::DynamicSerializationSupport;
|
||||
|
||||
DynamicMessageType::~DynamicMessageType()
|
||||
{} // STUBBED
|
||||
@@ -1,37 +0,0 @@
|
||||
// Copyright 2023 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 <rosidl_dynamic_typesupport/api/dynamic_data.h>
|
||||
#include <rosidl_dynamic_typesupport/api/dynamic_type.h>
|
||||
#include <rosidl_dynamic_typesupport/api/serialization_support.h>
|
||||
#include <rosidl_dynamic_typesupport/types.h>
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "rcutils/logging_macros.h"
|
||||
|
||||
#include "rclcpp/dynamic_typesupport/dynamic_message.hpp"
|
||||
#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp"
|
||||
#include "rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp"
|
||||
#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
|
||||
#include "rclcpp/exceptions.hpp"
|
||||
|
||||
using rclcpp::dynamic_typesupport::DynamicMessage;
|
||||
using rclcpp::dynamic_typesupport::DynamicMessageType;
|
||||
using rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder;
|
||||
using rclcpp::dynamic_typesupport::DynamicSerializationSupport;
|
||||
|
||||
DynamicMessageTypeBuilder::~DynamicMessageTypeBuilder()
|
||||
{} // STUBBED
|
||||
@@ -1,49 +0,0 @@
|
||||
// Copyright 2023 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 <rosidl_dynamic_typesupport/identifier.h>
|
||||
#include <rosidl_dynamic_typesupport/types.h>
|
||||
#include <rosidl_runtime_c/message_type_support_struct.h>
|
||||
#include <rosidl_runtime_c/type_description_utils.h>
|
||||
#include <rosidl_runtime_c/type_description/type_description__functions.h>
|
||||
#include <rosidl_runtime_c/type_description/type_description__struct.h>
|
||||
#include <rosidl_runtime_c/type_description/type_source__functions.h>
|
||||
#include <rosidl_runtime_c/type_description/type_source__struct.h>
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "rcl/allocator.h"
|
||||
#include "rcl/dynamic_message_type_support.h"
|
||||
#include "rcl/type_hash.h"
|
||||
#include "rcl/types.h"
|
||||
#include "rcutils/logging_macros.h"
|
||||
#include "rcutils/types/rcutils_ret.h"
|
||||
#include "rmw/dynamic_message_type_support.h"
|
||||
|
||||
#include "rclcpp/dynamic_typesupport/dynamic_message.hpp"
|
||||
#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp"
|
||||
#include "rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp"
|
||||
#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
|
||||
#include "rclcpp/exceptions.hpp"
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
using rclcpp::dynamic_typesupport::DynamicMessage;
|
||||
using rclcpp::dynamic_typesupport::DynamicMessageType;
|
||||
using rclcpp::dynamic_typesupport::DynamicMessageTypeSupport;
|
||||
using rclcpp::dynamic_typesupport::DynamicSerializationSupport;
|
||||
|
||||
DynamicMessageTypeSupport::~DynamicMessageTypeSupport()
|
||||
{} // STUBBED
|
||||
@@ -1,46 +0,0 @@
|
||||
// Copyright 2023 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 <rcl/allocator.h>
|
||||
#include <rcutils/logging_macros.h>
|
||||
#include <rmw/dynamic_message_type_support.h>
|
||||
#include <rmw/ret_types.h>
|
||||
#include <rosidl_dynamic_typesupport/api/serialization_support.h>
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp"
|
||||
#include "rclcpp/exceptions.hpp"
|
||||
|
||||
using rclcpp::dynamic_typesupport::DynamicSerializationSupport;
|
||||
|
||||
// CONSTRUCTION ====================================================================================
|
||||
DynamicSerializationSupport::DynamicSerializationSupport(rcl_allocator_t allocator)
|
||||
: DynamicSerializationSupport::DynamicSerializationSupport("", allocator)
|
||||
{
|
||||
throw std::runtime_error("Unimplemented");
|
||||
}
|
||||
|
||||
DynamicSerializationSupport::DynamicSerializationSupport(
|
||||
const std::string & /*serialization_library_name*/,
|
||||
rcl_allocator_t /*allocator*/)
|
||||
: rosidl_serialization_support_(
|
||||
rosidl_dynamic_typesupport_get_zero_initialized_serialization_support())
|
||||
{
|
||||
throw std::runtime_error("Unimplemented");
|
||||
}
|
||||
|
||||
DynamicSerializationSupport::~DynamicSerializationSupport()
|
||||
{} // STUBBED
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,228 +0,0 @@
|
||||
// Copyright 2023 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/executors/executor_entities_collection.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace executors
|
||||
{
|
||||
bool ExecutorEntitiesCollection::empty() const
|
||||
{
|
||||
return
|
||||
subscriptions.empty() &&
|
||||
timers.empty() &&
|
||||
guard_conditions.empty() &&
|
||||
clients.empty() &&
|
||||
services.empty() &&
|
||||
waitables.empty();
|
||||
}
|
||||
|
||||
void ExecutorEntitiesCollection::clear()
|
||||
{
|
||||
subscriptions.clear();
|
||||
timers.clear();
|
||||
guard_conditions.clear();
|
||||
clients.clear();
|
||||
services.clear();
|
||||
waitables.clear();
|
||||
}
|
||||
|
||||
void
|
||||
build_entities_collection(
|
||||
const std::vector<rclcpp::CallbackGroup::WeakPtr> & callback_groups,
|
||||
ExecutorEntitiesCollection & collection)
|
||||
{
|
||||
collection.clear();
|
||||
|
||||
for (auto weak_group_ptr : callback_groups) {
|
||||
auto group_ptr = weak_group_ptr.lock();
|
||||
if (!group_ptr) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (group_ptr->can_be_taken_from().load()) {
|
||||
group_ptr->collect_all_ptrs(
|
||||
[&collection, weak_group_ptr](const rclcpp::SubscriptionBase::SharedPtr & subscription) {
|
||||
collection.subscriptions.insert(
|
||||
{
|
||||
subscription->get_subscription_handle().get(),
|
||||
{subscription, weak_group_ptr}
|
||||
});
|
||||
},
|
||||
[&collection, weak_group_ptr](const rclcpp::ServiceBase::SharedPtr & service) {
|
||||
collection.services.insert(
|
||||
{
|
||||
service->get_service_handle().get(),
|
||||
{service, weak_group_ptr}
|
||||
});
|
||||
},
|
||||
[&collection, weak_group_ptr](const rclcpp::ClientBase::SharedPtr & client) {
|
||||
collection.clients.insert(
|
||||
{
|
||||
client->get_client_handle().get(),
|
||||
{client, weak_group_ptr}
|
||||
});
|
||||
},
|
||||
[&collection, weak_group_ptr](const rclcpp::TimerBase::SharedPtr & timer) {
|
||||
collection.timers.insert(
|
||||
{
|
||||
timer->get_timer_handle().get(),
|
||||
{timer, weak_group_ptr}
|
||||
});
|
||||
},
|
||||
[&collection, weak_group_ptr](const rclcpp::Waitable::SharedPtr & waitable) {
|
||||
collection.waitables.insert(
|
||||
{
|
||||
waitable.get(),
|
||||
{waitable, weak_group_ptr}
|
||||
});
|
||||
}
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
size_t
|
||||
ready_executables(
|
||||
const ExecutorEntitiesCollection & collection,
|
||||
rclcpp::WaitResult<rclcpp::WaitSet> & wait_result,
|
||||
std::deque<rclcpp::AnyExecutable> & executables
|
||||
)
|
||||
{
|
||||
size_t added = 0;
|
||||
if (wait_result.kind() != rclcpp::WaitResultKind::Ready) {
|
||||
return added;
|
||||
}
|
||||
auto rcl_wait_set = wait_result.get_wait_set().get_rcl_wait_set();
|
||||
|
||||
// Cache shared pointers to groups to avoid extra work re-locking them
|
||||
std::map<rclcpp::CallbackGroup::WeakPtr,
|
||||
rclcpp::CallbackGroup::SharedPtr,
|
||||
std::owner_less<rclcpp::CallbackGroup::WeakPtr>> group_map;
|
||||
|
||||
auto group_cache = [&group_map](const rclcpp::CallbackGroup::WeakPtr & weak_cbg_ptr)
|
||||
{
|
||||
if (group_map.count(weak_cbg_ptr) == 0) {
|
||||
group_map.insert({weak_cbg_ptr, weak_cbg_ptr.lock()});
|
||||
}
|
||||
return group_map.find(weak_cbg_ptr)->second;
|
||||
};
|
||||
|
||||
for (size_t ii = 0; ii < rcl_wait_set.size_of_timers; ++ii) {
|
||||
if (nullptr == rcl_wait_set.timers[ii]) {continue;}
|
||||
auto entity_iter = collection.timers.find(rcl_wait_set.timers[ii]);
|
||||
if (entity_iter != collection.timers.end()) {
|
||||
auto entity = entity_iter->second.entity.lock();
|
||||
if (!entity) {
|
||||
continue;
|
||||
}
|
||||
auto group_info = group_cache(entity_iter->second.callback_group);
|
||||
if (group_info && !group_info->can_be_taken_from().load()) {
|
||||
continue;
|
||||
}
|
||||
if (!entity->call()) {
|
||||
continue;
|
||||
}
|
||||
rclcpp::AnyExecutable exec;
|
||||
exec.timer = entity;
|
||||
exec.callback_group = group_info;
|
||||
executables.push_back(exec);
|
||||
added++;
|
||||
}
|
||||
}
|
||||
|
||||
for (size_t ii = 0; ii < rcl_wait_set.size_of_subscriptions; ++ii) {
|
||||
if (nullptr == rcl_wait_set.subscriptions[ii]) {continue;}
|
||||
auto entity_iter = collection.subscriptions.find(rcl_wait_set.subscriptions[ii]);
|
||||
if (entity_iter != collection.subscriptions.end()) {
|
||||
auto entity = entity_iter->second.entity.lock();
|
||||
if (!entity) {
|
||||
continue;
|
||||
}
|
||||
auto group_info = group_cache(entity_iter->second.callback_group);
|
||||
if (group_info && !group_info->can_be_taken_from().load()) {
|
||||
continue;
|
||||
}
|
||||
rclcpp::AnyExecutable exec;
|
||||
exec.subscription = entity;
|
||||
exec.callback_group = group_info;
|
||||
executables.push_back(exec);
|
||||
added++;
|
||||
}
|
||||
}
|
||||
|
||||
for (size_t ii = 0; ii < rcl_wait_set.size_of_services; ++ii) {
|
||||
if (nullptr == rcl_wait_set.services[ii]) {continue;}
|
||||
auto entity_iter = collection.services.find(rcl_wait_set.services[ii]);
|
||||
if (entity_iter != collection.services.end()) {
|
||||
auto entity = entity_iter->second.entity.lock();
|
||||
if (!entity) {
|
||||
continue;
|
||||
}
|
||||
auto group_info = group_cache(entity_iter->second.callback_group);
|
||||
if (group_info && !group_info->can_be_taken_from().load()) {
|
||||
continue;
|
||||
}
|
||||
rclcpp::AnyExecutable exec;
|
||||
exec.service = entity;
|
||||
exec.callback_group = group_info;
|
||||
executables.push_back(exec);
|
||||
added++;
|
||||
}
|
||||
}
|
||||
|
||||
for (size_t ii = 0; ii < rcl_wait_set.size_of_clients; ++ii) {
|
||||
if (nullptr == rcl_wait_set.clients[ii]) {continue;}
|
||||
auto entity_iter = collection.clients.find(rcl_wait_set.clients[ii]);
|
||||
if (entity_iter != collection.clients.end()) {
|
||||
auto entity = entity_iter->second.entity.lock();
|
||||
if (!entity) {
|
||||
continue;
|
||||
}
|
||||
auto group_info = group_cache(entity_iter->second.callback_group);
|
||||
if (group_info && !group_info->can_be_taken_from().load()) {
|
||||
continue;
|
||||
}
|
||||
rclcpp::AnyExecutable exec;
|
||||
exec.client = entity;
|
||||
exec.callback_group = group_info;
|
||||
executables.push_back(exec);
|
||||
added++;
|
||||
}
|
||||
}
|
||||
|
||||
for (auto & [handle, entry] : collection.waitables) {
|
||||
auto waitable = entry.entity.lock();
|
||||
if (!waitable) {
|
||||
continue;
|
||||
}
|
||||
if (!waitable->is_ready(&rcl_wait_set)) {
|
||||
continue;
|
||||
}
|
||||
auto group_info = group_cache(entry.callback_group);
|
||||
if (group_info && !group_info->can_be_taken_from().load()) {
|
||||
continue;
|
||||
}
|
||||
rclcpp::AnyExecutable exec;
|
||||
exec.waitable = waitable;
|
||||
exec.callback_group = group_info;
|
||||
exec.data = waitable->take_data();
|
||||
executables.push_back(exec);
|
||||
added++;
|
||||
}
|
||||
return added;
|
||||
}
|
||||
} // namespace executors
|
||||
} // namespace rclcpp
|
||||
@@ -1,420 +0,0 @@
|
||||
// Copyright 2023 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 <set>
|
||||
|
||||
#include "rclcpp/executors/executor_entities_collector.hpp"
|
||||
#include "rclcpp/executors/executor_notify_waitable.hpp"
|
||||
#include "rclcpp/node_interfaces/node_base_interface.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace executors
|
||||
{
|
||||
|
||||
ExecutorEntitiesCollector::ExecutorEntitiesCollector(
|
||||
std::shared_ptr<ExecutorNotifyWaitable> notify_waitable)
|
||||
: notify_waitable_(notify_waitable)
|
||||
{
|
||||
}
|
||||
|
||||
ExecutorEntitiesCollector::~ExecutorEntitiesCollector()
|
||||
{
|
||||
for (auto weak_node_it = weak_nodes_.begin(); weak_node_it != weak_nodes_.end(); ) {
|
||||
weak_node_it = remove_weak_node(weak_node_it);
|
||||
}
|
||||
|
||||
for (auto weak_group_it = automatically_added_groups_.begin();
|
||||
weak_group_it != automatically_added_groups_.end(); )
|
||||
{
|
||||
weak_group_it = remove_weak_callback_group(weak_group_it, automatically_added_groups_);
|
||||
}
|
||||
|
||||
for (auto weak_group_it = manually_added_groups_.begin();
|
||||
weak_group_it != manually_added_groups_.end(); )
|
||||
{
|
||||
weak_group_it = remove_weak_callback_group(weak_group_it, manually_added_groups_);
|
||||
}
|
||||
|
||||
for (auto weak_node_ptr : pending_added_nodes_) {
|
||||
auto node_ptr = weak_node_ptr.lock();
|
||||
if (node_ptr) {
|
||||
node_ptr->get_associated_with_executor_atomic().store(false);
|
||||
}
|
||||
}
|
||||
pending_added_nodes_.clear();
|
||||
pending_removed_nodes_.clear();
|
||||
|
||||
for (auto weak_group_ptr : pending_manually_added_groups_) {
|
||||
auto group_ptr = weak_group_ptr.lock();
|
||||
if (group_ptr) {
|
||||
group_ptr->get_associated_with_executor_atomic().store(false);
|
||||
}
|
||||
}
|
||||
pending_manually_added_groups_.clear();
|
||||
pending_manually_removed_groups_.clear();
|
||||
}
|
||||
|
||||
bool
|
||||
ExecutorEntitiesCollector::has_pending() const
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
return pending_manually_added_groups_.size() != 0 ||
|
||||
pending_manually_removed_groups_.size() != 0 ||
|
||||
pending_added_nodes_.size() != 0 ||
|
||||
pending_removed_nodes_.size() != 0;
|
||||
}
|
||||
|
||||
void
|
||||
ExecutorEntitiesCollector::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
|
||||
{
|
||||
// If the node already has an executor
|
||||
std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
|
||||
if (has_executor.exchange(true)) {
|
||||
throw std::runtime_error(
|
||||
std::string("Node '") + node_ptr->get_fully_qualified_name() +
|
||||
"' has already been added to an executor.");
|
||||
}
|
||||
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
bool associated = weak_nodes_.count(node_ptr) != 0;
|
||||
bool add_queued = pending_added_nodes_.count(node_ptr) != 0;
|
||||
bool remove_queued = pending_removed_nodes_.count(node_ptr) != 0;
|
||||
|
||||
if ((associated || add_queued) && !remove_queued) {
|
||||
throw std::runtime_error(
|
||||
std::string("Node '") + node_ptr->get_fully_qualified_name() +
|
||||
"' has already been added to this executor.");
|
||||
}
|
||||
|
||||
this->pending_added_nodes_.insert(node_ptr);
|
||||
}
|
||||
|
||||
void
|
||||
ExecutorEntitiesCollector::remove_node(
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
|
||||
{
|
||||
std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
|
||||
if (!has_executor.exchange(false)) {
|
||||
throw std::runtime_error(
|
||||
std::string("Node '") + node_ptr->get_fully_qualified_name() +
|
||||
"' needs to be associated with an executor.");
|
||||
}
|
||||
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
bool associated = weak_nodes_.count(node_ptr) != 0;
|
||||
bool add_queued = pending_added_nodes_.count(node_ptr) != 0;
|
||||
bool remove_queued = pending_removed_nodes_.count(node_ptr) != 0;
|
||||
|
||||
if (!(associated || add_queued) || remove_queued) {
|
||||
throw std::runtime_error(
|
||||
std::string("Node '") + node_ptr->get_fully_qualified_name() +
|
||||
"' needs to be associated with this executor.");
|
||||
}
|
||||
|
||||
this->pending_removed_nodes_.insert(node_ptr);
|
||||
}
|
||||
|
||||
void
|
||||
ExecutorEntitiesCollector::add_callback_group(rclcpp::CallbackGroup::SharedPtr group_ptr)
|
||||
{
|
||||
std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic();
|
||||
if (has_executor.exchange(true)) {
|
||||
throw std::runtime_error("Callback group has already been added to an executor.");
|
||||
}
|
||||
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
bool associated = manually_added_groups_.count(group_ptr) != 0;
|
||||
bool add_queued = pending_manually_added_groups_.count(group_ptr) != 0;
|
||||
bool remove_queued = pending_manually_removed_groups_.count(group_ptr) != 0;
|
||||
|
||||
if ((associated || add_queued) && !remove_queued) {
|
||||
throw std::runtime_error("Callback group has already been added to this executor.");
|
||||
}
|
||||
|
||||
this->pending_manually_added_groups_.insert(group_ptr);
|
||||
}
|
||||
|
||||
void
|
||||
ExecutorEntitiesCollector::remove_callback_group(rclcpp::CallbackGroup::SharedPtr group_ptr)
|
||||
{
|
||||
if (!group_ptr->get_associated_with_executor_atomic().load()) {
|
||||
throw std::runtime_error("Callback group needs to be associated with an executor.");
|
||||
}
|
||||
/**
|
||||
* TODO(mjcarroll): The callback groups, being created by a node, should never outlive
|
||||
* the node. Since we haven't historically enforced this, turning this on may cause
|
||||
* previously-functional code to fail.
|
||||
* Consider re-enablng this check (along with corresponding CallbackGroup::has_valid_node),
|
||||
* when we can guarantee node/group lifetimes.
|
||||
if (!group_ptr->has_valid_node()) {
|
||||
throw std::runtime_error("Node must not be deleted before its callback group(s).");
|
||||
}
|
||||
*/
|
||||
auto weak_group_ptr = rclcpp::CallbackGroup::WeakPtr(group_ptr);
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
bool associated = manually_added_groups_.count(group_ptr) != 0;
|
||||
bool add_queued = pending_manually_added_groups_.count(group_ptr) != 0;
|
||||
bool remove_queued = pending_manually_removed_groups_.count(group_ptr) != 0;
|
||||
|
||||
if (!(associated || add_queued) || remove_queued) {
|
||||
throw std::runtime_error("Callback group needs to be associated with this executor.");
|
||||
}
|
||||
|
||||
this->pending_manually_removed_groups_.insert(group_ptr);
|
||||
}
|
||||
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr>
|
||||
ExecutorEntitiesCollector::get_all_callback_groups() const
|
||||
{
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
for (const auto & group_ptr : manually_added_groups_) {
|
||||
groups.push_back(group_ptr);
|
||||
}
|
||||
for (auto const & group_ptr : automatically_added_groups_) {
|
||||
groups.push_back(group_ptr);
|
||||
}
|
||||
return groups;
|
||||
}
|
||||
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr>
|
||||
ExecutorEntitiesCollector::get_manually_added_callback_groups() const
|
||||
{
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
for (const auto & group_ptr : manually_added_groups_) {
|
||||
groups.push_back(group_ptr);
|
||||
}
|
||||
return groups;
|
||||
}
|
||||
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr>
|
||||
ExecutorEntitiesCollector::get_automatically_added_callback_groups() const
|
||||
{
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
for (auto const & group_ptr : automatically_added_groups_) {
|
||||
groups.push_back(group_ptr);
|
||||
}
|
||||
return groups;
|
||||
}
|
||||
|
||||
void
|
||||
ExecutorEntitiesCollector::update_collections()
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
this->process_queues();
|
||||
this->add_automatically_associated_callback_groups(this->weak_nodes_);
|
||||
this->prune_invalid_nodes_and_groups();
|
||||
}
|
||||
|
||||
ExecutorEntitiesCollector::NodeCollection::iterator
|
||||
ExecutorEntitiesCollector::remove_weak_node(NodeCollection::iterator weak_node)
|
||||
{
|
||||
// Disassociate the guard condition from the executor notify waitable
|
||||
auto guard_condition_it = weak_nodes_to_guard_conditions_.find(*weak_node);
|
||||
if (guard_condition_it != weak_nodes_to_guard_conditions_.end()) {
|
||||
this->notify_waitable_->remove_guard_condition(guard_condition_it->second);
|
||||
weak_nodes_to_guard_conditions_.erase(guard_condition_it);
|
||||
}
|
||||
|
||||
// Mark the node as disassociated (if the node is still valid)
|
||||
auto node_ptr = weak_node->lock();
|
||||
if (node_ptr) {
|
||||
std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
|
||||
has_executor.store(false);
|
||||
}
|
||||
|
||||
// Remove the node from tracked nodes
|
||||
return weak_nodes_.erase(weak_node);
|
||||
}
|
||||
|
||||
ExecutorEntitiesCollector::CallbackGroupCollection::iterator
|
||||
ExecutorEntitiesCollector::remove_weak_callback_group(
|
||||
CallbackGroupCollection::iterator weak_group_it,
|
||||
CallbackGroupCollection & collection
|
||||
)
|
||||
{
|
||||
// Disassociate the guard condition from the executor notify waitable
|
||||
auto guard_condition_it = weak_groups_to_guard_conditions_.find(*weak_group_it);
|
||||
if (guard_condition_it != weak_groups_to_guard_conditions_.end()) {
|
||||
this->notify_waitable_->remove_guard_condition(guard_condition_it->second);
|
||||
weak_groups_to_guard_conditions_.erase(guard_condition_it);
|
||||
}
|
||||
|
||||
// Mark the node as disassociated (if the group is still valid)
|
||||
auto group_ptr = weak_group_it->lock();
|
||||
if (group_ptr) {
|
||||
/**
|
||||
* TODO(mjcarroll): The callback groups, being created by a node, should never outlive
|
||||
* the node. Since we haven't historically enforced this, turning this on may cause
|
||||
* previously-functional code to fail.
|
||||
* Consider re-enablng this check (along with corresponding CallbackGroup::has_valid_node),
|
||||
* when we can guarantee node/group lifetimes.
|
||||
if (!group_ptr->has_valid_node()) {
|
||||
throw std::runtime_error("Node must not be deleted before its callback group(s).");
|
||||
}
|
||||
*/
|
||||
std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic();
|
||||
has_executor.store(false);
|
||||
}
|
||||
|
||||
// Remove the node from tracked nodes
|
||||
return collection.erase(weak_group_it);
|
||||
}
|
||||
|
||||
void
|
||||
ExecutorEntitiesCollector::add_callback_group_to_collection(
|
||||
rclcpp::CallbackGroup::SharedPtr group_ptr,
|
||||
CallbackGroupCollection & collection)
|
||||
{
|
||||
auto iter = collection.insert(group_ptr);
|
||||
if (iter.second == false) {
|
||||
throw std::runtime_error("Callback group has already been added to this executor.");
|
||||
}
|
||||
|
||||
// Store node guard condition in map and add it to the notify waitable
|
||||
auto group_guard_condition = group_ptr->get_notify_guard_condition();
|
||||
weak_groups_to_guard_conditions_.insert({group_ptr, group_guard_condition});
|
||||
this->notify_waitable_->add_guard_condition(group_guard_condition);
|
||||
}
|
||||
|
||||
void
|
||||
ExecutorEntitiesCollector::process_queues()
|
||||
{
|
||||
for (auto weak_node_ptr : pending_added_nodes_) {
|
||||
auto node_ptr = weak_node_ptr.lock();
|
||||
if (!node_ptr) {
|
||||
continue;
|
||||
}
|
||||
weak_nodes_.insert(weak_node_ptr);
|
||||
this->add_automatically_associated_callback_groups({weak_node_ptr});
|
||||
|
||||
// Store node guard condition in map and add it to the notify waitable
|
||||
auto node_guard_condition = node_ptr->get_shared_notify_guard_condition();
|
||||
weak_nodes_to_guard_conditions_.insert({weak_node_ptr, node_guard_condition});
|
||||
this->notify_waitable_->add_guard_condition(node_guard_condition);
|
||||
}
|
||||
pending_added_nodes_.clear();
|
||||
|
||||
for (auto weak_node_ptr : pending_removed_nodes_) {
|
||||
auto node_it = weak_nodes_.find(weak_node_ptr);
|
||||
if (node_it != weak_nodes_.end()) {
|
||||
remove_weak_node(node_it);
|
||||
} else {
|
||||
// The node may have been destroyed and removed from the colletion before
|
||||
// we processed the queues. Don't throw if the pointer is already expired.
|
||||
if (!weak_node_ptr.expired()) {
|
||||
throw std::runtime_error("Node needs to be associated with this executor.");
|
||||
}
|
||||
}
|
||||
|
||||
auto node_ptr = weak_node_ptr.lock();
|
||||
if (node_ptr) {
|
||||
for (auto group_it = automatically_added_groups_.begin();
|
||||
group_it != automatically_added_groups_.end(); )
|
||||
{
|
||||
auto group_ptr = group_it->lock();
|
||||
if (node_ptr->callback_group_in_node(group_ptr)) {
|
||||
group_it = remove_weak_callback_group(group_it, automatically_added_groups_);
|
||||
} else {
|
||||
++group_it;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
pending_removed_nodes_.clear();
|
||||
|
||||
for (auto weak_group_ptr : pending_manually_added_groups_) {
|
||||
auto group_ptr = weak_group_ptr.lock();
|
||||
if (group_ptr) {
|
||||
this->add_callback_group_to_collection(group_ptr, manually_added_groups_);
|
||||
}
|
||||
}
|
||||
pending_manually_added_groups_.clear();
|
||||
|
||||
for (auto weak_group_ptr : pending_manually_removed_groups_) {
|
||||
auto group_ptr = weak_group_ptr.lock();
|
||||
if (group_ptr) {
|
||||
auto group_it = manually_added_groups_.find(group_ptr);
|
||||
if (group_it != manually_added_groups_.end()) {
|
||||
remove_weak_callback_group(group_it, manually_added_groups_);
|
||||
} else {
|
||||
throw std::runtime_error(
|
||||
"Attempting to remove a callback group not added to this executor.");
|
||||
}
|
||||
}
|
||||
}
|
||||
pending_manually_removed_groups_.clear();
|
||||
}
|
||||
|
||||
void
|
||||
ExecutorEntitiesCollector::add_automatically_associated_callback_groups(
|
||||
const NodeCollection & nodes_to_check)
|
||||
{
|
||||
for (auto & weak_node : nodes_to_check) {
|
||||
auto node = weak_node.lock();
|
||||
if (node) {
|
||||
node->for_each_callback_group(
|
||||
[this, node](rclcpp::CallbackGroup::SharedPtr group_ptr)
|
||||
{
|
||||
if (!group_ptr->get_associated_with_executor_atomic().load() &&
|
||||
group_ptr->automatically_add_to_executor_with_node())
|
||||
{
|
||||
std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic();
|
||||
if (has_executor.exchange(true)) {
|
||||
throw std::runtime_error("Callback group has already been added to an executor.");
|
||||
}
|
||||
this->add_callback_group_to_collection(group_ptr, this->automatically_added_groups_);
|
||||
}
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
ExecutorEntitiesCollector::prune_invalid_nodes_and_groups()
|
||||
{
|
||||
for (auto node_it = weak_nodes_.begin();
|
||||
node_it != weak_nodes_.end(); )
|
||||
{
|
||||
if (node_it->expired()) {
|
||||
node_it = remove_weak_node(node_it);
|
||||
} else {
|
||||
node_it++;
|
||||
}
|
||||
}
|
||||
for (auto group_it = automatically_added_groups_.begin();
|
||||
group_it != automatically_added_groups_.end(); )
|
||||
{
|
||||
if (group_it->expired()) {
|
||||
group_it = remove_weak_callback_group(group_it, automatically_added_groups_);
|
||||
} else {
|
||||
group_it++;
|
||||
}
|
||||
}
|
||||
for (auto group_it = manually_added_groups_.begin();
|
||||
group_it != manually_added_groups_.end(); )
|
||||
{
|
||||
if (group_it->expired()) {
|
||||
group_it = remove_weak_callback_group(group_it, manually_added_groups_);
|
||||
} else {
|
||||
group_it++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace executors
|
||||
} // namespace rclcpp
|
||||
@@ -1,181 +0,0 @@
|
||||
// Copyright 2023 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 <iostream>
|
||||
|
||||
#include "rclcpp/exceptions.hpp"
|
||||
#include "rclcpp/executors/executor_notify_waitable.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace executors
|
||||
{
|
||||
|
||||
ExecutorNotifyWaitable::ExecutorNotifyWaitable(std::function<void(void)> on_execute_callback)
|
||||
: execute_callback_(on_execute_callback)
|
||||
{
|
||||
}
|
||||
|
||||
ExecutorNotifyWaitable::ExecutorNotifyWaitable(const ExecutorNotifyWaitable & other)
|
||||
: ExecutorNotifyWaitable(other.execute_callback_)
|
||||
{
|
||||
this->notify_guard_conditions_ = other.notify_guard_conditions_;
|
||||
}
|
||||
|
||||
ExecutorNotifyWaitable & ExecutorNotifyWaitable::operator=(const ExecutorNotifyWaitable & other)
|
||||
{
|
||||
if (this != &other) {
|
||||
this->execute_callback_ = other.execute_callback_;
|
||||
this->notify_guard_conditions_ = other.notify_guard_conditions_;
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
void
|
||||
ExecutorNotifyWaitable::add_to_wait_set(rcl_wait_set_t * wait_set)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(guard_condition_mutex_);
|
||||
for (auto weak_guard_condition : this->notify_guard_conditions_) {
|
||||
auto guard_condition = weak_guard_condition.lock();
|
||||
if (!guard_condition) {continue;}
|
||||
|
||||
rcl_guard_condition_t * cond = &guard_condition->get_rcl_guard_condition();
|
||||
|
||||
rcl_ret_t ret = rcl_wait_set_add_guard_condition(
|
||||
wait_set, cond, NULL);
|
||||
|
||||
if (RCL_RET_OK != ret) {
|
||||
rclcpp::exceptions::throw_from_rcl_error(
|
||||
ret, "failed to add guard condition to wait set");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool
|
||||
ExecutorNotifyWaitable::is_ready(rcl_wait_set_t * wait_set)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(guard_condition_mutex_);
|
||||
|
||||
bool any_ready = false;
|
||||
for (size_t ii = 0; ii < wait_set->size_of_guard_conditions; ++ii) {
|
||||
auto rcl_guard_condition = wait_set->guard_conditions[ii];
|
||||
|
||||
if (nullptr == rcl_guard_condition) {
|
||||
continue;
|
||||
}
|
||||
for (auto weak_guard_condition : this->notify_guard_conditions_) {
|
||||
auto guard_condition = weak_guard_condition.lock();
|
||||
if (guard_condition && &guard_condition->get_rcl_guard_condition() == rcl_guard_condition) {
|
||||
any_ready = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
return any_ready;
|
||||
}
|
||||
|
||||
void
|
||||
ExecutorNotifyWaitable::execute(std::shared_ptr<void> & data)
|
||||
{
|
||||
(void) data;
|
||||
this->execute_callback_();
|
||||
}
|
||||
|
||||
std::shared_ptr<void>
|
||||
ExecutorNotifyWaitable::take_data()
|
||||
{
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
std::shared_ptr<void>
|
||||
ExecutorNotifyWaitable::take_data_by_entity_id(size_t id)
|
||||
{
|
||||
(void) id;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
void
|
||||
ExecutorNotifyWaitable::set_on_ready_callback(std::function<void(size_t, int)> callback)
|
||||
{
|
||||
// The second argument of the callback could be used to identify which guard condition
|
||||
// triggered the event.
|
||||
// We could indicate which of the guard conditions was triggered, but the executor
|
||||
// is already going to check that.
|
||||
auto gc_callback = [callback](size_t count) {
|
||||
callback(count, 0);
|
||||
};
|
||||
|
||||
std::lock_guard<std::mutex> lock(guard_condition_mutex_);
|
||||
|
||||
on_ready_callback_ = gc_callback;
|
||||
for (auto weak_gc : notify_guard_conditions_) {
|
||||
auto gc = weak_gc.lock();
|
||||
if (!gc) {
|
||||
continue;
|
||||
}
|
||||
gc->set_on_trigger_callback(on_ready_callback_);
|
||||
}
|
||||
}
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
ExecutorNotifyWaitable::clear_on_ready_callback()
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(guard_condition_mutex_);
|
||||
|
||||
on_ready_callback_ = nullptr;
|
||||
for (auto weak_gc : notify_guard_conditions_) {
|
||||
auto gc = weak_gc.lock();
|
||||
if (!gc) {
|
||||
continue;
|
||||
}
|
||||
gc->set_on_trigger_callback(nullptr);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
ExecutorNotifyWaitable::add_guard_condition(rclcpp::GuardCondition::WeakPtr weak_guard_condition)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(guard_condition_mutex_);
|
||||
auto guard_condition = weak_guard_condition.lock();
|
||||
if (guard_condition && notify_guard_conditions_.count(weak_guard_condition) == 0) {
|
||||
notify_guard_conditions_.insert(weak_guard_condition);
|
||||
if (on_ready_callback_) {
|
||||
guard_condition->set_on_trigger_callback(on_ready_callback_);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
ExecutorNotifyWaitable::remove_guard_condition(rclcpp::GuardCondition::WeakPtr weak_guard_condition)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(guard_condition_mutex_);
|
||||
if (notify_guard_conditions_.count(weak_guard_condition) != 0) {
|
||||
notify_guard_conditions_.erase(weak_guard_condition);
|
||||
auto guard_condition = weak_guard_condition.lock();
|
||||
// If this notify waitable doesn't have an on_ready_callback, then there's nothing to unset
|
||||
if (guard_condition && on_ready_callback_) {
|
||||
guard_condition->set_on_trigger_callback(nullptr);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
size_t
|
||||
ExecutorNotifyWaitable::get_number_of_ready_guard_conditions()
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(guard_condition_mutex_);
|
||||
return notify_guard_conditions_.size();
|
||||
}
|
||||
|
||||
} // namespace executors
|
||||
} // namespace rclcpp
|
||||
@@ -99,19 +99,6 @@ MultiThreadedExecutor::run(size_t this_thread_number)
|
||||
|
||||
execute_any_executable(any_exec);
|
||||
|
||||
if (any_exec.callback_group &&
|
||||
any_exec.callback_group->type() == CallbackGroupType::MutuallyExclusive &&
|
||||
any_exec.callback_group->size() > 1)
|
||||
{
|
||||
try {
|
||||
interrupt_guard_condition_->trigger();
|
||||
} catch (const rclcpp::exceptions::RCLError & ex) {
|
||||
throw std::runtime_error(
|
||||
std::string(
|
||||
"Failed to trigger guard condition on callback group change: ") + ex.what());
|
||||
}
|
||||
}
|
||||
|
||||
// Clear the callback_group to prevent the AnyExecutable destructor from
|
||||
// resetting the callback group `can_be_taken_from`
|
||||
any_exec.callback_group.reset();
|
||||
|
||||
@@ -0,0 +1,524 @@
|
||||
// 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.
|
||||
|
||||
#include "rclcpp/executors/static_executor_entities_collector.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <memory>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "rclcpp/memory_strategy.hpp"
|
||||
#include "rclcpp/executors/static_single_threaded_executor.hpp"
|
||||
#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp"
|
||||
|
||||
using rclcpp::executors::StaticExecutorEntitiesCollector;
|
||||
|
||||
StaticExecutorEntitiesCollector::~StaticExecutorEntitiesCollector()
|
||||
{
|
||||
// Disassociate all callback groups and thus nodes.
|
||||
for (const auto & pair : weak_groups_associated_with_executor_to_nodes_) {
|
||||
auto group = pair.first.lock();
|
||||
if (group) {
|
||||
std::atomic_bool & has_executor = group->get_associated_with_executor_atomic();
|
||||
has_executor.store(false);
|
||||
}
|
||||
}
|
||||
for (const auto & pair : weak_groups_to_nodes_associated_with_executor_) {
|
||||
auto group = pair.first.lock();
|
||||
if (group) {
|
||||
std::atomic_bool & has_executor = group->get_associated_with_executor_atomic();
|
||||
has_executor.store(false);
|
||||
}
|
||||
}
|
||||
// Disassociate all nodes
|
||||
for (const auto & weak_node : weak_nodes_) {
|
||||
auto node = weak_node.lock();
|
||||
if (node) {
|
||||
std::atomic_bool & has_executor = node->get_associated_with_executor_atomic();
|
||||
has_executor.store(false);
|
||||
}
|
||||
}
|
||||
weak_groups_associated_with_executor_to_nodes_.clear();
|
||||
weak_groups_to_nodes_associated_with_executor_.clear();
|
||||
exec_list_.clear();
|
||||
weak_nodes_.clear();
|
||||
weak_nodes_to_guard_conditions_.clear();
|
||||
}
|
||||
|
||||
void
|
||||
StaticExecutorEntitiesCollector::init(
|
||||
rcl_wait_set_t * p_wait_set,
|
||||
rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy)
|
||||
{
|
||||
// Empty initialize executable list
|
||||
exec_list_ = rclcpp::experimental::ExecutableList();
|
||||
// Get executor's wait_set_ pointer
|
||||
p_wait_set_ = p_wait_set;
|
||||
// Get executor's memory strategy ptr
|
||||
if (memory_strategy == nullptr) {
|
||||
throw std::runtime_error("Received NULL memory strategy in executor waitable.");
|
||||
}
|
||||
memory_strategy_ = memory_strategy;
|
||||
|
||||
// Get memory strategy and executable list. Prepare wait_set_
|
||||
std::shared_ptr<void> shared_ptr;
|
||||
execute(shared_ptr);
|
||||
|
||||
// The entities collector is now initialized
|
||||
initialized_ = true;
|
||||
}
|
||||
|
||||
void
|
||||
StaticExecutorEntitiesCollector::fini()
|
||||
{
|
||||
memory_strategy_->clear_handles();
|
||||
exec_list_.clear();
|
||||
}
|
||||
|
||||
std::shared_ptr<void>
|
||||
StaticExecutorEntitiesCollector::take_data()
|
||||
{
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
void
|
||||
StaticExecutorEntitiesCollector::execute(std::shared_ptr<void> & data)
|
||||
{
|
||||
(void) data;
|
||||
// Fill memory strategy with entities coming from weak_nodes_
|
||||
fill_memory_strategy();
|
||||
// Fill exec_list_ with entities coming from weak_nodes_ (same as memory strategy)
|
||||
fill_executable_list();
|
||||
// Resize the wait_set_ based on memory_strategy handles (rcl_wait_set_resize)
|
||||
prepare_wait_set();
|
||||
// Add new nodes guard conditions to map
|
||||
std::lock_guard<std::mutex> guard{new_nodes_mutex_};
|
||||
for (const auto & weak_node : new_nodes_) {
|
||||
if (auto node_ptr = weak_node.lock()) {
|
||||
const auto & gc = node_ptr->get_notify_guard_condition();
|
||||
weak_nodes_to_guard_conditions_[node_ptr] = &gc;
|
||||
}
|
||||
}
|
||||
new_nodes_.clear();
|
||||
}
|
||||
|
||||
void
|
||||
StaticExecutorEntitiesCollector::fill_memory_strategy()
|
||||
{
|
||||
memory_strategy_->clear_handles();
|
||||
bool has_invalid_weak_groups_or_nodes =
|
||||
memory_strategy_->collect_entities(weak_groups_to_nodes_associated_with_executor_);
|
||||
// Clean up any invalid nodes, if they were detected
|
||||
if (has_invalid_weak_groups_or_nodes) {
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr> invalid_group_ptrs;
|
||||
for (const auto & pair : weak_groups_to_nodes_associated_with_executor_) {
|
||||
auto & weak_group_ptr = pair.first;
|
||||
auto & weak_node_ptr = pair.second;
|
||||
if (weak_group_ptr.expired() || weak_node_ptr.expired()) {
|
||||
invalid_group_ptrs.push_back(weak_group_ptr);
|
||||
}
|
||||
}
|
||||
std::for_each(
|
||||
invalid_group_ptrs.begin(), invalid_group_ptrs.end(),
|
||||
[this](rclcpp::CallbackGroup::WeakPtr group_ptr) {
|
||||
weak_groups_to_nodes_associated_with_executor_.erase(group_ptr);
|
||||
});
|
||||
}
|
||||
has_invalid_weak_groups_or_nodes =
|
||||
memory_strategy_->collect_entities(weak_groups_associated_with_executor_to_nodes_);
|
||||
// Clean up any invalid nodes, if they were detected
|
||||
if (has_invalid_weak_groups_or_nodes) {
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr> invalid_group_ptrs;
|
||||
for (const auto & pair : weak_groups_associated_with_executor_to_nodes_) {
|
||||
auto & weak_group_ptr = pair.first;
|
||||
const auto & weak_node_ptr = pair.second;
|
||||
if (weak_group_ptr.expired() || weak_node_ptr.expired()) {
|
||||
invalid_group_ptrs.push_back(weak_group_ptr);
|
||||
}
|
||||
}
|
||||
std::for_each(
|
||||
invalid_group_ptrs.begin(), invalid_group_ptrs.end(),
|
||||
[this](rclcpp::CallbackGroup::WeakPtr group_ptr) {
|
||||
weak_groups_associated_with_executor_to_nodes_.erase(group_ptr);
|
||||
});
|
||||
}
|
||||
|
||||
// Add the static executor waitable to the memory strategy
|
||||
memory_strategy_->add_waitable_handle(this->shared_from_this());
|
||||
}
|
||||
|
||||
void
|
||||
StaticExecutorEntitiesCollector::fill_executable_list()
|
||||
{
|
||||
exec_list_.clear();
|
||||
add_callback_groups_from_nodes_associated_to_executor();
|
||||
fill_executable_list_from_map(weak_groups_associated_with_executor_to_nodes_);
|
||||
fill_executable_list_from_map(weak_groups_to_nodes_associated_with_executor_);
|
||||
// Add the executor's waitable to the executable list
|
||||
exec_list_.add_waitable(shared_from_this());
|
||||
}
|
||||
void
|
||||
StaticExecutorEntitiesCollector::fill_executable_list_from_map(
|
||||
const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap &
|
||||
weak_groups_to_nodes)
|
||||
{
|
||||
for (const auto & pair : weak_groups_to_nodes) {
|
||||
auto group = pair.first.lock();
|
||||
auto node = pair.second.lock();
|
||||
if (!node || !group || !group->can_be_taken_from().load()) {
|
||||
continue;
|
||||
}
|
||||
group->find_timer_ptrs_if(
|
||||
[this](const rclcpp::TimerBase::SharedPtr & timer) {
|
||||
if (timer) {
|
||||
exec_list_.add_timer(timer);
|
||||
}
|
||||
return false;
|
||||
});
|
||||
group->find_subscription_ptrs_if(
|
||||
[this](const rclcpp::SubscriptionBase::SharedPtr & subscription) {
|
||||
if (subscription) {
|
||||
exec_list_.add_subscription(subscription);
|
||||
}
|
||||
return false;
|
||||
});
|
||||
group->find_service_ptrs_if(
|
||||
[this](const rclcpp::ServiceBase::SharedPtr & service) {
|
||||
if (service) {
|
||||
exec_list_.add_service(service);
|
||||
}
|
||||
return false;
|
||||
});
|
||||
group->find_client_ptrs_if(
|
||||
[this](const rclcpp::ClientBase::SharedPtr & client) {
|
||||
if (client) {
|
||||
exec_list_.add_client(client);
|
||||
}
|
||||
return false;
|
||||
});
|
||||
group->find_waitable_ptrs_if(
|
||||
[this](const rclcpp::Waitable::SharedPtr & waitable) {
|
||||
if (waitable) {
|
||||
exec_list_.add_waitable(waitable);
|
||||
}
|
||||
return false;
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
StaticExecutorEntitiesCollector::prepare_wait_set()
|
||||
{
|
||||
// clear wait set
|
||||
if (rcl_wait_set_clear(p_wait_set_) != RCL_RET_OK) {
|
||||
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(
|
||||
p_wait_set_, memory_strategy_->number_of_ready_subscriptions(),
|
||||
memory_strategy_->number_of_guard_conditions(), memory_strategy_->number_of_ready_timers(),
|
||||
memory_strategy_->number_of_ready_clients(), memory_strategy_->number_of_ready_services(),
|
||||
memory_strategy_->number_of_ready_events());
|
||||
|
||||
if (RCL_RET_OK != ret) {
|
||||
throw std::runtime_error(
|
||||
std::string("Couldn't resize the wait set: ") + rcl_get_error_string().str);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
StaticExecutorEntitiesCollector::refresh_wait_set(std::chrono::nanoseconds timeout)
|
||||
{
|
||||
// clear wait set (memset to '0' all wait_set_ entities
|
||||
// but keeps the wait_set_ number of entities)
|
||||
if (rcl_wait_set_clear(p_wait_set_) != RCL_RET_OK) {
|
||||
throw std::runtime_error("Couldn't clear wait set");
|
||||
}
|
||||
|
||||
if (!memory_strategy_->add_handles_to_wait_set(p_wait_set_)) {
|
||||
throw std::runtime_error("Couldn't fill wait set");
|
||||
}
|
||||
|
||||
rcl_ret_t status =
|
||||
rcl_wait(p_wait_set_, std::chrono::duration_cast<std::chrono::nanoseconds>(timeout).count());
|
||||
|
||||
if (status == RCL_RET_WAIT_SET_EMPTY) {
|
||||
RCUTILS_LOG_WARN_NAMED(
|
||||
"rclcpp",
|
||||
"empty wait set received in rcl_wait(). This should never happen.");
|
||||
} else if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT) {
|
||||
using rclcpp::exceptions::throw_from_rcl_error;
|
||||
throw_from_rcl_error(status, "rcl_wait() failed");
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
StaticExecutorEntitiesCollector::add_to_wait_set(rcl_wait_set_t * wait_set)
|
||||
{
|
||||
// Add waitable guard conditions (one for each registered node) into the wait set.
|
||||
for (const auto & pair : weak_nodes_to_guard_conditions_) {
|
||||
auto & gc = pair.second;
|
||||
detail::add_guard_condition_to_rcl_wait_set(*wait_set, *gc);
|
||||
}
|
||||
}
|
||||
|
||||
size_t StaticExecutorEntitiesCollector::get_number_of_ready_guard_conditions()
|
||||
{
|
||||
std::lock_guard<std::mutex> guard{new_nodes_mutex_};
|
||||
return weak_nodes_to_guard_conditions_.size() + new_nodes_.size();
|
||||
}
|
||||
|
||||
bool
|
||||
StaticExecutorEntitiesCollector::add_node(
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
|
||||
{
|
||||
bool is_new_node = false;
|
||||
// If the node already has an executor
|
||||
std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
|
||||
if (has_executor.exchange(true)) {
|
||||
throw std::runtime_error("Node has already been added to an executor.");
|
||||
}
|
||||
node_ptr->for_each_callback_group(
|
||||
[this, node_ptr, &is_new_node](rclcpp::CallbackGroup::SharedPtr group_ptr)
|
||||
{
|
||||
if (
|
||||
!group_ptr->get_associated_with_executor_atomic().load() &&
|
||||
group_ptr->automatically_add_to_executor_with_node())
|
||||
{
|
||||
is_new_node = (add_callback_group(
|
||||
group_ptr,
|
||||
node_ptr,
|
||||
weak_groups_to_nodes_associated_with_executor_) ||
|
||||
is_new_node);
|
||||
}
|
||||
});
|
||||
weak_nodes_.push_back(node_ptr);
|
||||
return is_new_node;
|
||||
}
|
||||
|
||||
bool
|
||||
StaticExecutorEntitiesCollector::add_callback_group(
|
||||
rclcpp::CallbackGroup::SharedPtr group_ptr,
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
|
||||
rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
|
||||
{
|
||||
// If the callback_group already has an executor
|
||||
std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic();
|
||||
if (has_executor.exchange(true)) {
|
||||
throw std::runtime_error("Callback group has already been added to an executor.");
|
||||
}
|
||||
bool is_new_node = !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_) &&
|
||||
!has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_);
|
||||
rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr;
|
||||
auto insert_info = weak_groups_to_nodes.insert(
|
||||
std::make_pair(weak_group_ptr, node_ptr));
|
||||
bool was_inserted = insert_info.second;
|
||||
if (!was_inserted) {
|
||||
throw std::runtime_error("Callback group was already added to executor.");
|
||||
}
|
||||
if (is_new_node) {
|
||||
std::lock_guard<std::mutex> guard{new_nodes_mutex_};
|
||||
new_nodes_.push_back(node_ptr);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool
|
||||
StaticExecutorEntitiesCollector::add_callback_group(
|
||||
rclcpp::CallbackGroup::SharedPtr group_ptr,
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
|
||||
{
|
||||
return add_callback_group(group_ptr, node_ptr, weak_groups_associated_with_executor_to_nodes_);
|
||||
}
|
||||
|
||||
bool
|
||||
StaticExecutorEntitiesCollector::remove_callback_group(
|
||||
rclcpp::CallbackGroup::SharedPtr group_ptr)
|
||||
{
|
||||
return this->remove_callback_group_from_map(
|
||||
group_ptr,
|
||||
weak_groups_associated_with_executor_to_nodes_);
|
||||
}
|
||||
|
||||
bool
|
||||
StaticExecutorEntitiesCollector::remove_callback_group_from_map(
|
||||
rclcpp::CallbackGroup::SharedPtr group_ptr,
|
||||
rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
|
||||
{
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr;
|
||||
rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr;
|
||||
auto iter = weak_groups_to_nodes.find(weak_group_ptr);
|
||||
if (iter != weak_groups_to_nodes.end()) {
|
||||
node_ptr = iter->second.lock();
|
||||
if (node_ptr == nullptr) {
|
||||
throw std::runtime_error("Node must not be deleted before its callback group(s).");
|
||||
}
|
||||
weak_groups_to_nodes.erase(iter);
|
||||
} else {
|
||||
throw std::runtime_error("Callback group needs to be associated with executor.");
|
||||
}
|
||||
// If the node was matched and removed, interrupt waiting.
|
||||
if (!has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_) &&
|
||||
!has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_))
|
||||
{
|
||||
rclcpp::node_interfaces::NodeBaseInterface::WeakPtr node_weak_ptr(node_ptr);
|
||||
weak_nodes_to_guard_conditions_.erase(node_weak_ptr);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool
|
||||
StaticExecutorEntitiesCollector::remove_node(
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr)
|
||||
{
|
||||
if (!node_ptr->get_associated_with_executor_atomic().load()) {
|
||||
return false;
|
||||
}
|
||||
bool node_found = false;
|
||||
auto node_it = weak_nodes_.begin();
|
||||
while (node_it != weak_nodes_.end()) {
|
||||
bool matched = (node_it->lock() == node_ptr);
|
||||
if (matched) {
|
||||
weak_nodes_.erase(node_it);
|
||||
node_found = true;
|
||||
break;
|
||||
}
|
||||
++node_it;
|
||||
}
|
||||
if (!node_found) {
|
||||
return false;
|
||||
}
|
||||
std::vector<rclcpp::CallbackGroup::SharedPtr> found_group_ptrs;
|
||||
std::for_each(
|
||||
weak_groups_to_nodes_associated_with_executor_.begin(),
|
||||
weak_groups_to_nodes_associated_with_executor_.end(),
|
||||
[&found_group_ptrs, node_ptr](std::pair<rclcpp::CallbackGroup::WeakPtr,
|
||||
rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> key_value_pair) {
|
||||
auto & weak_node_ptr = key_value_pair.second;
|
||||
auto shared_node_ptr = weak_node_ptr.lock();
|
||||
auto group_ptr = key_value_pair.first.lock();
|
||||
if (shared_node_ptr == node_ptr) {
|
||||
found_group_ptrs.push_back(group_ptr);
|
||||
}
|
||||
});
|
||||
std::for_each(
|
||||
found_group_ptrs.begin(), found_group_ptrs.end(), [this]
|
||||
(rclcpp::CallbackGroup::SharedPtr group_ptr) {
|
||||
this->remove_callback_group_from_map(
|
||||
group_ptr,
|
||||
weak_groups_to_nodes_associated_with_executor_);
|
||||
});
|
||||
std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
|
||||
has_executor.store(false);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool
|
||||
StaticExecutorEntitiesCollector::is_ready(rcl_wait_set_t * p_wait_set)
|
||||
{
|
||||
// Check wait_set guard_conditions for added/removed entities to/from a node
|
||||
for (size_t i = 0; i < p_wait_set->size_of_guard_conditions; ++i) {
|
||||
if (p_wait_set->guard_conditions[i] != NULL) {
|
||||
auto found_guard_condition = std::find_if(
|
||||
weak_nodes_to_guard_conditions_.begin(), weak_nodes_to_guard_conditions_.end(),
|
||||
[&](std::pair<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
|
||||
const GuardCondition *> pair) -> bool {
|
||||
const rcl_guard_condition_t & rcl_gc = pair.second->get_rcl_guard_condition();
|
||||
return &rcl_gc == p_wait_set->guard_conditions[i];
|
||||
});
|
||||
if (found_guard_condition != weak_nodes_to_guard_conditions_.end()) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
// None of the guard conditions triggered belong to a registered node
|
||||
return false;
|
||||
}
|
||||
|
||||
// Returns true iff the weak_groups_to_nodes map has node_ptr as the value in any of its entry.
|
||||
bool
|
||||
StaticExecutorEntitiesCollector::has_node(
|
||||
const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
|
||||
const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap &
|
||||
weak_groups_to_nodes) const
|
||||
{
|
||||
return std::find_if(
|
||||
weak_groups_to_nodes.begin(),
|
||||
weak_groups_to_nodes.end(),
|
||||
[&](const WeakCallbackGroupsToNodesMap::value_type & other) -> bool {
|
||||
auto other_ptr = other.second.lock();
|
||||
return other_ptr == node_ptr;
|
||||
}) != weak_groups_to_nodes.end();
|
||||
}
|
||||
|
||||
void
|
||||
StaticExecutorEntitiesCollector::add_callback_groups_from_nodes_associated_to_executor()
|
||||
{
|
||||
for (const auto & weak_node : weak_nodes_) {
|
||||
auto node = weak_node.lock();
|
||||
if (node) {
|
||||
node->for_each_callback_group(
|
||||
[this, node](rclcpp::CallbackGroup::SharedPtr shared_group_ptr)
|
||||
{
|
||||
if (shared_group_ptr->automatically_add_to_executor_with_node() &&
|
||||
!shared_group_ptr->get_associated_with_executor_atomic().load())
|
||||
{
|
||||
add_callback_group(
|
||||
shared_group_ptr,
|
||||
node,
|
||||
weak_groups_to_nodes_associated_with_executor_);
|
||||
}
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr>
|
||||
StaticExecutorEntitiesCollector::get_all_callback_groups()
|
||||
{
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
|
||||
for (const auto & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) {
|
||||
groups.push_back(group_node_ptr.first);
|
||||
}
|
||||
for (const auto & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) {
|
||||
groups.push_back(group_node_ptr.first);
|
||||
}
|
||||
return groups;
|
||||
}
|
||||
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr>
|
||||
StaticExecutorEntitiesCollector::get_manually_added_callback_groups()
|
||||
{
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
|
||||
for (const auto & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) {
|
||||
groups.push_back(group_node_ptr.first);
|
||||
}
|
||||
return groups;
|
||||
}
|
||||
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr>
|
||||
StaticExecutorEntitiesCollector::get_automatically_added_callback_groups_from_nodes()
|
||||
{
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
|
||||
for (const auto & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) {
|
||||
groups.push_back(group_node_ptr.first);
|
||||
}
|
||||
return groups;
|
||||
}
|
||||
@@ -12,21 +12,31 @@
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include "rclcpp/executors/executor_entities_collection.hpp"
|
||||
#include "rclcpp/executors/executor_notify_waitable.hpp"
|
||||
#include "rclcpp/executors/static_single_threaded_executor.hpp"
|
||||
|
||||
#include <chrono>
|
||||
#include <memory>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "rcpputils/scope_exit.hpp"
|
||||
|
||||
#include "rclcpp/executors/static_single_threaded_executor.hpp"
|
||||
#include "rclcpp/any_executable.hpp"
|
||||
|
||||
using rclcpp::executors::StaticSingleThreadedExecutor;
|
||||
using rclcpp::experimental::ExecutableList;
|
||||
|
||||
StaticSingleThreadedExecutor::StaticSingleThreadedExecutor(const rclcpp::ExecutorOptions & options)
|
||||
StaticSingleThreadedExecutor::StaticSingleThreadedExecutor(
|
||||
const rclcpp::ExecutorOptions & options)
|
||||
: rclcpp::Executor(options)
|
||||
{
|
||||
entities_collector_ = std::make_shared<StaticExecutorEntitiesCollector>();
|
||||
}
|
||||
|
||||
StaticSingleThreadedExecutor::~StaticSingleThreadedExecutor() {}
|
||||
StaticSingleThreadedExecutor::~StaticSingleThreadedExecutor()
|
||||
{
|
||||
if (entities_collector_->is_init()) {
|
||||
entities_collector_->fini();
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
StaticSingleThreadedExecutor::spin()
|
||||
@@ -36,25 +46,14 @@ StaticSingleThreadedExecutor::spin()
|
||||
}
|
||||
RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
|
||||
|
||||
// This is essentially the contents of the rclcpp::Executor::wait_for_work method,
|
||||
// except we need to keep the wait result to reproduce the StaticSingleThreadedExecutor
|
||||
// behavior.
|
||||
// Set memory_strategy_ and exec_list_ based on weak_nodes_
|
||||
// Prepare wait_set_ based on memory_strategy_
|
||||
entities_collector_->init(&wait_set_, memory_strategy_);
|
||||
|
||||
while (rclcpp::ok(this->context_) && spinning.load()) {
|
||||
std::deque<rclcpp::AnyExecutable> to_exec;
|
||||
|
||||
std::lock_guard<std::mutex> guard(mutex_);
|
||||
if (current_collection_.empty() || this->entities_need_rebuild_.load()) {
|
||||
this->collect_entities();
|
||||
}
|
||||
|
||||
auto wait_result = wait_set_.wait(std::chrono::nanoseconds(-1));
|
||||
if (wait_result.kind() == WaitResultKind::Empty) {
|
||||
RCUTILS_LOG_WARN_NAMED(
|
||||
"rclcpp",
|
||||
"empty wait set received in wait(). This should never happen.");
|
||||
continue;
|
||||
}
|
||||
execute_ready_executables(current_collection_, wait_result, false);
|
||||
// Refresh wait set and wait for work
|
||||
entities_collector_->refresh_wait_set();
|
||||
execute_ready_executables();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -81,6 +80,11 @@ StaticSingleThreadedExecutor::spin_all(std::chrono::nanoseconds max_duration)
|
||||
void
|
||||
StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive)
|
||||
{
|
||||
// Make sure the entities collector has been initialized
|
||||
if (!entities_collector_->is_init()) {
|
||||
entities_collector_->init(&wait_set_, memory_strategy_);
|
||||
}
|
||||
|
||||
auto start = std::chrono::steady_clock::now();
|
||||
auto max_duration_not_elapsed = [max_duration, start]() {
|
||||
if (std::chrono::nanoseconds(0) == max_duration) {
|
||||
@@ -101,21 +105,9 @@ StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_durati
|
||||
|
||||
while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) {
|
||||
// Get executables that are ready now
|
||||
std::lock_guard<std::mutex> guard(mutex_);
|
||||
if (current_collection_.empty() || this->entities_need_rebuild_.load()) {
|
||||
this->collect_entities();
|
||||
}
|
||||
|
||||
auto wait_result = wait_set_.wait(std::chrono::nanoseconds(0));
|
||||
if (wait_result.kind() == WaitResultKind::Empty) {
|
||||
RCUTILS_LOG_WARN_NAMED(
|
||||
"rclcpp",
|
||||
"empty wait set received in wait(). This should never happen.");
|
||||
continue;
|
||||
}
|
||||
|
||||
entities_collector_->refresh_wait_set(std::chrono::milliseconds::zero());
|
||||
// Execute ready executables
|
||||
bool work_available = execute_ready_executables(current_collection_, wait_result, false);
|
||||
bool work_available = execute_ready_executables();
|
||||
if (!work_available || !exhaustive) {
|
||||
break;
|
||||
}
|
||||
@@ -125,122 +117,164 @@ StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_durati
|
||||
void
|
||||
StaticSingleThreadedExecutor::spin_once_impl(std::chrono::nanoseconds timeout)
|
||||
{
|
||||
// Make sure the entities collector has been initialized
|
||||
if (!entities_collector_->is_init()) {
|
||||
entities_collector_->init(&wait_set_, memory_strategy_);
|
||||
}
|
||||
|
||||
if (rclcpp::ok(context_) && spinning.load()) {
|
||||
std::lock_guard<std::mutex> guard(mutex_);
|
||||
if (current_collection_.empty() || this->entities_need_rebuild_.load()) {
|
||||
this->collect_entities();
|
||||
}
|
||||
|
||||
auto wait_result = wait_set_.wait(std::chrono::nanoseconds(timeout));
|
||||
if (wait_result.kind() == WaitResultKind::Empty) {
|
||||
RCUTILS_LOG_WARN_NAMED(
|
||||
"rclcpp",
|
||||
"empty wait set received in wait(). This should never happen.");
|
||||
return;
|
||||
}
|
||||
|
||||
// Wait until we have a ready entity or timeout expired
|
||||
entities_collector_->refresh_wait_set(timeout);
|
||||
// Execute ready executables
|
||||
execute_ready_executables(current_collection_, wait_result, true);
|
||||
execute_ready_executables(true);
|
||||
}
|
||||
}
|
||||
|
||||
// This preserves the "scheduling semantics" of the StaticSingleThreadedExecutor
|
||||
// from the original implementation.
|
||||
bool StaticSingleThreadedExecutor::execute_ready_executables(
|
||||
const rclcpp::executors::ExecutorEntitiesCollection & collection,
|
||||
rclcpp::WaitResult<rclcpp::WaitSet> & wait_result,
|
||||
bool spin_once)
|
||||
void
|
||||
StaticSingleThreadedExecutor::add_callback_group(
|
||||
rclcpp::CallbackGroup::SharedPtr group_ptr,
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
|
||||
bool notify)
|
||||
{
|
||||
bool is_new_node = entities_collector_->add_callback_group(group_ptr, node_ptr);
|
||||
if (is_new_node && notify) {
|
||||
// Interrupt waiting to handle new node
|
||||
interrupt_guard_condition_.trigger();
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
StaticSingleThreadedExecutor::add_node(
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
|
||||
{
|
||||
bool is_new_node = entities_collector_->add_node(node_ptr);
|
||||
if (is_new_node && notify) {
|
||||
// Interrupt waiting to handle new node
|
||||
interrupt_guard_condition_.trigger();
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
StaticSingleThreadedExecutor::add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify)
|
||||
{
|
||||
this->add_node(node_ptr->get_node_base_interface(), notify);
|
||||
}
|
||||
|
||||
void
|
||||
StaticSingleThreadedExecutor::remove_callback_group(
|
||||
rclcpp::CallbackGroup::SharedPtr group_ptr, bool notify)
|
||||
{
|
||||
bool node_removed = entities_collector_->remove_callback_group(group_ptr);
|
||||
// If the node was matched and removed, interrupt waiting
|
||||
if (node_removed && notify) {
|
||||
interrupt_guard_condition_.trigger();
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
StaticSingleThreadedExecutor::remove_node(
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
|
||||
{
|
||||
bool node_removed = entities_collector_->remove_node(node_ptr);
|
||||
if (!node_removed) {
|
||||
throw std::runtime_error("Node needs to be associated with this executor.");
|
||||
}
|
||||
// If the node was matched and removed, interrupt waiting
|
||||
if (notify) {
|
||||
interrupt_guard_condition_.trigger();
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr>
|
||||
StaticSingleThreadedExecutor::get_all_callback_groups()
|
||||
{
|
||||
return entities_collector_->get_all_callback_groups();
|
||||
}
|
||||
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr>
|
||||
StaticSingleThreadedExecutor::get_manually_added_callback_groups()
|
||||
{
|
||||
return entities_collector_->get_manually_added_callback_groups();
|
||||
}
|
||||
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr>
|
||||
StaticSingleThreadedExecutor::get_automatically_added_callback_groups_from_nodes()
|
||||
{
|
||||
return entities_collector_->get_automatically_added_callback_groups_from_nodes();
|
||||
}
|
||||
|
||||
void
|
||||
StaticSingleThreadedExecutor::remove_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify)
|
||||
{
|
||||
this->remove_node(node_ptr->get_node_base_interface(), notify);
|
||||
}
|
||||
|
||||
bool
|
||||
StaticSingleThreadedExecutor::execute_ready_executables(bool spin_once)
|
||||
{
|
||||
bool any_ready_executable = false;
|
||||
|
||||
if (wait_result.kind() != rclcpp::WaitResultKind::Ready) {
|
||||
return any_ready_executable;
|
||||
// Execute all the ready subscriptions
|
||||
for (size_t i = 0; i < wait_set_.size_of_subscriptions; ++i) {
|
||||
if (i < entities_collector_->get_number_of_subscriptions()) {
|
||||
if (wait_set_.subscriptions[i]) {
|
||||
execute_subscription(entities_collector_->get_subscription(i));
|
||||
if (spin_once) {
|
||||
return true;
|
||||
}
|
||||
any_ready_executable = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
auto rcl_wait_set = wait_result.get_wait_set().get_rcl_wait_set();
|
||||
|
||||
for (size_t ii = 0; ii < rcl_wait_set.size_of_timers; ++ii) {
|
||||
if (nullptr == rcl_wait_set.timers[ii]) {continue;}
|
||||
auto entity_iter = collection.timers.find(rcl_wait_set.timers[ii]);
|
||||
if (entity_iter != collection.timers.end()) {
|
||||
auto entity = entity_iter->second.entity.lock();
|
||||
if (!entity) {
|
||||
continue;
|
||||
// Execute all the ready timers
|
||||
for (size_t i = 0; i < wait_set_.size_of_timers; ++i) {
|
||||
if (i < entities_collector_->get_number_of_timers()) {
|
||||
if (wait_set_.timers[i] && entities_collector_->get_timer(i)->is_ready()) {
|
||||
auto timer = entities_collector_->get_timer(i);
|
||||
timer->call();
|
||||
execute_timer(std::move(timer));
|
||||
if (spin_once) {
|
||||
return true;
|
||||
}
|
||||
any_ready_executable = true;
|
||||
}
|
||||
if (!entity->call()) {
|
||||
continue;
|
||||
}
|
||||
}
|
||||
// Execute all the ready services
|
||||
for (size_t i = 0; i < wait_set_.size_of_services; ++i) {
|
||||
if (i < entities_collector_->get_number_of_services()) {
|
||||
if (wait_set_.services[i]) {
|
||||
execute_service(entities_collector_->get_service(i));
|
||||
if (spin_once) {
|
||||
return true;
|
||||
}
|
||||
any_ready_executable = true;
|
||||
}
|
||||
execute_timer(entity);
|
||||
}
|
||||
}
|
||||
// Execute all the ready clients
|
||||
for (size_t i = 0; i < wait_set_.size_of_clients; ++i) {
|
||||
if (i < entities_collector_->get_number_of_clients()) {
|
||||
if (wait_set_.clients[i]) {
|
||||
execute_client(entities_collector_->get_client(i));
|
||||
if (spin_once) {
|
||||
return true;
|
||||
}
|
||||
any_ready_executable = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
// Execute all the ready waitables
|
||||
for (size_t i = 0; i < entities_collector_->get_number_of_waitables(); ++i) {
|
||||
auto waitable = entities_collector_->get_waitable(i);
|
||||
if (waitable->is_ready(&wait_set_)) {
|
||||
auto data = waitable->take_data();
|
||||
waitable->execute(data);
|
||||
if (spin_once) {
|
||||
return true;
|
||||
}
|
||||
any_ready_executable = true;
|
||||
}
|
||||
}
|
||||
|
||||
for (size_t ii = 0; ii < rcl_wait_set.size_of_subscriptions; ++ii) {
|
||||
if (nullptr == rcl_wait_set.subscriptions[ii]) {continue;}
|
||||
auto entity_iter = collection.subscriptions.find(rcl_wait_set.subscriptions[ii]);
|
||||
if (entity_iter != collection.subscriptions.end()) {
|
||||
auto entity = entity_iter->second.entity.lock();
|
||||
if (!entity) {
|
||||
continue;
|
||||
}
|
||||
execute_subscription(entity);
|
||||
if (spin_once) {
|
||||
return true;
|
||||
}
|
||||
any_ready_executable = true;
|
||||
}
|
||||
}
|
||||
|
||||
for (size_t ii = 0; ii < rcl_wait_set.size_of_services; ++ii) {
|
||||
if (nullptr == rcl_wait_set.services[ii]) {continue;}
|
||||
auto entity_iter = collection.services.find(rcl_wait_set.services[ii]);
|
||||
if (entity_iter != collection.services.end()) {
|
||||
auto entity = entity_iter->second.entity.lock();
|
||||
if (!entity) {
|
||||
continue;
|
||||
}
|
||||
execute_service(entity);
|
||||
if (spin_once) {
|
||||
return true;
|
||||
}
|
||||
any_ready_executable = true;
|
||||
}
|
||||
}
|
||||
|
||||
for (size_t ii = 0; ii < rcl_wait_set.size_of_clients; ++ii) {
|
||||
if (nullptr == rcl_wait_set.clients[ii]) {continue;}
|
||||
auto entity_iter = collection.clients.find(rcl_wait_set.clients[ii]);
|
||||
if (entity_iter != collection.clients.end()) {
|
||||
auto entity = entity_iter->second.entity.lock();
|
||||
if (!entity) {
|
||||
continue;
|
||||
}
|
||||
execute_client(entity);
|
||||
if (spin_once) {
|
||||
return true;
|
||||
}
|
||||
any_ready_executable = true;
|
||||
}
|
||||
}
|
||||
|
||||
for (auto & [handle, entry] : collection.waitables) {
|
||||
auto waitable = entry.entity.lock();
|
||||
if (!waitable) {
|
||||
continue;
|
||||
}
|
||||
if (!waitable->is_ready(&rcl_wait_set)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
auto data = waitable->take_data();
|
||||
waitable->execute(data);
|
||||
if (spin_once) {
|
||||
return true;
|
||||
}
|
||||
any_ready_executable = true;
|
||||
}
|
||||
return any_ready_executable;
|
||||
}
|
||||
|
||||
@@ -1,488 +0,0 @@
|
||||
// Copyright 2023 iRobot Corporation.
|
||||
//
|
||||
// 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/experimental/executors/events_executor/events_executor.hpp"
|
||||
|
||||
#include <memory>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "rcpputils/scope_exit.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
using rclcpp::experimental::executors::EventsExecutor;
|
||||
|
||||
EventsExecutor::EventsExecutor(
|
||||
rclcpp::experimental::executors::EventsQueue::UniquePtr events_queue,
|
||||
bool execute_timers_separate_thread,
|
||||
const rclcpp::ExecutorOptions & options)
|
||||
: rclcpp::Executor(options)
|
||||
{
|
||||
// Get ownership of the queue used to store events.
|
||||
if (!events_queue) {
|
||||
throw std::invalid_argument("events_queue can't be a null pointer");
|
||||
}
|
||||
events_queue_ = std::move(events_queue);
|
||||
|
||||
// Create timers manager
|
||||
// The timers manager can be used either to only track timers (in this case an expired
|
||||
// timer will generate an executor event and then it will be executed by the executor thread)
|
||||
// or it can also take care of executing expired timers in its dedicated thread.
|
||||
std::function<void(const rclcpp::TimerBase *)> timer_on_ready_cb = nullptr;
|
||||
if (!execute_timers_separate_thread) {
|
||||
timer_on_ready_cb = [this](const rclcpp::TimerBase * timer_id) {
|
||||
ExecutorEvent event = {timer_id, -1, ExecutorEventType::TIMER_EVENT, 1};
|
||||
this->events_queue_->enqueue(event);
|
||||
};
|
||||
}
|
||||
timers_manager_ =
|
||||
std::make_shared<rclcpp::experimental::TimersManager>(context_, timer_on_ready_cb);
|
||||
|
||||
notify_waitable_ = std::make_shared<rclcpp::executors::ExecutorNotifyWaitable>(
|
||||
[this]() {
|
||||
// This callback is invoked when:
|
||||
// - the interrupt or shutdown guard condition is triggered:
|
||||
// ---> we need to wake up the executor so that it can terminate
|
||||
// - a node or callback group guard condition is triggered:
|
||||
// ---> the entities collection is changed, we need to update callbacks
|
||||
notify_waitable_event_pushed_ = false;
|
||||
this->refresh_current_collection_from_callback_groups();
|
||||
});
|
||||
|
||||
notify_waitable_->add_guard_condition(interrupt_guard_condition_);
|
||||
notify_waitable_->add_guard_condition(shutdown_guard_condition_);
|
||||
|
||||
notify_waitable_->set_on_ready_callback(
|
||||
this->create_waitable_callback(notify_waitable_.get()));
|
||||
|
||||
auto notify_waitable_entity_id = notify_waitable_.get();
|
||||
notify_waitable_->set_on_ready_callback(
|
||||
[this, notify_waitable_entity_id](size_t num_events, int waitable_data) {
|
||||
// The notify waitable has a special callback.
|
||||
// We don't care about how many events as when we wake up the executor we are going to
|
||||
// process everything regardless.
|
||||
// For the same reason, if an event of this type has already been pushed but it has not been
|
||||
// processed yet, we avoid pushing additional events.
|
||||
(void)num_events;
|
||||
if (notify_waitable_event_pushed_.exchange(true)) {
|
||||
return;
|
||||
}
|
||||
|
||||
ExecutorEvent event =
|
||||
{notify_waitable_entity_id, waitable_data, ExecutorEventType::WAITABLE_EVENT, 1};
|
||||
this->events_queue_->enqueue(event);
|
||||
});
|
||||
|
||||
this->entities_collector_ =
|
||||
std::make_shared<rclcpp::executors::ExecutorEntitiesCollector>(notify_waitable_);
|
||||
|
||||
this->current_entities_collection_ =
|
||||
std::make_shared<rclcpp::executors::ExecutorEntitiesCollection>();
|
||||
}
|
||||
|
||||
EventsExecutor::~EventsExecutor()
|
||||
{
|
||||
spinning.store(false);
|
||||
notify_waitable_->clear_on_ready_callback();
|
||||
this->refresh_current_collection({});
|
||||
}
|
||||
|
||||
void
|
||||
EventsExecutor::spin()
|
||||
{
|
||||
if (spinning.exchange(true)) {
|
||||
throw std::runtime_error("spin() called while already spinning");
|
||||
}
|
||||
RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
|
||||
|
||||
timers_manager_->start();
|
||||
RCPPUTILS_SCOPE_EXIT(timers_manager_->stop(); );
|
||||
|
||||
while (rclcpp::ok(context_) && spinning.load()) {
|
||||
// Wait until we get an event
|
||||
ExecutorEvent event;
|
||||
bool has_event = events_queue_->dequeue(event);
|
||||
if (has_event) {
|
||||
this->execute_event(event);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
EventsExecutor::spin_some(std::chrono::nanoseconds max_duration)
|
||||
{
|
||||
return this->spin_some_impl(max_duration, false);
|
||||
}
|
||||
|
||||
void
|
||||
EventsExecutor::spin_all(std::chrono::nanoseconds max_duration)
|
||||
{
|
||||
if (max_duration <= 0ns) {
|
||||
throw std::invalid_argument("max_duration must be positive");
|
||||
}
|
||||
return this->spin_some_impl(max_duration, true);
|
||||
}
|
||||
|
||||
void
|
||||
EventsExecutor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive)
|
||||
{
|
||||
if (spinning.exchange(true)) {
|
||||
throw std::runtime_error("spin_some() called while already spinning");
|
||||
}
|
||||
|
||||
RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
|
||||
|
||||
auto start = std::chrono::steady_clock::now();
|
||||
|
||||
auto max_duration_not_elapsed = [max_duration, start]() {
|
||||
if (std::chrono::nanoseconds(0) == max_duration) {
|
||||
// told to spin forever if need be
|
||||
return true;
|
||||
} else if (std::chrono::steady_clock::now() - start < max_duration) {
|
||||
// told to spin only for some maximum amount of time
|
||||
return true;
|
||||
}
|
||||
// spun too long
|
||||
return false;
|
||||
};
|
||||
|
||||
// Get the number of events and timers ready at start
|
||||
const size_t ready_events_at_start = events_queue_->size();
|
||||
size_t executed_events = 0;
|
||||
const size_t ready_timers_at_start = timers_manager_->get_number_ready_timers();
|
||||
size_t executed_timers = 0;
|
||||
|
||||
while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) {
|
||||
// Execute first ready event from queue if exists
|
||||
if (exhaustive || (executed_events < ready_events_at_start)) {
|
||||
bool has_event = !events_queue_->empty();
|
||||
|
||||
if (has_event) {
|
||||
ExecutorEvent event;
|
||||
bool ret = events_queue_->dequeue(event, std::chrono::nanoseconds(0));
|
||||
if (ret) {
|
||||
this->execute_event(event);
|
||||
executed_events++;
|
||||
continue;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Execute first timer if it is ready
|
||||
if (exhaustive || (executed_timers < ready_timers_at_start)) {
|
||||
bool timer_executed = timers_manager_->execute_head_timer();
|
||||
if (timer_executed) {
|
||||
executed_timers++;
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
// If there's no more work available, exit
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
EventsExecutor::spin_once_impl(std::chrono::nanoseconds timeout)
|
||||
{
|
||||
// In this context a negative input timeout means no timeout
|
||||
if (timeout < 0ns) {
|
||||
timeout = std::chrono::nanoseconds::max();
|
||||
}
|
||||
|
||||
// Select the smallest between input timeout and timer timeout
|
||||
bool is_timer_timeout = false;
|
||||
auto next_timer_timeout = timers_manager_->get_head_timeout();
|
||||
if (next_timer_timeout < timeout) {
|
||||
timeout = next_timer_timeout;
|
||||
is_timer_timeout = true;
|
||||
}
|
||||
|
||||
ExecutorEvent event;
|
||||
bool has_event = events_queue_->dequeue(event, timeout);
|
||||
|
||||
// If we wake up from the wait with an event, it means that it
|
||||
// arrived before any of the timers expired.
|
||||
if (has_event) {
|
||||
this->execute_event(event);
|
||||
} else if (is_timer_timeout) {
|
||||
timers_manager_->execute_head_timer();
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
EventsExecutor::add_node(
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
|
||||
{
|
||||
// This field is unused because we don't have to wake up the executor when a node is added.
|
||||
(void) notify;
|
||||
|
||||
// Add node to entities collector
|
||||
this->entities_collector_->add_node(node_ptr);
|
||||
|
||||
this->refresh_current_collection_from_callback_groups();
|
||||
}
|
||||
|
||||
void
|
||||
EventsExecutor::add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify)
|
||||
{
|
||||
this->add_node(node_ptr->get_node_base_interface(), notify);
|
||||
}
|
||||
|
||||
void
|
||||
EventsExecutor::remove_node(
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
|
||||
{
|
||||
// This field is unused because we don't have to wake up the executor when a node is removed.
|
||||
(void)notify;
|
||||
|
||||
// Remove node from entities collector.
|
||||
// This will result in un-setting all the event callbacks from its entities.
|
||||
// After this function returns, this executor will not receive any more events associated
|
||||
// to these entities.
|
||||
this->entities_collector_->remove_node(node_ptr);
|
||||
|
||||
this->refresh_current_collection_from_callback_groups();
|
||||
}
|
||||
|
||||
void
|
||||
EventsExecutor::remove_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify)
|
||||
{
|
||||
this->remove_node(node_ptr->get_node_base_interface(), notify);
|
||||
}
|
||||
|
||||
void
|
||||
EventsExecutor::execute_event(const ExecutorEvent & event)
|
||||
{
|
||||
switch (event.type) {
|
||||
case ExecutorEventType::CLIENT_EVENT:
|
||||
{
|
||||
auto client = this->retrieve_entity(
|
||||
static_cast<const rcl_client_t *>(event.entity_key),
|
||||
current_entities_collection_->clients);
|
||||
|
||||
if (client) {
|
||||
for (size_t i = 0; i < event.num_events; i++) {
|
||||
execute_client(client);
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
case ExecutorEventType::SUBSCRIPTION_EVENT:
|
||||
{
|
||||
auto subscription = this->retrieve_entity(
|
||||
static_cast<const rcl_subscription_t *>(event.entity_key),
|
||||
current_entities_collection_->subscriptions);
|
||||
if (subscription) {
|
||||
for (size_t i = 0; i < event.num_events; i++) {
|
||||
execute_subscription(subscription);
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
case ExecutorEventType::SERVICE_EVENT:
|
||||
{
|
||||
auto service = this->retrieve_entity(
|
||||
static_cast<const rcl_service_t *>(event.entity_key),
|
||||
current_entities_collection_->services);
|
||||
|
||||
if (service) {
|
||||
for (size_t i = 0; i < event.num_events; i++) {
|
||||
execute_service(service);
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
case ExecutorEventType::TIMER_EVENT:
|
||||
{
|
||||
timers_manager_->execute_ready_timer(
|
||||
static_cast<const rclcpp::TimerBase *>(event.entity_key));
|
||||
break;
|
||||
}
|
||||
case ExecutorEventType::WAITABLE_EVENT:
|
||||
{
|
||||
auto waitable = this->retrieve_entity(
|
||||
static_cast<const rclcpp::Waitable *>(event.entity_key),
|
||||
current_entities_collection_->waitables);
|
||||
if (waitable) {
|
||||
for (size_t i = 0; i < event.num_events; i++) {
|
||||
auto data = waitable->take_data_by_entity_id(event.waitable_data);
|
||||
waitable->execute(data);
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
EventsExecutor::add_callback_group(
|
||||
rclcpp::CallbackGroup::SharedPtr group_ptr,
|
||||
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
|
||||
bool notify)
|
||||
{
|
||||
// This field is unused because we don't have to wake up
|
||||
// the executor when a callback group is added.
|
||||
(void)notify;
|
||||
(void)node_ptr;
|
||||
|
||||
this->entities_collector_->add_callback_group(group_ptr);
|
||||
|
||||
this->refresh_current_collection_from_callback_groups();
|
||||
}
|
||||
|
||||
void
|
||||
EventsExecutor::remove_callback_group(
|
||||
rclcpp::CallbackGroup::SharedPtr group_ptr, bool notify)
|
||||
{
|
||||
// This field is unused because we don't have to wake up
|
||||
// the executor when a callback group is removed.
|
||||
(void)notify;
|
||||
|
||||
this->entities_collector_->remove_callback_group(group_ptr);
|
||||
|
||||
this->refresh_current_collection_from_callback_groups();
|
||||
}
|
||||
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr>
|
||||
EventsExecutor::get_all_callback_groups()
|
||||
{
|
||||
this->entities_collector_->update_collections();
|
||||
return this->entities_collector_->get_all_callback_groups();
|
||||
}
|
||||
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr>
|
||||
EventsExecutor::get_manually_added_callback_groups()
|
||||
{
|
||||
this->entities_collector_->update_collections();
|
||||
return this->entities_collector_->get_manually_added_callback_groups();
|
||||
}
|
||||
|
||||
std::vector<rclcpp::CallbackGroup::WeakPtr>
|
||||
EventsExecutor::get_automatically_added_callback_groups_from_nodes()
|
||||
{
|
||||
this->entities_collector_->update_collections();
|
||||
return this->entities_collector_->get_automatically_added_callback_groups();
|
||||
}
|
||||
|
||||
void
|
||||
EventsExecutor::refresh_current_collection_from_callback_groups()
|
||||
{
|
||||
this->entities_collector_->update_collections();
|
||||
auto callback_groups = this->entities_collector_->get_all_callback_groups();
|
||||
rclcpp::executors::ExecutorEntitiesCollection new_collection;
|
||||
rclcpp::executors::build_entities_collection(callback_groups, new_collection);
|
||||
|
||||
// TODO(alsora): this may be implemented in a better way.
|
||||
// We need the notify waitable to be included in the executor "current_collection"
|
||||
// because we need to be able to retrieve events for it.
|
||||
// We could explicitly check for the notify waitable ID when we receive a waitable event
|
||||
// but I think that it's better if the waitable was in the collection and it could be
|
||||
// retrieved in the "standard" way.
|
||||
// To do it, we need to add the notify waitable as an entry in both the new and
|
||||
// current collections such that it's neither added or removed.
|
||||
rclcpp::CallbackGroup::WeakPtr weak_group_ptr;
|
||||
new_collection.waitables.insert(
|
||||
{
|
||||
this->notify_waitable_.get(),
|
||||
{this->notify_waitable_, weak_group_ptr}
|
||||
});
|
||||
|
||||
this->current_entities_collection_->waitables.insert(
|
||||
{
|
||||
this->notify_waitable_.get(),
|
||||
{this->notify_waitable_, weak_group_ptr}
|
||||
});
|
||||
|
||||
this->refresh_current_collection(new_collection);
|
||||
}
|
||||
|
||||
void
|
||||
EventsExecutor::refresh_current_collection(
|
||||
const rclcpp::executors::ExecutorEntitiesCollection & new_collection)
|
||||
{
|
||||
current_entities_collection_->timers.update(
|
||||
new_collection.timers,
|
||||
[this](rclcpp::TimerBase::SharedPtr timer) {timers_manager_->add_timer(timer);},
|
||||
[this](rclcpp::TimerBase::SharedPtr timer) {timers_manager_->remove_timer(timer);});
|
||||
|
||||
current_entities_collection_->subscriptions.update(
|
||||
new_collection.subscriptions,
|
||||
[this](auto subscription) {
|
||||
subscription->set_on_new_message_callback(
|
||||
this->create_entity_callback(
|
||||
subscription->get_subscription_handle().get(), ExecutorEventType::SUBSCRIPTION_EVENT));
|
||||
},
|
||||
[](auto subscription) {subscription->clear_on_new_message_callback();});
|
||||
|
||||
current_entities_collection_->clients.update(
|
||||
new_collection.clients,
|
||||
[this](auto client) {
|
||||
client->set_on_new_response_callback(
|
||||
this->create_entity_callback(
|
||||
client->get_client_handle().get(), ExecutorEventType::CLIENT_EVENT));
|
||||
},
|
||||
[](auto client) {client->clear_on_new_response_callback();});
|
||||
|
||||
current_entities_collection_->services.update(
|
||||
new_collection.services,
|
||||
[this](auto service) {
|
||||
service->set_on_new_request_callback(
|
||||
this->create_entity_callback(
|
||||
service->get_service_handle().get(), ExecutorEventType::SERVICE_EVENT));
|
||||
},
|
||||
[](auto service) {service->clear_on_new_request_callback();});
|
||||
|
||||
// DO WE NEED THIS? WE ARE NOT DOING ANYTHING WITH GUARD CONDITIONS
|
||||
/*
|
||||
current_entities_collection_->guard_conditions.update(new_collection.guard_conditions,
|
||||
[](auto guard_condition) {(void)guard_condition;},
|
||||
[](auto guard_condition) {guard_condition->set_on_trigger_callback(nullptr);});
|
||||
*/
|
||||
|
||||
current_entities_collection_->waitables.update(
|
||||
new_collection.waitables,
|
||||
[this](auto waitable) {
|
||||
waitable->set_on_ready_callback(
|
||||
this->create_waitable_callback(waitable.get()));
|
||||
},
|
||||
[](auto waitable) {waitable->clear_on_ready_callback();});
|
||||
}
|
||||
|
||||
std::function<void(size_t)>
|
||||
EventsExecutor::create_entity_callback(
|
||||
void * entity_key, ExecutorEventType event_type)
|
||||
{
|
||||
std::function<void(size_t)>
|
||||
callback = [this, entity_key, event_type](size_t num_events) {
|
||||
ExecutorEvent event = {entity_key, -1, event_type, num_events};
|
||||
this->events_queue_->enqueue(event);
|
||||
};
|
||||
return callback;
|
||||
}
|
||||
|
||||
std::function<void(size_t, int)>
|
||||
EventsExecutor::create_waitable_callback(const rclcpp::Waitable * entity_key)
|
||||
{
|
||||
std::function<void(size_t, int)>
|
||||
callback = [this, entity_key](size_t num_events, int waitable_data) {
|
||||
ExecutorEvent event =
|
||||
{entity_key, waitable_data, ExecutorEventType::WAITABLE_EVENT, num_events};
|
||||
this->events_queue_->enqueue(event);
|
||||
};
|
||||
return callback;
|
||||
}
|
||||
@@ -1,299 +0,0 @@
|
||||
// Copyright 2023 iRobot Corporation.
|
||||
//
|
||||
// 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/experimental/timers_manager.hpp"
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
#include <ctime>
|
||||
#include <iostream>
|
||||
#include <memory>
|
||||
#include <stdexcept>
|
||||
|
||||
#include "rcpputils/scope_exit.hpp"
|
||||
|
||||
using rclcpp::experimental::TimersManager;
|
||||
|
||||
TimersManager::TimersManager(
|
||||
std::shared_ptr<rclcpp::Context> context,
|
||||
std::function<void(const rclcpp::TimerBase *)> on_ready_callback)
|
||||
{
|
||||
context_ = context;
|
||||
on_ready_callback_ = on_ready_callback;
|
||||
}
|
||||
|
||||
TimersManager::~TimersManager()
|
||||
{
|
||||
// Remove all timers
|
||||
this->clear();
|
||||
|
||||
// Make sure timers thread is stopped before destroying this object
|
||||
this->stop();
|
||||
}
|
||||
|
||||
void TimersManager::add_timer(rclcpp::TimerBase::SharedPtr timer)
|
||||
{
|
||||
if (!timer) {
|
||||
throw std::invalid_argument("TimersManager::add_timer() trying to add nullptr timer");
|
||||
}
|
||||
|
||||
bool added = false;
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(timers_mutex_);
|
||||
added = weak_timers_heap_.add_timer(timer);
|
||||
timers_updated_ = timers_updated_ || added;
|
||||
}
|
||||
|
||||
timer->set_on_reset_callback(
|
||||
[this](size_t arg) {
|
||||
{
|
||||
(void)arg;
|
||||
std::unique_lock<std::mutex> lock(timers_mutex_);
|
||||
timers_updated_ = true;
|
||||
}
|
||||
timers_cv_.notify_one();
|
||||
});
|
||||
|
||||
if (added) {
|
||||
// Notify that a timer has been added
|
||||
timers_cv_.notify_one();
|
||||
}
|
||||
}
|
||||
|
||||
void TimersManager::start()
|
||||
{
|
||||
// Make sure that the thread is not already running
|
||||
if (running_.exchange(true)) {
|
||||
throw std::runtime_error("TimersManager::start() can't start timers thread as already running");
|
||||
}
|
||||
|
||||
timers_thread_ = std::thread(&TimersManager::run_timers, this);
|
||||
}
|
||||
|
||||
void TimersManager::stop()
|
||||
{
|
||||
// Lock stop() function to prevent race condition in destructor
|
||||
std::unique_lock<std::mutex> lock(stop_mutex_);
|
||||
running_ = false;
|
||||
|
||||
// Notify the timers manager thread to wake up
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(timers_mutex_);
|
||||
timers_updated_ = true;
|
||||
}
|
||||
timers_cv_.notify_one();
|
||||
|
||||
// Join timers thread if it's running
|
||||
if (timers_thread_.joinable()) {
|
||||
timers_thread_.join();
|
||||
}
|
||||
}
|
||||
|
||||
std::chrono::nanoseconds TimersManager::get_head_timeout()
|
||||
{
|
||||
// Do not allow to interfere with the thread running
|
||||
if (running_) {
|
||||
throw std::runtime_error(
|
||||
"get_head_timeout() can't be used while timers thread is running");
|
||||
}
|
||||
|
||||
std::unique_lock<std::mutex> lock(timers_mutex_);
|
||||
return this->get_head_timeout_unsafe();
|
||||
}
|
||||
|
||||
size_t TimersManager::get_number_ready_timers()
|
||||
{
|
||||
// Do not allow to interfere with the thread running
|
||||
if (running_) {
|
||||
throw std::runtime_error(
|
||||
"get_number_ready_timers() can't be used while timers thread is running");
|
||||
}
|
||||
|
||||
std::unique_lock<std::mutex> lock(timers_mutex_);
|
||||
TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();
|
||||
return locked_heap.get_number_ready_timers();
|
||||
}
|
||||
|
||||
bool TimersManager::execute_head_timer()
|
||||
{
|
||||
// Do not allow to interfere with the thread running
|
||||
if (running_) {
|
||||
throw std::runtime_error(
|
||||
"execute_head_timer() can't be used while timers thread is running");
|
||||
}
|
||||
|
||||
std::unique_lock<std::mutex> lock(timers_mutex_);
|
||||
|
||||
TimersHeap timers_heap = weak_timers_heap_.validate_and_lock();
|
||||
|
||||
// Nothing to do if we don't have any timer
|
||||
if (timers_heap.empty()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
TimerPtr head_timer = timers_heap.front();
|
||||
|
||||
const bool timer_ready = head_timer->is_ready();
|
||||
if (timer_ready) {
|
||||
// NOTE: here we always execute the timer, regardless of whether the
|
||||
// on_ready_callback is set or not.
|
||||
head_timer->call();
|
||||
head_timer->execute_callback();
|
||||
timers_heap.heapify_root();
|
||||
weak_timers_heap_.store(timers_heap);
|
||||
}
|
||||
|
||||
return timer_ready;
|
||||
}
|
||||
|
||||
void TimersManager::execute_ready_timer(const rclcpp::TimerBase * timer_id)
|
||||
{
|
||||
TimerPtr ready_timer;
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(timers_mutex_);
|
||||
ready_timer = weak_timers_heap_.get_timer(timer_id);
|
||||
}
|
||||
if (ready_timer) {
|
||||
ready_timer->execute_callback();
|
||||
}
|
||||
}
|
||||
|
||||
std::chrono::nanoseconds TimersManager::get_head_timeout_unsafe()
|
||||
{
|
||||
// If we don't have any weak pointer, then we just return maximum timeout
|
||||
if (weak_timers_heap_.empty()) {
|
||||
return std::chrono::nanoseconds::max();
|
||||
}
|
||||
// Weak heap is not empty, so try to lock the first element.
|
||||
// If it is still a valid pointer, it is guaranteed to be the correct head
|
||||
TimerPtr head_timer = weak_timers_heap_.front().lock();
|
||||
|
||||
if (!head_timer) {
|
||||
// The first element has expired, we can't make other assumptions on the heap
|
||||
// and we need to entirely validate it.
|
||||
TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();
|
||||
// NOTE: the following operations will not modify any element in the heap, so we
|
||||
// don't have to call `weak_timers_heap_.store(locked_heap)` at the end.
|
||||
|
||||
if (locked_heap.empty()) {
|
||||
return std::chrono::nanoseconds::max();
|
||||
}
|
||||
head_timer = locked_heap.front();
|
||||
}
|
||||
|
||||
return head_timer->time_until_trigger();
|
||||
}
|
||||
|
||||
void TimersManager::execute_ready_timers_unsafe()
|
||||
{
|
||||
// We start by locking the timers
|
||||
TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();
|
||||
|
||||
// Nothing to do if we don't have any timer
|
||||
if (locked_heap.empty()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Keep executing timers until they are ready and they were already ready when we started.
|
||||
// The two checks prevent this function from blocking indefinitely if the
|
||||
// time required for executing the timers is longer than their period.
|
||||
|
||||
TimerPtr head_timer = locked_heap.front();
|
||||
const size_t number_ready_timers = locked_heap.get_number_ready_timers();
|
||||
size_t executed_timers = 0;
|
||||
while (executed_timers < number_ready_timers && head_timer->is_ready()) {
|
||||
head_timer->call();
|
||||
if (on_ready_callback_) {
|
||||
on_ready_callback_(head_timer.get());
|
||||
} else {
|
||||
head_timer->execute_callback();
|
||||
}
|
||||
|
||||
executed_timers++;
|
||||
// Executing a timer will result in updating its time_until_trigger, so re-heapify
|
||||
locked_heap.heapify_root();
|
||||
// Get new head timer
|
||||
head_timer = locked_heap.front();
|
||||
}
|
||||
|
||||
// After having performed work on the locked heap we reflect the changes to weak one.
|
||||
// Timers will be already sorted the next time we need them if none went out of scope.
|
||||
weak_timers_heap_.store(locked_heap);
|
||||
}
|
||||
|
||||
void TimersManager::run_timers()
|
||||
{
|
||||
// Make sure the running flag is set to false when we exit from this function
|
||||
// to allow restarting the timers thread.
|
||||
RCPPUTILS_SCOPE_EXIT(this->running_.store(false); );
|
||||
|
||||
while (rclcpp::ok(context_) && running_) {
|
||||
// Lock mutex
|
||||
std::unique_lock<std::mutex> lock(timers_mutex_);
|
||||
|
||||
std::chrono::nanoseconds time_to_sleep = get_head_timeout_unsafe();
|
||||
|
||||
// No need to wait if a timer is already available
|
||||
if (time_to_sleep > std::chrono::nanoseconds::zero()) {
|
||||
if (time_to_sleep != std::chrono::nanoseconds::max()) {
|
||||
// Wait until timeout or notification that timers have been updated
|
||||
timers_cv_.wait_for(lock, time_to_sleep, [this]() {return timers_updated_;});
|
||||
} else {
|
||||
// Wait until notification that timers have been updated
|
||||
timers_cv_.wait(lock, [this]() {return timers_updated_;});
|
||||
}
|
||||
}
|
||||
|
||||
// Reset timers updated flag
|
||||
timers_updated_ = false;
|
||||
|
||||
// Execute timers
|
||||
this->execute_ready_timers_unsafe();
|
||||
}
|
||||
}
|
||||
|
||||
void TimersManager::clear()
|
||||
{
|
||||
{
|
||||
// Lock mutex and then clear all data structures
|
||||
std::unique_lock<std::mutex> lock(timers_mutex_);
|
||||
|
||||
TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();
|
||||
locked_heap.clear_timers_on_reset_callbacks();
|
||||
|
||||
weak_timers_heap_.clear();
|
||||
|
||||
timers_updated_ = true;
|
||||
}
|
||||
|
||||
// Notify timers thread such that it can re-compute its timeout
|
||||
timers_cv_.notify_one();
|
||||
}
|
||||
|
||||
void TimersManager::remove_timer(TimerPtr timer)
|
||||
{
|
||||
bool removed = false;
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(timers_mutex_);
|
||||
removed = weak_timers_heap_.remove_timer(timer);
|
||||
|
||||
timers_updated_ = timers_updated_ || removed;
|
||||
}
|
||||
|
||||
if (removed) {
|
||||
// Notify timers thread such that it can re-compute its timeout
|
||||
timers_cv_.notify_one();
|
||||
timer->clear_on_reset_callback();
|
||||
}
|
||||
}
|
||||
@@ -25,20 +25,17 @@
|
||||
namespace rclcpp
|
||||
{
|
||||
|
||||
std::shared_ptr<void>
|
||||
GenericSubscription::create_message()
|
||||
std::shared_ptr<void> GenericSubscription::create_message()
|
||||
{
|
||||
return create_serialized_message();
|
||||
}
|
||||
|
||||
std::shared_ptr<rclcpp::SerializedMessage>
|
||||
GenericSubscription::create_serialized_message()
|
||||
std::shared_ptr<rclcpp::SerializedMessage> GenericSubscription::create_serialized_message()
|
||||
{
|
||||
return std::make_shared<rclcpp::SerializedMessage>(0);
|
||||
}
|
||||
|
||||
void
|
||||
GenericSubscription::handle_message(
|
||||
void GenericSubscription::handle_message(
|
||||
std::shared_ptr<void> &,
|
||||
const rclcpp::MessageInfo &)
|
||||
{
|
||||
@@ -54,8 +51,7 @@ GenericSubscription::handle_serialized_message(
|
||||
callback_(message);
|
||||
}
|
||||
|
||||
void
|
||||
GenericSubscription::handle_loaned_message(
|
||||
void GenericSubscription::handle_loaned_message(
|
||||
void * message, const rclcpp::MessageInfo & message_info)
|
||||
{
|
||||
(void) message;
|
||||
@@ -64,69 +60,16 @@ GenericSubscription::handle_loaned_message(
|
||||
"handle_loaned_message is not implemented for GenericSubscription");
|
||||
}
|
||||
|
||||
void
|
||||
GenericSubscription::return_message(std::shared_ptr<void> & message)
|
||||
void GenericSubscription::return_message(std::shared_ptr<void> & message)
|
||||
{
|
||||
auto typed_message = std::static_pointer_cast<rclcpp::SerializedMessage>(message);
|
||||
return_serialized_message(typed_message);
|
||||
}
|
||||
|
||||
void
|
||||
GenericSubscription::return_serialized_message(
|
||||
void GenericSubscription::return_serialized_message(
|
||||
std::shared_ptr<rclcpp::SerializedMessage> & message)
|
||||
{
|
||||
message.reset();
|
||||
}
|
||||
|
||||
|
||||
// DYNAMIC TYPE ====================================================================================
|
||||
// TODO(methylDragon): Reorder later
|
||||
rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr
|
||||
GenericSubscription::get_shared_dynamic_message_type()
|
||||
{
|
||||
throw rclcpp::exceptions::UnimplementedError(
|
||||
"get_shared_dynamic_message_type is not implemented for GenericSubscription");
|
||||
}
|
||||
|
||||
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr
|
||||
GenericSubscription::get_shared_dynamic_message()
|
||||
{
|
||||
throw rclcpp::exceptions::UnimplementedError(
|
||||
"get_shared_dynamic_message is not implemented for GenericSubscription");
|
||||
}
|
||||
|
||||
rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr
|
||||
GenericSubscription::get_shared_dynamic_serialization_support()
|
||||
{
|
||||
throw rclcpp::exceptions::UnimplementedError(
|
||||
"get_shared_dynamic_serialization_support is not implemented for GenericSubscription");
|
||||
}
|
||||
|
||||
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr
|
||||
GenericSubscription::create_dynamic_message()
|
||||
{
|
||||
throw rclcpp::exceptions::UnimplementedError(
|
||||
"create_dynamic_message is not implemented for GenericSubscription");
|
||||
}
|
||||
|
||||
void
|
||||
GenericSubscription::return_dynamic_message(
|
||||
rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message)
|
||||
{
|
||||
(void) message;
|
||||
throw rclcpp::exceptions::UnimplementedError(
|
||||
"return_dynamic_message is not implemented for GenericSubscription");
|
||||
}
|
||||
|
||||
void
|
||||
GenericSubscription::handle_dynamic_message(
|
||||
const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message,
|
||||
const rclcpp::MessageInfo & message_info)
|
||||
{
|
||||
(void) message;
|
||||
(void) message_info;
|
||||
throw rclcpp::exceptions::UnimplementedError(
|
||||
"handle_dynamic_message is not implemented for GenericSubscription");
|
||||
}
|
||||
|
||||
} // namespace rclcpp
|
||||
|
||||
@@ -12,19 +12,14 @@
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
|
||||
#include "rcl_logging_interface/rcl_logging_interface.h"
|
||||
#include "rcl/logging_rosout.h"
|
||||
|
||||
#include "rclcpp/exceptions.hpp"
|
||||
#include "rclcpp/logger.hpp"
|
||||
#include "rclcpp/logging.hpp"
|
||||
|
||||
#include "./logging_mutex.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
|
||||
@@ -67,46 +62,6 @@ get_logging_directory()
|
||||
return path;
|
||||
}
|
||||
|
||||
Logger
|
||||
Logger::get_child(const std::string & suffix)
|
||||
{
|
||||
if (!name_) {
|
||||
return Logger();
|
||||
}
|
||||
|
||||
rcl_ret_t rcl_ret = RCL_RET_OK;
|
||||
std::shared_ptr<std::recursive_mutex> logging_mutex;
|
||||
logging_mutex = get_global_logging_mutex();
|
||||
{
|
||||
std::lock_guard<std::recursive_mutex> guard(*logging_mutex);
|
||||
rcl_ret = rcl_logging_rosout_add_sublogger((*name_).c_str(), suffix.c_str());
|
||||
if (RCL_RET_OK != rcl_ret) {
|
||||
exceptions::throw_from_rcl_error(
|
||||
rcl_ret, "failed to call rcl_logging_rosout_add_sublogger",
|
||||
rcutils_get_error_state(), rcutils_reset_error);
|
||||
}
|
||||
}
|
||||
|
||||
Logger logger(*name_ + RCUTILS_LOGGING_SEPARATOR_STRING + suffix);
|
||||
if (RCL_RET_OK == rcl_ret) {
|
||||
logger.logger_sublogger_pairname_.reset(
|
||||
new std::pair<std::string, std::string>({*name_, suffix}),
|
||||
[logging_mutex](std::pair<std::string, std::string> * logger_sublogger_pairname_ptr) {
|
||||
std::lock_guard<std::recursive_mutex> guard(*logging_mutex);
|
||||
rcl_ret_t rcl_ret = rcl_logging_rosout_remove_sublogger(
|
||||
logger_sublogger_pairname_ptr->first.c_str(),
|
||||
logger_sublogger_pairname_ptr->second.c_str());
|
||||
delete logger_sublogger_pairname_ptr;
|
||||
if (RCL_RET_OK != rcl_ret) {
|
||||
exceptions::throw_from_rcl_error(
|
||||
rcl_ret, "failed to call rcl_logging_rosout_remove_sublogger",
|
||||
rcutils_get_error_state(), rcutils_reset_error);
|
||||
}
|
||||
});
|
||||
}
|
||||
return logger;
|
||||
}
|
||||
|
||||
void
|
||||
Logger::set_level(Level level)
|
||||
{
|
||||
@@ -125,18 +80,4 @@ Logger::set_level(Level level)
|
||||
}
|
||||
}
|
||||
|
||||
Logger::Level
|
||||
Logger::get_effective_level() const
|
||||
{
|
||||
int logger_level = rcutils_logging_get_logger_effective_level(get_name());
|
||||
|
||||
if (logger_level < 0) {
|
||||
exceptions::throw_from_rcl_error(
|
||||
RCL_RET_ERROR, "Couldn't get logger level",
|
||||
rcutils_get_error_state(), rcutils_reset_error);
|
||||
}
|
||||
|
||||
return static_cast<Level>(logger_level);
|
||||
}
|
||||
|
||||
} // namespace rclcpp
|
||||
|
||||
@@ -176,8 +176,7 @@ Node::Node(
|
||||
node_topics_,
|
||||
node_graph_,
|
||||
node_services_,
|
||||
node_logging_,
|
||||
options.clock_type()
|
||||
node_logging_
|
||||
)),
|
||||
node_parameters_(new rclcpp::node_interfaces::NodeParameters(
|
||||
node_base_,
|
||||
@@ -376,7 +375,7 @@ Node::get_parameter(const std::string & name) const
|
||||
}
|
||||
|
||||
bool
|
||||
Node::get_parameter(const std::string & name, rclcpp::Parameter & parameter) const
|
||||
Node::get_parameter(const std::string & name, rclcpp::Parameter & parameter) const noexcept
|
||||
{
|
||||
return node_parameters_->get_parameter(name, parameter);
|
||||
}
|
||||
|
||||
@@ -45,7 +45,7 @@ NodeBase::NodeBase(
|
||||
node_handle_(nullptr),
|
||||
default_callback_group_(default_callback_group),
|
||||
associated_with_executor_(false),
|
||||
notify_guard_condition_(std::make_shared<rclcpp::GuardCondition>(context)),
|
||||
notify_guard_condition_(context),
|
||||
notify_guard_condition_is_valid_(false)
|
||||
{
|
||||
// Create the rcl node and store it in a shared_ptr with a custom destructor.
|
||||
@@ -132,10 +132,8 @@ NodeBase::NodeBase(
|
||||
// Create the default callback group, if needed.
|
||||
if (nullptr == default_callback_group_) {
|
||||
using rclcpp::CallbackGroupType;
|
||||
// Default callback group is mutually exclusive and automatically associated with
|
||||
// any executors that this node is added to.
|
||||
default_callback_group_ =
|
||||
NodeBase::create_callback_group(CallbackGroupType::MutuallyExclusive, true);
|
||||
NodeBase::create_callback_group(CallbackGroupType::MutuallyExclusive);
|
||||
}
|
||||
|
||||
// Indicate the notify_guard_condition is now valid.
|
||||
@@ -204,27 +202,11 @@ NodeBase::create_callback_group(
|
||||
rclcpp::CallbackGroupType group_type,
|
||||
bool automatically_add_to_executor_with_node)
|
||||
{
|
||||
auto weak_context = this->get_context()->weak_from_this();
|
||||
auto get_node_context = [weak_context]() -> rclcpp::Context::SharedPtr {
|
||||
return weak_context.lock();
|
||||
};
|
||||
|
||||
auto group = std::make_shared<rclcpp::CallbackGroup>(
|
||||
group_type,
|
||||
get_node_context,
|
||||
automatically_add_to_executor_with_node);
|
||||
std::lock_guard<std::mutex> lock(callback_groups_mutex_);
|
||||
callback_groups_.push_back(group);
|
||||
|
||||
// This guard condition is generally used to signal to this node's executor that a callback
|
||||
// group has been added that should be considered for new entities.
|
||||
// If this is creating the default callback group, then the notify guard condition won't be
|
||||
// ready or needed yet, as the node is not done being constructed and therefore cannot be added.
|
||||
// If the callback group is not automatically associated with this node's executors, then
|
||||
// triggering the guard condition is also unnecessary, it will be manually added to an exector.
|
||||
if (notify_guard_condition_is_valid_ && automatically_add_to_executor_with_node) {
|
||||
this->trigger_notify_guard_condition();
|
||||
}
|
||||
return group;
|
||||
}
|
||||
|
||||
@@ -271,29 +253,9 @@ NodeBase::get_notify_guard_condition()
|
||||
if (!notify_guard_condition_is_valid_) {
|
||||
throw std::runtime_error("failed to get notify guard condition because it is invalid");
|
||||
}
|
||||
return *notify_guard_condition_;
|
||||
}
|
||||
|
||||
rclcpp::GuardCondition::SharedPtr
|
||||
NodeBase::get_shared_notify_guard_condition()
|
||||
{
|
||||
std::lock_guard<std::recursive_mutex> notify_condition_lock(notify_guard_condition_mutex_);
|
||||
if (!notify_guard_condition_is_valid_) {
|
||||
return nullptr;
|
||||
}
|
||||
return notify_guard_condition_;
|
||||
}
|
||||
|
||||
void
|
||||
NodeBase::trigger_notify_guard_condition()
|
||||
{
|
||||
std::lock_guard<std::recursive_mutex> notify_condition_lock(notify_guard_condition_mutex_);
|
||||
if (!notify_guard_condition_is_valid_) {
|
||||
throw std::runtime_error("failed to trigger notify guard condition because it is invalid");
|
||||
}
|
||||
notify_guard_condition_->trigger();
|
||||
}
|
||||
|
||||
bool
|
||||
NodeBase::get_use_intra_process_default() const
|
||||
{
|
||||
|
||||
@@ -24,14 +24,13 @@ NodeClock::NodeClock(
|
||||
rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
|
||||
rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
|
||||
rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services,
|
||||
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
|
||||
rcl_clock_type_t clock_type)
|
||||
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging)
|
||||
: node_base_(node_base),
|
||||
node_topics_(node_topics),
|
||||
node_graph_(node_graph),
|
||||
node_services_(node_services),
|
||||
node_logging_(node_logging),
|
||||
clock_(std::make_shared<rclcpp::Clock>(clock_type))
|
||||
ros_clock_(std::make_shared<rclcpp::Clock>(RCL_ROS_TIME))
|
||||
{}
|
||||
|
||||
NodeClock::~NodeClock()
|
||||
@@ -40,11 +39,11 @@ NodeClock::~NodeClock()
|
||||
rclcpp::Clock::SharedPtr
|
||||
NodeClock::get_clock()
|
||||
{
|
||||
return clock_;
|
||||
return ros_clock_;
|
||||
}
|
||||
|
||||
rclcpp::Clock::ConstSharedPtr
|
||||
NodeClock::get_clock() const
|
||||
{
|
||||
return clock_;
|
||||
return ros_clock_;
|
||||
}
|
||||
|
||||
@@ -533,8 +533,9 @@ NodeGraph::notify_graph_change()
|
||||
}
|
||||
}
|
||||
graph_cv_.notify_all();
|
||||
auto & node_gc = node_base_->get_notify_guard_condition();
|
||||
try {
|
||||
node_base_->trigger_notify_guard_condition();
|
||||
node_gc.trigger();
|
||||
} catch (const rclcpp::exceptions::RCLError & ex) {
|
||||
throw std::runtime_error(
|
||||
std::string("failed to notify wait set on graph change: ") + ex.what());
|
||||
@@ -788,15 +789,3 @@ rclcpp::TopicEndpointInfo::qos_profile() const
|
||||
{
|
||||
return qos_profile_;
|
||||
}
|
||||
|
||||
rosidl_type_hash_t &
|
||||
rclcpp::TopicEndpointInfo::topic_type_hash()
|
||||
{
|
||||
return topic_type_hash_;
|
||||
}
|
||||
|
||||
const rosidl_type_hash_t &
|
||||
rclcpp::TopicEndpointInfo::topic_type_hash() const
|
||||
{
|
||||
return topic_type_hash_;
|
||||
}
|
||||
|
||||
@@ -651,7 +651,7 @@ NodeParameters::undeclare_parameter(const std::string & name)
|
||||
}
|
||||
if (!parameter_info->second.descriptor.dynamic_typing) {
|
||||
throw rclcpp::exceptions::InvalidParameterTypeException{
|
||||
name, "cannot undeclare a statically typed parameter"};
|
||||
name, "cannot undeclare an statically typed parameter"};
|
||||
}
|
||||
|
||||
parameters_.erase(parameter_info);
|
||||
@@ -824,7 +824,7 @@ NodeParameters::set_parameters_atomically(const std::vector<rclcpp::Parameter> &
|
||||
auto it = parameters_.find(parameter.get_name());
|
||||
if (it != parameters_.end() && rclcpp::PARAMETER_NOT_SET != it->second.value.get_type()) {
|
||||
if (!it->second.descriptor.dynamic_typing) {
|
||||
result.reason = "cannot undeclare a statically typed parameter";
|
||||
result.reason = "cannot undeclare an statically typed parameter";
|
||||
result.successful = false;
|
||||
return result;
|
||||
}
|
||||
@@ -938,7 +938,7 @@ NodeParameters::get_parameter(const std::string & name) const
|
||||
bool
|
||||
NodeParameters::get_parameter(
|
||||
const std::string & name,
|
||||
rclcpp::Parameter & parameter) const
|
||||
rclcpp::Parameter & parameter) const noexcept
|
||||
{
|
||||
std::lock_guard<std::recursive_mutex> lock(mutex_);
|
||||
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user