From 065a966f57d1ecd2065143ff00b9599b051ecd79 Mon Sep 17 00:00:00 2001 From: Ross Desmond Date: Wed, 27 Feb 2019 21:37:24 +0000 Subject: [PATCH 01/27] Add interfaces for events in memory_strategy Signed-off-by: Miaofei --- .../rclcpp/strategies/allocator_memory_strategy.hpp | 9 +++++++++ rclcpp/include/rclcpp/waitable.hpp | 11 +++++++++++ rclcpp/src/rclcpp/executor.cpp | 9 +++++---- rclcpp/src/rclcpp/waitable.cpp | 6 ++++++ 4 files changed, 31 insertions(+), 4 deletions(-) diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index ca1d2fba3c..d4055aadbd 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -430,6 +430,15 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy return number_of_services; } + size_t number_of_ready_events() const + { + size_t number_of_events = event_handles_.size(); + for (auto waitable : waitable_handles_) { + number_of_events += waitable->get_number_of_ready_events(); + } + return number_of_events; + } + size_t number_of_ready_clients() const { size_t number_of_clients = client_handles_.size(); diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index 4f48bbc238..5ff01792b7 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -61,6 +61,17 @@ class Waitable size_t get_number_of_ready_clients(); + /// Get the number of ready events + /** + * Returns a value of 0 by default. + * This should be overridden if the Waitable contains one or more events. + * \return The number of events associated with the Waitable. + */ + RCLCPP_PUBLIC + virtual + size_t + get_number_of_ready_events(); + /// Get the number of ready services /** * Returns a value of 0 by default. diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index ac837314a4..5670150235 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -1,5 +1,5 @@ -// Copyright 2015 Open Source Robotics Foundation, Inc. -// +// Copyright 2015 Open Source Robotics Foundation, Inc., +// memory_strategy_->number_of_ready_events())// // 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 @@ -60,7 +60,7 @@ Executor::Executor(const ExecutorArgs & args) // Store the context for later use. context_ = args.context; - ret = rcl_wait_set_init(&wait_set_, 0, 2, 0, 0, 0, context_->get_rcl_context().get(), allocator); + ret = rcl_wait_set_init(&wait_set_, 0, 2, 0, 0, 0, 0, context_->get_rcl_context().get(), allocator); if (RCL_RET_OK != ret) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", @@ -434,7 +434,8 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) rcl_ret_t ret = rcl_wait_set_resize( &wait_set_, memory_strategy_->number_of_ready_subscriptions(), memory_strategy_->number_of_guard_conditions(), memory_strategy_->number_of_ready_timers(), - memory_strategy_->number_of_ready_clients(), memory_strategy_->number_of_ready_services()); + 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); diff --git a/rclcpp/src/rclcpp/waitable.cpp b/rclcpp/src/rclcpp/waitable.cpp index a29b80de1d..542b10a016 100644 --- a/rclcpp/src/rclcpp/waitable.cpp +++ b/rclcpp/src/rclcpp/waitable.cpp @@ -34,6 +34,12 @@ Waitable::get_number_of_ready_clients() return 0u; } +size_t +Waitable::get_number_of_ready_events() +{ + return 0u; +} + size_t Waitable::get_number_of_ready_services() { From 6ddded4183f34a46670286d608a8afdbc21380a8 Mon Sep 17 00:00:00 2001 From: Miaofei Date: Fri, 1 Mar 2019 10:42:05 -0800 Subject: [PATCH 02/27] refactor waitables Signed-off-by: Miaofei --- rclcpp/include/rclcpp/any_executable.hpp | 12 +- rclcpp/include/rclcpp/callback_group.hpp | 10 + rclcpp/include/rclcpp/client.hpp | 38 ++- .../executors/multi_threaded_executor.hpp | 2 +- rclcpp/include/rclcpp/memory_strategy.hpp | 16 +- rclcpp/include/rclcpp/publisher.hpp | 35 ++- rclcpp/include/rclcpp/service.hpp | 39 ++- .../strategies/allocator_memory_strategy.hpp | 229 ++---------------- rclcpp/include/rclcpp/subscription.hpp | 51 +++- rclcpp/include/rclcpp/timer.hpp | 22 +- rclcpp/src/rclcpp/any_executable.cpp | 16 +- rclcpp/src/rclcpp/callback_group.cpp | 15 ++ rclcpp/src/rclcpp/client.cpp | 78 ++++++ rclcpp/src/rclcpp/executor.cpp | 44 +--- .../executors/multi_threaded_executor.cpp | 10 +- rclcpp/src/rclcpp/graph_listener.cpp | 3 +- rclcpp/src/rclcpp/publisher.cpp | 43 ++++ rclcpp/src/rclcpp/service.cpp | 78 ++++++ rclcpp/src/rclcpp/subscription.cpp | 141 ++++++++++- rclcpp/src/rclcpp/timer.cpp | 31 +++ 20 files changed, 607 insertions(+), 306 deletions(-) diff --git a/rclcpp/include/rclcpp/any_executable.hpp b/rclcpp/include/rclcpp/any_executable.hpp index 7b9bac00aa..a8741bdca8 100644 --- a/rclcpp/include/rclcpp/any_executable.hpp +++ b/rclcpp/include/rclcpp/any_executable.hpp @@ -35,18 +35,16 @@ namespace executor struct AnyExecutable { RCLCPP_PUBLIC - AnyExecutable(); + AnyExecutable() = default; RCLCPP_PUBLIC virtual ~AnyExecutable(); - // Only one of the following pointers will be set. - rclcpp::SubscriptionBase::SharedPtr subscription; - rclcpp::SubscriptionBase::SharedPtr subscription_intra_process; - rclcpp::TimerBase::SharedPtr timer; - rclcpp::ServiceBase::SharedPtr service; - rclcpp::ClientBase::SharedPtr client; + bool + has_timer() const; + rclcpp::Waitable::SharedPtr waitable; + // These are used to keep the scope on the containing items rclcpp::callback_group::CallbackGroup::SharedPtr callback_group; rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base; diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index c53bffbd7d..0a5c49cbb7 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -22,6 +22,7 @@ #include "rclcpp/client.hpp" #include "rclcpp/service.hpp" +#include "rclcpp/publisher.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/timer.hpp" #include "rclcpp/visibility_control.hpp" @@ -61,6 +62,10 @@ class CallbackGroup RCLCPP_PUBLIC explicit CallbackGroup(CallbackGroupType group_type); + RCLCPP_PUBLIC + const std::vector & + get_publisher_ptrs() const; + RCLCPP_PUBLIC const std::vector & get_subscription_ptrs() const; @@ -92,6 +97,10 @@ class CallbackGroup protected: RCLCPP_DISABLE_COPY(CallbackGroup) + RCLCPP_PUBLIC + void + add_publisher(const rclcpp::PublisherBase::SharedPtr publisher_ptr); + RCLCPP_PUBLIC void add_subscription(const rclcpp::SubscriptionBase::SharedPtr subscription_ptr); @@ -119,6 +128,7 @@ class CallbackGroup CallbackGroupType type_; // Mutex to protect the subsequent vectors of pointers. mutable std::mutex mutex_; + std::vector publisher_ptrs_; std::vector subscription_ptrs_; std::vector timer_ptrs_; std::vector service_ptrs_; diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 1f22b55c6d..083dbab223 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -27,6 +27,7 @@ #include "rcl/error_handling.h" #include "rcl/wait.h" +#include "rclcpp/waitable.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/function_traits.hpp" #include "rclcpp/macros.hpp" @@ -49,7 +50,7 @@ namespace node_interfaces class NodeBaseInterface; } // namespace node_interfaces -class ClientBase +class ClientBase : public Waitable { public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientBase) @@ -74,6 +75,34 @@ class ClientBase std::shared_ptr get_client_handle() const; + RCLCPP_PUBLIC + std::shared_ptr + get_event_handle(); + + RCLCPP_PUBLIC + std::shared_ptr + get_event_handle() const; + + RCLCPP_PUBLIC + size_t + get_number_of_ready_clients() override; + + RCLCPP_PUBLIC + size_t + get_number_of_ready_events() override; + + RCLCPP_PUBLIC + bool + add_to_wait_set(rcl_wait_set_t * wait_set) override; + + RCLCPP_PUBLIC + bool + is_ready(rcl_wait_set_t * wait_set) override; + + RCLCPP_PUBLIC + void + execute() override; + RCLCPP_PUBLIC bool service_is_ready() const; @@ -113,6 +142,13 @@ class ClientBase std::shared_ptr context_; std::shared_ptr client_handle_; + std::shared_ptr event_handle_; + + size_t wait_set_client_index_; + size_t wait_set_event_index_; + + bool client_ready_; + bool event_ready_; }; template diff --git a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp index eb41a93e11..709c71190f 100644 --- a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp @@ -79,7 +79,7 @@ class MultiThreadedExecutor : public executor::Executor bool yield_before_execute_; std::mutex scheduled_timers_mutex_; - std::set scheduled_timers_; + std::set scheduled_timers_; }; } // namespace executors diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp index 416890ed6c..429b53f90c 100644 --- a/rclcpp/include/rclcpp/memory_strategy.hpp +++ b/rclcpp/include/rclcpp/memory_strategy.hpp @@ -51,6 +51,7 @@ class RCLCPP_PUBLIC MemoryStrategy virtual size_t number_of_ready_subscriptions() const = 0; virtual size_t number_of_ready_services() const = 0; virtual size_t number_of_ready_clients() const = 0; + virtual size_t number_of_ready_events() const = 0; virtual size_t number_of_ready_timers() const = 0; virtual size_t number_of_guard_conditions() const = 0; virtual size_t number_of_waitables() const = 0; @@ -63,21 +64,6 @@ class RCLCPP_PUBLIC MemoryStrategy virtual void remove_guard_condition(const rcl_guard_condition_t * guard_condition) = 0; - virtual void - get_next_subscription( - rclcpp::executor::AnyExecutable & any_exec, - const WeakNodeVector & weak_nodes) = 0; - - virtual void - get_next_service( - rclcpp::executor::AnyExecutable & any_exec, - const WeakNodeVector & weak_nodes) = 0; - - virtual void - get_next_client( - rclcpp::executor::AnyExecutable & any_exec, - const WeakNodeVector & weak_nodes) = 0; - virtual void get_next_waitable( rclcpp::executor::AnyExecutable & any_exec, diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 4500fedab5..19d2fba4aa 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -27,13 +27,14 @@ #include "rcl/error_handling.h" #include "rcl/publisher.h" +#include "rosidl_typesupport_cpp/message_type_support.hpp" #include "rcl_interfaces/msg/intra_process_message.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/waitable.hpp" #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/allocator/allocator_deleter.hpp" #include "rclcpp/macros.hpp" -#include "rclcpp/node_interfaces/node_base_interface.hpp" -#include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -42,6 +43,7 @@ namespace rclcpp // Forward declaration is used for friend statement. namespace node_interfaces { +class NodeBaseInterface; class NodeTopicsInterface; } @@ -54,7 +56,7 @@ namespace intra_process_manager class IntraProcessManager; } -class PublisherBase +class PublisherBase : public Waitable { friend ::rclcpp::node_interfaces::NodeTopicsInterface; @@ -116,6 +118,30 @@ class PublisherBase const rcl_publisher_t * get_publisher_handle() const; + RCLCPP_PUBLIC + std::shared_ptr + get_event_handle(); + + RCLCPP_PUBLIC + std::shared_ptr + get_event_handle() const; + + RCLCPP_PUBLIC + size_t + get_number_of_ready_events() override; + + RCLCPP_PUBLIC + bool + add_to_wait_set(rcl_wait_set_t * wait_set) override; + + RCLCPP_PUBLIC + bool + is_ready(rcl_wait_set_t * wait_set) override; + + RCLCPP_PUBLIC + void + execute() override; + /// Get subscription count /** \return The number of subscriptions. */ RCLCPP_PUBLIC @@ -166,6 +192,9 @@ class PublisherBase rcl_publisher_t publisher_handle_ = rcl_get_zero_initialized_publisher(); rcl_publisher_t intra_process_publisher_handle_ = rcl_get_zero_initialized_publisher(); + std::shared_ptr event_handle_; + + size_t wait_set_event_index_; using IntraProcessManagerWeakPtr = std::weak_ptr; diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 9dc8d027b0..35262e5f05 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -24,6 +24,7 @@ #include "rcl/error_handling.h" #include "rcl/service.h" +#include "rclcpp/waitable.hpp" #include "rclcpp/any_service_callback.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/macros.hpp" @@ -37,7 +38,7 @@ namespace rclcpp { -class ServiceBase +class ServiceBase : public Waitable { public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ServiceBase) @@ -61,6 +62,34 @@ class ServiceBase std::shared_ptr get_service_handle() const; + RCLCPP_PUBLIC + std::shared_ptr + get_event_handle(); + + RCLCPP_PUBLIC + std::shared_ptr + get_event_handle() const; + + RCLCPP_PUBLIC + size_t + get_number_of_ready_services() override; + + RCLCPP_PUBLIC + size_t + get_number_of_ready_events() override; + + RCLCPP_PUBLIC + bool + add_to_wait_set(rcl_wait_set_t * wait_set) override; + + RCLCPP_PUBLIC + bool + is_ready(rcl_wait_set_t * wait_set) override; + + RCLCPP_PUBLIC + void + execute() override; + virtual std::shared_ptr create_request() = 0; virtual std::shared_ptr create_request_header() = 0; virtual void handle_request( @@ -81,7 +110,13 @@ class ServiceBase std::shared_ptr node_handle_; std::shared_ptr service_handle_; - bool owns_rcl_handle_ = true; + std::shared_ptr event_handle_; + + size_t wait_set_service_index_; + size_t wait_set_event_index_; + + bool service_ready_; + bool event_ready_; }; template diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index d4055aadbd..f22ad4fe22 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -83,10 +83,6 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy void clear_handles() { - subscription_handles_.clear(); - service_handles_.clear(); - client_handles_.clear(); - timer_handles_.clear(); waitable_handles_.clear(); } @@ -95,55 +91,12 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy // TODO(jacobperron): Check if wait set sizes are what we expect them to be? // e.g. wait_set->size_of_clients == client_handles_.size() - // Important to use subscription_handles_.size() instead of wait set's size since - // there may be more subscriptions in the wait set due to Waitables added to the end. - // The same logic applies for other entities. - for (size_t i = 0; i < subscription_handles_.size(); ++i) { - if (!wait_set->subscriptions[i]) { - subscription_handles_[i].reset(); - } - } - for (size_t i = 0; i < service_handles_.size(); ++i) { - if (!wait_set->services[i]) { - service_handles_[i].reset(); - } - } - for (size_t i = 0; i < client_handles_.size(); ++i) { - if (!wait_set->clients[i]) { - client_handles_[i].reset(); - } - } - for (size_t i = 0; i < timer_handles_.size(); ++i) { - if (!wait_set->timers[i]) { - timer_handles_[i].reset(); - } - } for (size_t i = 0; i < waitable_handles_.size(); ++i) { if (!waitable_handles_[i]->is_ready(wait_set)) { waitable_handles_[i].reset(); } } - subscription_handles_.erase( - std::remove(subscription_handles_.begin(), subscription_handles_.end(), nullptr), - subscription_handles_.end() - ); - - service_handles_.erase( - std::remove(service_handles_.begin(), service_handles_.end(), nullptr), - service_handles_.end() - ); - - client_handles_.erase( - std::remove(client_handles_.begin(), client_handles_.end(), nullptr), - client_handles_.end() - ); - - timer_handles_.erase( - std::remove(timer_handles_.begin(), timer_handles_.end(), nullptr), - timer_handles_.end() - ); - waitable_handles_.erase( std::remove(waitable_handles_.begin(), waitable_handles_.end(), nullptr), waitable_handles_.end() @@ -164,32 +117,35 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy if (!group || !group->can_be_taken_from().load()) { continue; } + + for (auto & weak_publisher : group->get_publisher_ptrs()) { + auto publisher = weak_publisher.lock(); + if (publisher) { + waitable_handles_.push_back(publisher); + } + } for (auto & weak_subscription : group->get_subscription_ptrs()) { auto subscription = weak_subscription.lock(); if (subscription) { - subscription_handles_.push_back(subscription->get_subscription_handle()); - if (subscription->get_intra_process_subscription_handle()) { - subscription_handles_.push_back( - subscription->get_intra_process_subscription_handle()); - } + waitable_handles_.push_back(subscription); } } for (auto & weak_service : group->get_service_ptrs()) { auto service = weak_service.lock(); if (service) { - service_handles_.push_back(service->get_service_handle()); + waitable_handles_.push_back(service); } } for (auto & weak_client : group->get_client_ptrs()) { auto client = weak_client.lock(); if (client) { - client_handles_.push_back(client->get_client_handle()); + waitable_handles_.push_back(client); } } for (auto & weak_timer : group->get_timer_ptrs()) { auto timer = weak_timer.lock(); if (timer) { - timer_handles_.push_back(timer->get_timer_handle()); + waitable_handles_.push_back(timer); } } for (auto & weak_waitable : group->get_waitable_ptrs()) { @@ -205,42 +161,6 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy bool add_handles_to_wait_set(rcl_wait_set_t * wait_set) { - for (auto subscription : subscription_handles_) { - if (rcl_wait_set_add_subscription(wait_set, subscription.get(), NULL) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't add subscription to wait set: %s", rcl_get_error_string().str); - return false; - } - } - - for (auto client : client_handles_) { - if (rcl_wait_set_add_client(wait_set, client.get(), NULL) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't add client to wait set: %s", rcl_get_error_string().str); - return false; - } - } - - for (auto service : service_handles_) { - if (rcl_wait_set_add_service(wait_set, service.get(), NULL) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't add service to wait set: %s", rcl_get_error_string().str); - return false; - } - } - - for (auto timer : timer_handles_) { - if (rcl_wait_set_add_timer(wait_set, timer.get(), NULL) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't add timer to wait set: %s", rcl_get_error_string().str); - return false; - } - } - for (auto guard_condition : guard_conditions_) { if (rcl_wait_set_add_guard_condition(wait_set, guard_condition, NULL) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( @@ -262,118 +182,6 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy return true; } - virtual void - get_next_subscription( - executor::AnyExecutable & any_exec, - const WeakNodeVector & weak_nodes) - { - auto it = subscription_handles_.begin(); - while (it != subscription_handles_.end()) { - auto subscription = get_subscription_by_handle(*it, weak_nodes); - if (subscription) { - // Figure out if this is for intra-process or not. - bool is_intra_process = false; - if (subscription->get_intra_process_subscription_handle()) { - is_intra_process = subscription->get_intra_process_subscription_handle() == *it; - } - // Find the group for this handle and see if it can be serviced - auto group = get_group_by_subscription(subscription, weak_nodes); - if (!group) { - // Group was not found, meaning the subscription is not valid... - // Remove it from the ready list and continue looking - it = subscription_handles_.erase(it); - continue; - } - if (!group->can_be_taken_from().load()) { - // Group is mutually exclusive and is being used, so skip it for now - // Leave it to be checked next time, but continue searching - ++it; - continue; - } - // Otherwise it is safe to set and return the any_exec - if (is_intra_process) { - any_exec.subscription_intra_process = subscription; - } else { - any_exec.subscription = subscription; - } - any_exec.callback_group = group; - any_exec.node_base = get_node_by_group(group, weak_nodes); - subscription_handles_.erase(it); - return; - } - // Else, the subscription is no longer valid, remove it and continue - it = subscription_handles_.erase(it); - } - } - - virtual void - get_next_service( - executor::AnyExecutable & any_exec, - const WeakNodeVector & weak_nodes) - { - auto it = service_handles_.begin(); - while (it != service_handles_.end()) { - auto service = get_service_by_handle(*it, weak_nodes); - if (service) { - // Find the group for this handle and see if it can be serviced - auto group = get_group_by_service(service, weak_nodes); - if (!group) { - // Group was not found, meaning the service is not valid... - // Remove it from the ready list and continue looking - it = service_handles_.erase(it); - continue; - } - if (!group->can_be_taken_from().load()) { - // Group is mutually exclusive and is being used, so skip it for now - // Leave it to be checked next time, but continue searching - ++it; - continue; - } - // Otherwise it is safe to set and return the any_exec - any_exec.service = service; - any_exec.callback_group = group; - any_exec.node_base = get_node_by_group(group, weak_nodes); - service_handles_.erase(it); - return; - } - // Else, the service is no longer valid, remove it and continue - it = service_handles_.erase(it); - } - } - - virtual void - get_next_client(executor::AnyExecutable & any_exec, const WeakNodeVector & weak_nodes) - { - auto it = client_handles_.begin(); - while (it != client_handles_.end()) { - auto client = get_client_by_handle(*it, weak_nodes); - if (client) { - // Find the group for this handle and see if it can be serviced - auto group = get_group_by_client(client, weak_nodes); - if (!group) { - // Group was not found, meaning the service is not valid... - // Remove it from the ready list and continue looking - it = client_handles_.erase(it); - continue; - } - if (!group->can_be_taken_from().load()) { - // Group is mutually exclusive and is being used, so skip it for now - // Leave it to be checked next time, but continue searching - ++it; - continue; - } - // Otherwise it is safe to set and return the any_exec - any_exec.client = client; - any_exec.callback_group = group; - any_exec.node_base = get_node_by_group(group, weak_nodes); - client_handles_.erase(it); - return; - } - // Else, the service is no longer valid, remove it and continue - it = client_handles_.erase(it); - } - } - virtual void get_next_waitable(executor::AnyExecutable & any_exec, const WeakNodeVector & weak_nodes) { @@ -414,7 +222,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy size_t number_of_ready_subscriptions() const { - size_t number_of_subscriptions = subscription_handles_.size(); + size_t number_of_subscriptions = 0; for (auto waitable : waitable_handles_) { number_of_subscriptions += waitable->get_number_of_ready_subscriptions(); } @@ -423,7 +231,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy size_t number_of_ready_services() const { - size_t number_of_services = service_handles_.size(); + size_t number_of_services = 0; for (auto waitable : waitable_handles_) { number_of_services += waitable->get_number_of_ready_services(); } @@ -432,7 +240,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy size_t number_of_ready_events() const { - size_t number_of_events = event_handles_.size(); + size_t number_of_events = 0; for (auto waitable : waitable_handles_) { number_of_events += waitable->get_number_of_ready_events(); } @@ -441,7 +249,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy size_t number_of_ready_clients() const { - size_t number_of_clients = client_handles_.size(); + size_t number_of_clients = 0; for (auto waitable : waitable_handles_) { number_of_clients += waitable->get_number_of_ready_clients(); } @@ -459,7 +267,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy size_t number_of_ready_timers() const { - size_t number_of_timers = timer_handles_.size(); + size_t number_of_timers = 0; for (auto waitable : waitable_handles_) { number_of_timers += waitable->get_number_of_ready_timers(); } @@ -477,11 +285,6 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy std::vector::template rebind_alloc>; VectorRebind guard_conditions_; - - VectorRebind> subscription_handles_; - VectorRebind> service_handles_; - VectorRebind> client_handles_; - VectorRebind> timer_handles_; VectorRebind> waitable_handles_; std::shared_ptr allocator_; diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 2a2a3e4eef..4f55cc19a8 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -29,6 +29,7 @@ #include "rcl_interfaces/msg/intra_process_message.hpp" +#include "rclcpp/waitable.hpp" #include "rclcpp/any_subscription_callback.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/expand_topic_or_service_name.hpp" @@ -57,7 +58,7 @@ class IntraProcessManager; /// Virtual base class for subscriptions. This pattern allows us to iterate over different template /// specializations of Subscription, among other things. -class SubscriptionBase +class SubscriptionBase : public Waitable { public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(SubscriptionBase) @@ -92,13 +93,41 @@ class SubscriptionBase get_subscription_handle(); RCLCPP_PUBLIC - const std::shared_ptr + std::shared_ptr get_subscription_handle() const; RCLCPP_PUBLIC - virtual const std::shared_ptr + virtual std::shared_ptr get_intra_process_subscription_handle() const; + RCLCPP_PUBLIC + std::shared_ptr + get_event_handle(); + + RCLCPP_PUBLIC + std::shared_ptr + get_event_handle() const; + + RCLCPP_PUBLIC + size_t + get_number_of_ready_subscriptions() override; + + RCLCPP_PUBLIC + size_t + get_number_of_ready_events() override; + + RCLCPP_PUBLIC + bool + add_to_wait_set(rcl_wait_set_t * wait_set) override; + + RCLCPP_PUBLIC + bool + is_ready(rcl_wait_set_t * wait_set) override; + + RCLCPP_PUBLIC + void + execute() override; + /// Borrow a new message. /** \return Shared pointer to the fresh message. */ virtual std::shared_ptr @@ -145,10 +174,20 @@ class SubscriptionBase get_publisher_count() const; protected: - std::shared_ptr intra_process_subscription_handle_; - std::shared_ptr subscription_handle_; std::shared_ptr node_handle_; + std::shared_ptr subscription_handle_; + std::shared_ptr intra_process_subscription_handle_; + std::shared_ptr event_handle_; + + size_t wait_set_subscription_index_; + size_t wait_set_intra_process_subscription_index_; + size_t wait_set_event_index_; + + bool subscription_ready_; + bool intra_process_subscription_ready_; + bool event_ready_; + using IntraProcessManagerWeakPtr = std::weak_ptr; bool use_intra_process_; @@ -331,7 +370,7 @@ class Subscription : public SubscriptionBase } /// Implemenation detail. - const std::shared_ptr + std::shared_ptr get_intra_process_subscription_handle() const { if (!get_intra_process_message_callback_) { diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index 34eac08348..2f47d206e1 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -24,6 +24,7 @@ #include #include "rclcpp/clock.hpp" +#include "rclcpp/waitable.hpp" #include "rclcpp/context.hpp" #include "rclcpp/function_traits.hpp" #include "rclcpp/macros.hpp" @@ -40,7 +41,7 @@ namespace rclcpp { -class TimerBase +class TimerBase : public Waitable { public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(TimerBase) @@ -70,6 +71,22 @@ class TimerBase std::shared_ptr get_timer_handle(); + RCLCPP_PUBLIC + size_t + get_number_of_ready_timers() override; + + RCLCPP_PUBLIC + bool + add_to_wait_set(rcl_wait_set_t * wait_set) override; + + RCLCPP_PUBLIC + bool + is_ready(rcl_wait_set_t * wait_set) override; + + RCLCPP_PUBLIC + void + execute() override; + /// Check how long the timer has until its next scheduled callback. /** \return A std::chrono::duration representing the relative time until the next callback. */ RCLCPP_PUBLIC @@ -91,7 +108,10 @@ class TimerBase protected: Clock::SharedPtr clock_; + std::shared_ptr timer_handle_; + size_t wait_set_timer_index_; + bool timer_expired_; }; diff --git a/rclcpp/src/rclcpp/any_executable.cpp b/rclcpp/src/rclcpp/any_executable.cpp index 256033bbad..ee9ce7ff9f 100644 --- a/rclcpp/src/rclcpp/any_executable.cpp +++ b/rclcpp/src/rclcpp/any_executable.cpp @@ -16,16 +16,6 @@ using rclcpp::executor::AnyExecutable; -AnyExecutable::AnyExecutable() -: subscription(nullptr), - subscription_intra_process(nullptr), - timer(nullptr), - service(nullptr), - client(nullptr), - callback_group(nullptr), - node_base(nullptr) -{} - AnyExecutable::~AnyExecutable() { // Make sure that discarded (taken but not executed) AnyExecutable's have @@ -35,3 +25,9 @@ AnyExecutable::~AnyExecutable() callback_group->can_be_taken_from().store(true); } } + +bool +AnyExecutable::has_timer() const +{ + return (waitable->get_number_of_ready_timers() > 0); +} diff --git a/rclcpp/src/rclcpp/callback_group.cpp b/rclcpp/src/rclcpp/callback_group.cpp index b812afc516..e269a6120b 100644 --- a/rclcpp/src/rclcpp/callback_group.cpp +++ b/rclcpp/src/rclcpp/callback_group.cpp @@ -23,6 +23,13 @@ CallbackGroup::CallbackGroup(CallbackGroupType group_type) : type_(group_type), can_be_taken_from_(true) {} +const std::vector & +CallbackGroup::get_publisher_ptrs() const +{ + std::lock_guard lock(mutex_); + return publisher_ptrs_; +} + const std::vector & CallbackGroup::get_subscription_ptrs() const { @@ -70,6 +77,14 @@ CallbackGroup::type() const return type_; } +void +CallbackGroup::add_publisher( + const rclcpp::PublisherBase::SharedPtr publisher_ptr) +{ + std::lock_guard lock(mutex_); + publisher_ptrs_.push_back(publisher_ptr); +} + void CallbackGroup::add_subscription( const rclcpp::SubscriptionBase::SharedPtr subscription_ptr) diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index e7841542d3..570cb25233 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -88,6 +88,84 @@ ClientBase::get_client_handle() const return client_handle_; } +std::shared_ptr +ClientBase::get_event_handle() +{ + return event_handle_; +} + +std::shared_ptr +ClientBase::get_event_handle() const +{ + return event_handle_; +} + +size_t +ClientBase::get_number_of_ready_clients() +{ + return 1; +} + +size_t +ClientBase::get_number_of_ready_events() +{ + return 1; +} + +bool +ClientBase::add_to_wait_set(rcl_wait_set_t * wait_set) +{ + if (rcl_wait_set_add_client(wait_set, client_handle_.get(), &wait_set_client_index_) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't add client to wait set: %s", rcl_get_error_string().str); + return false; + } + + if (rcl_wait_set_add_event(wait_set, event_handle_.get(), &wait_set_event_index_) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't add client event to wait set: %s", rcl_get_error_string().str); + return false; + } + + return true; +} + +bool +ClientBase::is_ready(rcl_wait_set_t * wait_set) +{ + client_ready_ = (wait_set->clients[wait_set_client_index_] == client_handle_.get()); + event_ready_ = (wait_set->events[wait_set_event_index_] == event_handle_.get()); + return client_ready_ || event_ready_; +} + +void +ClientBase::execute() +{ + if (client_ready_) { + auto request_header = create_request_header(); + std::shared_ptr response = create_response(); + rcl_ret_t status = rcl_take_response( + get_client_handle().get(), + request_header.get(), + response.get()); + if (status == RCL_RET_OK) { + handle_response(request_header, response); + } else if (status != RCL_RET_CLIENT_TAKE_FAILED) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "take response failed for client of service '%s': %s", + get_service_name(), rcl_get_error_string().str); + rcl_reset_error(); + } + } + + if (event_ready_) { + // rcl_take_event(); + } +} + bool ClientBase::service_is_ready() const { diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 5670150235..72d4487eb4 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -1,5 +1,5 @@ -// Copyright 2015 Open Source Robotics Foundation, Inc., -// memory_strategy_->number_of_ready_events())// +// 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 @@ -266,21 +266,7 @@ Executor::execute_any_executable(AnyExecutable & any_exec) if (!spinning.load()) { return; } - if (any_exec.timer) { - execute_timer(any_exec.timer); - } - if (any_exec.subscription) { - execute_subscription(any_exec.subscription); - } - if (any_exec.subscription_intra_process) { - execute_intra_process_subscription(any_exec.subscription_intra_process); - } - if (any_exec.service) { - execute_service(any_exec.service); - } - if (any_exec.client) { - execute_client(any_exec.client); - } + if (any_exec.waitable) { any_exec.waitable->execute(); } @@ -521,7 +507,7 @@ Executor::get_next_timer(AnyExecutable & any_exec) for (auto & timer_ref : group->get_timer_ptrs()) { auto timer = timer_ref.lock(); if (timer && timer->is_ready()) { - any_exec.timer = timer; + any_exec.waitable = timer; any_exec.callback_group = group; node = get_node_by_group(group); return; @@ -534,32 +520,12 @@ Executor::get_next_timer(AnyExecutable & any_exec) bool Executor::get_next_ready_executable(AnyExecutable & any_executable) { - // Check the timers to see if there are any that are ready, if so return - get_next_timer(any_executable); - if (any_executable.timer) { - return true; - } - // Check the subscriptions to see if there are any that are ready - memory_strategy_->get_next_subscription(any_executable, weak_nodes_); - if (any_executable.subscription || any_executable.subscription_intra_process) { - return true; - } - // Check the services to see if there are any that are ready - memory_strategy_->get_next_service(any_executable, weak_nodes_); - if (any_executable.service) { - return true; - } - // Check the clients to see if there are any that are ready - memory_strategy_->get_next_client(any_executable, weak_nodes_); - if (any_executable.client) { - return true; - } // Check the waitables to see if there are any that are ready memory_strategy_->get_next_waitable(any_executable, weak_nodes_); if (any_executable.waitable) { return true; } - // If there is no ready executable, return a null ptr + // If there is no ready executable, return false return false; } diff --git a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp index 08318534b2..b0d916a8c7 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -80,13 +80,13 @@ MultiThreadedExecutor::run(size_t) if (!get_next_executable(any_exec)) { continue; } - if (any_exec.timer) { + if (any_exec.has_timer()) { // Guard against multiple threads getting the same timer. std::lock_guard lock(scheduled_timers_mutex_); - if (scheduled_timers_.count(any_exec.timer) != 0) { + if (scheduled_timers_.count(any_exec.waitable) != 0) { continue; } - scheduled_timers_.insert(any_exec.timer); + scheduled_timers_.insert(any_exec.waitable); } } if (yield_before_execute_) { @@ -95,9 +95,9 @@ MultiThreadedExecutor::run(size_t) execute_any_executable(any_exec); - if (any_exec.timer) { + if (any_exec.has_timer()) { std::lock_guard lock(scheduled_timers_mutex_); - auto it = scheduled_timers_.find(any_exec.timer); + auto it = scheduled_timers_.find(any_exec.waitable); if (it != scheduled_timers_.end()) { scheduled_timers_.erase(it); } diff --git a/rclcpp/src/rclcpp/graph_listener.cpp b/rclcpp/src/rclcpp/graph_listener.cpp index 1a181898c4..b097564333 100644 --- a/rclcpp/src/rclcpp/graph_listener.cpp +++ b/rclcpp/src/rclcpp/graph_listener.cpp @@ -79,6 +79,7 @@ GraphListener::start_if_not_started() 0, // number_of_timers 0, // number_of_clients 0, // number_of_services + 0, // number_of_events this->parent_context_->get_rcl_context().get(), rcl_get_default_allocator()); if (RCL_RET_OK != ret) { @@ -145,7 +146,7 @@ GraphListener::run_loop() const size_t node_graph_interfaces_size = node_graph_interfaces_.size(); // Add 2 for the interrupt and shutdown guard conditions if (wait_set_.size_of_guard_conditions < (node_graph_interfaces_size + 2)) { - ret = rcl_wait_set_resize(&wait_set_, 0, node_graph_interfaces_size + 2, 0, 0, 0); + ret = rcl_wait_set_resize(&wait_set_, 0, node_graph_interfaces_size + 2, 0, 0, 0, 0); if (RCL_RET_OK != ret) { throw_from_rcl_error(ret, "failed to resize wait set"); } diff --git a/rclcpp/src/rclcpp/publisher.cpp b/rclcpp/src/rclcpp/publisher.cpp index ce6ebadaee..7ca781c90a 100644 --- a/rclcpp/src/rclcpp/publisher.cpp +++ b/rclcpp/src/rclcpp/publisher.cpp @@ -154,6 +154,49 @@ PublisherBase::get_publisher_handle() const return &publisher_handle_; } +std::shared_ptr +PublisherBase::get_event_handle() +{ + return event_handle_; +} + +std::shared_ptr +PublisherBase::get_event_handle() const +{ + return event_handle_; +} + +size_t +PublisherBase::get_number_of_ready_events() +{ + return 1; +} + +bool +PublisherBase::add_to_wait_set(rcl_wait_set_t * wait_set) +{ + if (rcl_wait_set_add_event(wait_set, event_handle_.get(), &wait_set_event_index_) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't add publisher event to wait set: %s", rcl_get_error_string().str); + return false; + } + + return true; +} + +bool +PublisherBase::is_ready(rcl_wait_set_t * wait_set) +{ + return (wait_set->events[wait_set_event_index_] == event_handle_.get()); +} + +void +PublisherBase::execute() +{ + // rcl_take_event(); +} + size_t PublisherBase::get_subscription_count() const { diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index c7f8ff9449..9b428ca224 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -52,6 +52,84 @@ ServiceBase::get_service_handle() const return service_handle_; } +std::shared_ptr +ServiceBase::get_event_handle() +{ + return event_handle_; +} + +std::shared_ptr +ServiceBase::get_event_handle() const +{ + return event_handle_; +} + +size_t +ServiceBase::get_number_of_ready_services() +{ + return 1; +} + +size_t +ServiceBase::get_number_of_ready_events() +{ + return 1; +} + +bool +ServiceBase::add_to_wait_set(rcl_wait_set_t * wait_set) +{ + if (rcl_wait_set_add_service(wait_set, service_handle_.get(), &wait_set_service_index_) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't add service to wait set: %s", rcl_get_error_string().str); + return false; + } + + if (rcl_wait_set_add_event(wait_set, event_handle_.get(), &wait_set_event_index_) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't add service event to wait set: %s", rcl_get_error_string().str); + return false; + } + + return true; +} + +bool +ServiceBase::is_ready(rcl_wait_set_t * wait_set) +{ + service_ready_ = (wait_set->services[wait_set_service_index_] == service_handle_.get()); + event_ready_ = (wait_set->events[wait_set_event_index_] == event_handle_.get()); + return service_ready_ || event_ready_; +} + +void +ServiceBase::execute() +{ + if (service_ready_) { + auto request_header = create_request_header(); + std::shared_ptr request = create_request(); + rcl_ret_t status = rcl_take_request( + get_service_handle().get(), + request_header.get(), + request.get()); + if (status == RCL_RET_OK) { + handle_request(request_header, request); + } else if (status != RCL_RET_SERVICE_TAKE_FAILED) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "take request failed for server of service '%s': %s", + get_service_name(), rcl_get_error_string().str); + rcl_reset_error(); + } + } + + if (event_ready_) { + // rcl_take_event(); + } +} + rcl_node_t * ServiceBase::get_rcl_node_handle() { diff --git a/rclcpp/src/rclcpp/subscription.cpp b/rclcpp/src/rclcpp/subscription.cpp index ed26acaf17..7d4b149683 100644 --- a/rclcpp/src/rclcpp/subscription.cpp +++ b/rclcpp/src/rclcpp/subscription.cpp @@ -109,18 +109,155 @@ SubscriptionBase::get_subscription_handle() return subscription_handle_; } -const std::shared_ptr +std::shared_ptr SubscriptionBase::get_subscription_handle() const { return subscription_handle_; } -const std::shared_ptr +std::shared_ptr SubscriptionBase::get_intra_process_subscription_handle() const { return intra_process_subscription_handle_; } +std::shared_ptr +SubscriptionBase::get_event_handle() +{ + return event_handle_; +} + +std::shared_ptr +SubscriptionBase::get_event_handle() const +{ + return event_handle_; +} + +size_t +SubscriptionBase::get_number_of_ready_subscriptions() +{ + if (use_intra_process_) { + return 2; + } else { + return 1; + } +} + +size_t +SubscriptionBase::get_number_of_ready_events() +{ + return 1; +} + +bool +SubscriptionBase::add_to_wait_set(rcl_wait_set_t * wait_set) +{ + if (rcl_wait_set_add_subscription(wait_set, subscription_handle_.get(), + &wait_set_subscription_index_) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't add subscription to wait set: %s", rcl_get_error_string().str); + return false; + } + + if (use_intra_process_) { + if (rcl_wait_set_add_subscription(wait_set, intra_process_subscription_handle_.get(), + &wait_set_intra_process_subscription_index_) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't add intra process subscription to wait set: %s", rcl_get_error_string().str); + return false; + } + } + + if (rcl_wait_set_add_event(wait_set, event_handle_.get(), &wait_set_event_index_) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't add subscription event to wait set: %s", rcl_get_error_string().str); + return false; + } + + return true; +} + +bool +SubscriptionBase::is_ready(rcl_wait_set_t * wait_set) +{ + subscription_ready_ = + (wait_set->subscriptions[wait_set_subscription_index_] == subscription_handle_.get()); + intra_process_subscription_ready_ = use_intra_process_ && + (wait_set->subscriptions[wait_set_intra_process_subscription_index_] == + intra_process_subscription_handle_.get()); + event_ready_ = (wait_set->events[wait_set_event_index_] == event_handle_.get()); + return subscription_ready_ || intra_process_subscription_ready_ || event_ready_; +} + +void +SubscriptionBase::execute() +{ + if (subscription_ready_) { + rmw_message_info_t message_info; + message_info.from_intra_process = false; + + if (is_serialized()) { + auto serialized_msg = create_serialized_message(); + auto ret = rcl_take_serialized_message( + get_subscription_handle().get(), + serialized_msg.get(), &message_info); + if (RCL_RET_OK == ret) { + auto void_serialized_msg = std::static_pointer_cast(serialized_msg); + handle_message(void_serialized_msg, message_info); + } else if (RCL_RET_SUBSCRIPTION_TAKE_FAILED != ret) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "take_serialized failed for subscription on topic '%s': %s", + get_topic_name(), rcl_get_error_string().str); + rcl_reset_error(); + } + return_serialized_message(serialized_msg); + } else { + std::shared_ptr message = create_message(); + auto ret = rcl_take( + get_subscription_handle().get(), + message.get(), &message_info); + if (RCL_RET_OK == ret) { + handle_message(message, message_info); + } else if (RCL_RET_SUBSCRIPTION_TAKE_FAILED != ret) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "could not deserialize serialized message on topic '%s': %s", + get_topic_name(), rcl_get_error_string().str); + rcl_reset_error(); + } + return_message(message); + } + } + + if (intra_process_subscription_ready_) { + rcl_interfaces::msg::IntraProcessMessage ipm; + rmw_message_info_t message_info; + rcl_ret_t status = rcl_take( + get_intra_process_subscription_handle().get(), + &ipm, + &message_info); + + if (status == RCL_RET_OK) { + message_info.from_intra_process = true; + handle_intra_process_message(ipm, message_info); + } else if (status != RCL_RET_SUBSCRIPTION_TAKE_FAILED) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "take failed for intra process subscription on topic '%s': %s", + get_topic_name(), rcl_get_error_string().str); + rcl_reset_error(); + } + } + + if (event_ready_) { + // rcl_take_event(); + } +} + const rosidl_message_type_support_t & SubscriptionBase::get_message_type_support_handle() const { diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index b46b3fd423..731ff8ea5f 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -112,3 +112,34 @@ TimerBase::get_timer_handle() { return timer_handle_; } + +size_t +TimerBase::get_number_of_ready_timers() +{ + return 1; +} + +bool +TimerBase::add_to_wait_set(rcl_wait_set_t * wait_set) +{ + if (rcl_wait_set_add_timer(wait_set, timer_handle_.get(), &wait_set_timer_index_) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't add timer to wait set: %s", rcl_get_error_string().str); + return false; + } + + return true; +} + +bool +TimerBase::is_ready(rcl_wait_set_t * wait_set) +{ + return (wait_set->timers[wait_set_timer_index_] == timer_handle_.get()); +} + +void +TimerBase::execute() +{ + execute_callback(); +} From a078e8ee90d916d6efcf2b0e9159e3458f522ba0 Mon Sep 17 00:00:00 2001 From: Miaofei Date: Mon, 4 Mar 2019 14:59:59 -0800 Subject: [PATCH 03/27] add event callbacks to publisher, subscriber, client, service Signed-off-by: Miaofei --- rclcpp/include/rclcpp/client.hpp | 23 +++- rclcpp/include/rclcpp/create_publisher.hpp | 11 +- rclcpp/include/rclcpp/create_service.hpp | 6 +- rclcpp/include/rclcpp/create_subscription.hpp | 7 +- rclcpp/include/rclcpp/event.hpp | 19 ++- rclcpp/include/rclcpp/exceptions.hpp | 4 +- rclcpp/include/rclcpp/executor.hpp | 22 ---- rclcpp/include/rclcpp/node.hpp | 40 +++++-- rclcpp/include/rclcpp/node_impl.hpp | 61 ++++++---- .../rclcpp/node_interfaces/node_graph.hpp | 6 +- .../node_interfaces/node_graph_interface.hpp | 14 +-- .../rclcpp/node_interfaces/node_topics.hpp | 3 +- .../node_interfaces/node_topics_interface.hpp | 3 +- rclcpp/include/rclcpp/parameter_client.hpp | 3 +- rclcpp/include/rclcpp/publisher.hpp | 18 ++- rclcpp/include/rclcpp/publisher_factory.hpp | 12 +- rclcpp/include/rclcpp/rclcpp.hpp | 2 +- rclcpp/include/rclcpp/service.hpp | 35 +++++- rclcpp/include/rclcpp/subscription.hpp | 25 +++- .../include/rclcpp/subscription_factory.hpp | 8 +- rclcpp/include/rclcpp/time_source.hpp | 2 +- rclcpp/src/rclcpp/client.cpp | 16 +++ rclcpp/src/rclcpp/event.cpp | 8 +- rclcpp/src/rclcpp/executor.cpp | 113 ------------------ rclcpp/src/rclcpp/node.cpp | 4 +- .../src/rclcpp/node_interfaces/node_graph.cpp | 8 +- .../node_interfaces/node_parameters.cpp | 5 +- .../rclcpp/node_interfaces/node_topics.cpp | 16 ++- rclcpp/src/rclcpp/parameter_client.cpp | 18 ++- rclcpp/src/rclcpp/parameter_service.cpp | 12 +- rclcpp/src/rclcpp/publisher.cpp | 22 +++- rclcpp/src/rclcpp/service.cpp | 2 + rclcpp/src/rclcpp/subscription.cpp | 69 +++++++++-- rclcpp/src/rclcpp/time_source.cpp | 3 +- 34 files changed, 373 insertions(+), 247 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 083dbab223..721321c57e 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -122,6 +122,9 @@ class ClientBase : public Waitable virtual void handle_response( std::shared_ptr request_header, std::shared_ptr response) = 0; + virtual void + handle_event(ResourceStatusEvent event) const = 0; + protected: RCLCPP_DISABLE_COPY(ClientBase) @@ -176,12 +179,15 @@ class Client : public ClientBase rclcpp::node_interfaces::NodeBaseInterface * node_base, rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, const std::string & service_name, - rcl_client_options_t & client_options) - : ClientBase(node_base, node_graph) + rcl_client_options_t & client_options, + ResourceStatusEventCallbackType event_callback) + : ClientBase(node_base, node_graph), + event_callback_(event_callback) { using rosidl_typesupport_cpp::get_service_type_support_handle; auto service_type_support_handle = get_service_type_support_handle(); + rcl_ret_t ret = rcl_client_init( this->get_client_handle().get(), this->get_rcl_node_handle(), @@ -201,6 +207,11 @@ class Client : public ClientBase } rclcpp::exceptions::throw_from_rcl_error(ret, "could not create client"); } + + ret = rcl_client_event_init(get_event_handle().get(), get_client_handle().get()); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "could not create client event"); + } } virtual ~Client() @@ -248,6 +259,12 @@ class Client : public ClientBase callback(future); } + void + handle_event(ResourceStatusEvent event) const + { + event_callback_(event); + } + SharedFuture async_send_request(SharedRequest request) { @@ -311,6 +328,8 @@ class Client : public ClientBase std::map> pending_requests_; std::mutex pending_requests_mutex_; + + ResourceStatusEventCallbackType event_callback_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/create_publisher.hpp b/rclcpp/include/rclcpp/create_publisher.hpp index f136f33653..a1b0b5e163 100644 --- a/rclcpp/include/rclcpp/create_publisher.hpp +++ b/rclcpp/include/rclcpp/create_publisher.hpp @@ -25,12 +25,14 @@ namespace rclcpp { -template +template std::shared_ptr create_publisher( rclcpp::node_interfaces::NodeTopicsInterface * node_topics, const std::string & topic_name, const rmw_qos_profile_t & qos_profile, + EventCallbackT && event_callback, + rclcpp::callback_group::CallbackGroup::SharedPtr group, bool use_intra_process_comms, std::shared_ptr allocator) { @@ -39,10 +41,13 @@ create_publisher( auto pub = node_topics->create_publisher( topic_name, - rclcpp::create_publisher_factory(allocator), + rclcpp::create_publisher_factory( + std::forward(event_callback), allocator), publisher_options, use_intra_process_comms); - node_topics->add_publisher(pub); + + node_topics->add_publisher(pub, group); + return std::dynamic_pointer_cast(pub); } diff --git a/rclcpp/include/rclcpp/create_service.hpp b/rclcpp/include/rclcpp/create_service.hpp index ac7846239d..2f6e0a0384 100644 --- a/rclcpp/include/rclcpp/create_service.hpp +++ b/rclcpp/include/rclcpp/create_service.hpp @@ -29,7 +29,7 @@ namespace rclcpp /// Create a service with a given type. /// \internal -template +template typename rclcpp::Service::SharedPtr create_service( std::shared_ptr node_base, @@ -37,6 +37,7 @@ create_service( const std::string & service_name, CallbackT && callback, const rmw_qos_profile_t & qos_profile, + EventCallbackT && event_callback, rclcpp::callback_group::CallbackGroup::SharedPtr group) { rclcpp::AnyServiceCallback any_service_callback; @@ -47,9 +48,10 @@ create_service( auto serv = Service::make_shared( node_base->get_shared_rcl_node_handle(), - service_name, any_service_callback, service_options); + service_name, service_options, any_service_callback, event_callback); auto serv_base_ptr = std::dynamic_pointer_cast(serv); node_services->add_service(serv_base_ptr, group); + return serv; } diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index a4ea581092..dd13506f40 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -29,6 +29,7 @@ namespace rclcpp template< typename MessageT, typename CallbackT, + typename EventCallbackT, typename AllocatorT, typename CallbackMessageT, typename SubscriptionT = rclcpp::Subscription> @@ -38,6 +39,7 @@ create_subscription( const std::string & topic_name, CallbackT && callback, const rmw_qos_profile_t & qos_profile, + EventCallbackT && event_callback, rclcpp::callback_group::CallbackGroup::SharedPtr group, bool ignore_local_publications, bool use_intra_process_comms, @@ -51,8 +53,9 @@ create_subscription( subscription_options.ignore_local_publications = ignore_local_publications; auto factory = rclcpp::create_subscription_factory - ( - std::forward(callback), msg_mem_strat, allocator); + ( + std::forward(callback), std::forward(event_callback), + msg_mem_strat, allocator); auto sub = node_topics->create_subscription( topic_name, diff --git a/rclcpp/include/rclcpp/event.hpp b/rclcpp/include/rclcpp/event.hpp index 988dba29e2..5099d3091a 100644 --- a/rclcpp/include/rclcpp/event.hpp +++ b/rclcpp/include/rclcpp/event.hpp @@ -17,6 +17,7 @@ #include #include +#include #include "rclcpp/macros.hpp" #include "rclcpp/visibility_control.hpp" @@ -24,13 +25,23 @@ namespace rclcpp { -class Event +enum class ResourceStatusEvent +{ + DEADLINE_MISSED, + LIVELINESS_CHANGED, + LIFESPAN_EXPIRED +}; + +using ResourceStatusEventCallbackType = std::function; + + +class GraphEvent { public: - RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Event) + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(GraphEvent) RCLCPP_PUBLIC - Event(); + GraphEvent(); RCLCPP_PUBLIC bool @@ -45,7 +56,7 @@ class Event check_and_clear(); private: - RCLCPP_DISABLE_COPY(Event) + RCLCPP_DISABLE_COPY(GraphEvent) std::atomic_bool state_; }; diff --git a/rclcpp/include/rclcpp/exceptions.hpp b/rclcpp/include/rclcpp/exceptions.hpp index 326b51882b..63a76e5ee0 100644 --- a/rclcpp/include/rclcpp/exceptions.hpp +++ b/rclcpp/include/rclcpp/exceptions.hpp @@ -165,7 +165,7 @@ class RCLInvalidArgument : public RCLErrorBase, public std::invalid_argument RCLInvalidArgument(const RCLErrorBase & base_exc, const std::string & prefix); }; -/// Thrown when an invalid rclcpp::Event object or SharedPtr is encountered. +/// Thrown when an invalid rclcpp::GraphEvent object or SharedPtr is encountered. class InvalidEventError : public std::runtime_error { public: @@ -173,7 +173,7 @@ class InvalidEventError : public std::runtime_error : std::runtime_error("event is invalid") {} }; -/// Thrown when an unregistered rclcpp::Event is encountered where a registered one was expected. +/// Thrown when an unregistered rclcpp::GraphEvent is encountered where a registered one was expected. class EventNotRegisteredError : public std::runtime_error { public: diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 2de4bdaea4..a1bf7657ea 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -298,28 +298,6 @@ class Executor void execute_any_executable(AnyExecutable & any_exec); - RCLCPP_PUBLIC - static void - execute_subscription( - rclcpp::SubscriptionBase::SharedPtr subscription); - - RCLCPP_PUBLIC - static void - execute_intra_process_subscription( - rclcpp::SubscriptionBase::SharedPtr subscription); - - RCLCPP_PUBLIC - static void - execute_timer(rclcpp::TimerBase::SharedPtr timer); - - RCLCPP_PUBLIC - static void - execute_service(rclcpp::ServiceBase::SharedPtr service); - - RCLCPP_PUBLIC - static void - execute_client(rclcpp::ClientBase::SharedPtr client); - RCLCPP_PUBLIC void wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 1f1a24b108..2174c87f12 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -139,11 +139,16 @@ class Node : public std::enable_shared_from_this * \return Shared pointer to the created publisher. */ template< - typename MessageT, typename Alloc = std::allocator, + typename MessageT, + typename EventCallbackT = ResourceStatusEventCallbackType, + typename Alloc = std::allocator, typename PublisherT = ::rclcpp::Publisher> std::shared_ptr create_publisher( - const std::string & topic_name, size_t qos_history_depth, + const std::string & topic_name, + size_t qos_history_depth, + EventCallbackT && event_callback = {}, + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr, std::shared_ptr allocator = nullptr); /// Create and return a Publisher. @@ -154,12 +159,16 @@ class Node : public std::enable_shared_from_this * \return Shared pointer to the created publisher. */ template< - typename MessageT, typename Alloc = std::allocator, + typename MessageT, + typename EventCallbackT = ResourceStatusEventCallbackType, + typename Alloc = std::allocator, typename PublisherT = ::rclcpp::Publisher> std::shared_ptr create_publisher( const std::string & topic_name, const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default, + EventCallbackT && event_callback = {}, + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr, std::shared_ptr allocator = nullptr); /// Create and return a Subscription. @@ -180,6 +189,7 @@ class Node : public std::enable_shared_from_this template< typename MessageT, typename CallbackT, + typename EventCallbackT = ResourceStatusEventCallbackType, typename Alloc = std::allocator, typename SubscriptionT = rclcpp::Subscription< typename rclcpp::subscription_traits::has_message_type::type, Alloc>> @@ -188,6 +198,7 @@ class Node : public std::enable_shared_from_this const std::string & topic_name, CallbackT && callback, const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default, + EventCallbackT && event_callback = {}, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr, bool ignore_local_publications = false, typename rclcpp::message_memory_strategy::MessageMemoryStrategy< @@ -213,6 +224,7 @@ class Node : public std::enable_shared_from_this template< typename MessageT, typename CallbackT, + typename EventCallbackT = ResourceStatusEventCallbackType, typename Alloc = std::allocator, typename SubscriptionT = rclcpp::Subscription< typename rclcpp::subscription_traits::has_message_type::type, Alloc>> @@ -221,6 +233,7 @@ class Node : public std::enable_shared_from_this const std::string & topic_name, CallbackT && callback, size_t qos_history_depth, + EventCallbackT && event_callback = {}, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr, bool ignore_local_publications = false, typename rclcpp::message_memory_strategy::MessageMemoryStrategy< @@ -242,20 +255,25 @@ class Node : public std::enable_shared_from_this rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); /* Create and return a Client. */ - template + template typename rclcpp::Client::SharedPtr create_client( const std::string & service_name, const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, + EventCallbackT && event_callback = {}, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); /* Create and return a Service. */ - template + template< + typename ServiceT, + typename CallbackT, + typename EventCallbackT = ResourceStatusEventCallbackType> typename rclcpp::Service::SharedPtr create_service( const std::string & service_name, CallbackT && callback, const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, + EventCallbackT && event_callback = {}, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); RCLCPP_PUBLIC @@ -409,17 +427,17 @@ class Node : public std::enable_shared_from_this count_subscribers(const std::string & topic_name) const; /// Return a graph event, which will be set anytime a graph change occurs. - /* The graph Event object is a loan which must be returned. - * The Event object is scoped and therefore to return the loan just let it go + /* The graph GraphEvent object is a loan which must be returned. + * The GraphEvent object is scoped and therefore to return the loan just let it go * out of scope. */ RCLCPP_PUBLIC - rclcpp::Event::SharedPtr + rclcpp::GraphEvent::SharedPtr get_graph_event(); - /// Wait for a graph event to occur by waiting on an Event to become set. + /// Wait for a graph event to occur by waiting on an GraphEvent to become set. /** - * The given Event must be acquire through the get_graph_event() method. + * The given GraphEvent must be acquire through the get_graph_event() method. * * \throws InvalidEventError if the given event is nullptr * \throws EventNotRegisteredError if the given event was not acquired with @@ -428,7 +446,7 @@ class Node : public std::enable_shared_from_this RCLCPP_PUBLIC void wait_for_graph_change( - rclcpp::Event::SharedPtr event, + rclcpp::GraphEvent::SharedPtr event, std::chrono::nanoseconds timeout); RCLCPP_PUBLIC diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index c584d92b77..5382a79837 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -51,10 +51,13 @@ namespace rclcpp { -template +template std::shared_ptr Node::create_publisher( - const std::string & topic_name, size_t qos_history_depth, + const std::string & topic_name, + size_t qos_history_depth, + EventCallbackT && event_callback, + rclcpp::callback_group::CallbackGroup::SharedPtr group, std::shared_ptr allocator) { if (!allocator) { @@ -62,7 +65,8 @@ Node::create_publisher( } rmw_qos_profile_t qos = rmw_qos_profile_default; qos.depth = qos_history_depth; - return this->create_publisher(topic_name, qos, allocator); + return this->create_publisher( + topic_name, qos, std::forward(event_callback), group, allocator); } RCLCPP_LOCAL @@ -77,20 +81,25 @@ extend_name_with_sub_namespace(const std::string & name, const std::string & sub return name_with_sub_namespace; } -template +template std::shared_ptr Node::create_publisher( - const std::string & topic_name, const rmw_qos_profile_t & qos_profile, + const std::string & topic_name, + const rmw_qos_profile_t & qos_profile, + EventCallbackT && event_callback, + rclcpp::callback_group::CallbackGroup::SharedPtr group, std::shared_ptr allocator) { if (!allocator) { allocator = std::make_shared(); } - return rclcpp::create_publisher( + return rclcpp::create_publisher( this->node_topics_.get(), extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), qos_profile, + std::forward(event_callback), + group, this->get_node_options().use_intra_process_comms(), allocator); } @@ -98,6 +107,7 @@ Node::create_publisher( template< typename MessageT, typename CallbackT, + typename EventCallbackT, typename Alloc, typename SubscriptionT> std::shared_ptr @@ -105,6 +115,7 @@ Node::create_subscription( const std::string & topic_name, CallbackT && callback, const rmw_qos_profile_t & qos_profile, + EventCallbackT && event_callback, rclcpp::callback_group::CallbackGroup::SharedPtr group, bool ignore_local_publications, typename rclcpp::message_memory_strategy::MessageMemoryStrategy< @@ -123,21 +134,24 @@ Node::create_subscription( msg_mem_strat = MessageMemoryStrategy::create_default(); } - return rclcpp::create_subscription( - this->node_topics_.get(), - extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), - std::forward(callback), - qos_profile, - group, - ignore_local_publications, - this->get_node_options().use_intra_process_comms(), - msg_mem_strat, - allocator); + return rclcpp::create_subscription( + this->node_topics_.get(), + extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), + std::forward(callback), + qos_profile, + std::forward(event_callback), + group, + ignore_local_publications, + this->get_node_options().use_intra_process_comms(), + msg_mem_strat, + allocator); } template< typename MessageT, typename CallbackT, + typename EventCallbackT, typename Alloc, typename SubscriptionT> std::shared_ptr @@ -145,6 +159,7 @@ Node::create_subscription( const std::string & topic_name, CallbackT && callback, size_t qos_history_depth, + EventCallbackT && event_callback, rclcpp::callback_group::CallbackGroup::SharedPtr group, bool ignore_local_publications, typename rclcpp::message_memory_strategy::MessageMemoryStrategy< @@ -159,6 +174,7 @@ Node::create_subscription( topic_name, std::forward(callback), qos, + std::forward(event_callback), group, ignore_local_publications, msg_mem_strat, @@ -180,11 +196,12 @@ Node::create_wall_timer( return timer; } -template +template typename Client::SharedPtr Node::create_client( const std::string & service_name, const rmw_qos_profile_t & qos_profile, + EventCallbackT && event_callback, rclcpp::callback_group::CallbackGroup::SharedPtr group) { rcl_client_options_t options = rcl_client_get_default_options(); @@ -197,27 +214,31 @@ Node::create_client( node_base_.get(), node_graph_, extend_name_with_sub_namespace(service_name, this->get_sub_namespace()), - options); + options, + event_callback); auto cli_base_ptr = std::dynamic_pointer_cast(cli); node_services_->add_client(cli_base_ptr, group); + return cli; } -template +template typename rclcpp::Service::SharedPtr Node::create_service( const std::string & service_name, CallbackT && callback, const rmw_qos_profile_t & qos_profile, + EventCallbackT && event_callback, rclcpp::callback_group::CallbackGroup::SharedPtr group) { - return rclcpp::create_service( + return rclcpp::create_service( node_base_, node_services_, extend_name_with_sub_namespace(service_name, this->get_sub_namespace()), std::forward(callback), qos_profile, + std::forward(event_callback), group); } diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp index 8ecaf074f2..06160d3983 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp @@ -102,14 +102,14 @@ class NodeGraph : public NodeGraphInterface RCLCPP_PUBLIC virtual - rclcpp::Event::SharedPtr + rclcpp::GraphEvent::SharedPtr get_graph_event(); RCLCPP_PUBLIC virtual void wait_for_graph_change( - rclcpp::Event::SharedPtr event, + rclcpp::GraphEvent::SharedPtr event, std::chrono::nanoseconds timeout); RCLCPP_PUBLIC @@ -133,7 +133,7 @@ class NodeGraph : public NodeGraphInterface /// For notifying waiting threads (wait_for_graph_change()) on changes (notify_graph_change()). std::condition_variable graph_cv_; /// Weak references to graph events out on loan. - std::vector graph_events_; + std::vector graph_events_; /// Number of graph events out on loan, used to determine if the graph should be monitored. /** graph_users_count_ is atomic so that it can be accessed without acquiring the graph_mutex_ */ std::atomic_size_t graph_users_count_; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp index 022984dd47..f6c91c1bce 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp @@ -99,7 +99,7 @@ class NodeGraphInterface /** * Affects threads waiting on the notify guard condition, see: * get_notify_guard_condition(), as well as the threads waiting on graph - * changes using a graph Event, see: wait_for_graph_change(). + * changes using a graph GraphEvent, see: wait_for_graph_change(). * * This is typically only used by the rclcpp::graph_listener::GraphListener. * @@ -118,18 +118,18 @@ class NodeGraphInterface /// Return a graph event, which will be set anytime a graph change occurs. /** - * The graph Event object is a loan which must be returned. - * The Event object is scoped and therefore to return the load just let it go + * The graph GraphEvent object is a loan which must be returned. + * The GraphEvent object is scoped and therefore to return the load just let it go * out of scope. */ RCLCPP_PUBLIC virtual - rclcpp::Event::SharedPtr + rclcpp::GraphEvent::SharedPtr get_graph_event() = 0; - /// Wait for a graph event to occur by waiting on an Event to become set. + /// Wait for a graph event to occur by waiting on an GraphEvent to become set. /** - * The given Event must be acquire through the get_graph_event() method. + * The given GraphEvent must be acquire through the get_graph_event() method. * * \throws InvalidEventError if the given event is nullptr * \throws EventNotRegisteredError if the given event was not acquired with @@ -139,7 +139,7 @@ class NodeGraphInterface virtual void wait_for_graph_change( - rclcpp::Event::SharedPtr event, + rclcpp::GraphEvent::SharedPtr event, std::chrono::nanoseconds timeout) = 0; /// Return the number of on loan graph events, see get_graph_event(). diff --git a/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp b/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp index 31cf62e54c..40beb3aacb 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp @@ -58,7 +58,8 @@ class NodeTopics : public NodeTopicsInterface virtual void add_publisher( - rclcpp::PublisherBase::SharedPtr publisher); + rclcpp::PublisherBase::SharedPtr publisher, + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group); RCLCPP_PUBLIC virtual diff --git a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp index b3d46ff334..d888e3ccd2 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp @@ -57,7 +57,8 @@ class NodeTopicsInterface virtual void add_publisher( - rclcpp::PublisherBase::SharedPtr publisher) = 0; + rclcpp::PublisherBase::SharedPtr publisher, + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) = 0; RCLCPP_PUBLIC virtual diff --git a/rclcpp/include/rclcpp/parameter_client.hpp b/rclcpp/include/rclcpp/parameter_client.hpp index a525c8ef8b..05c69c9746 100644 --- a/rclcpp/include/rclcpp/parameter_client.hpp +++ b/rclcpp/include/rclcpp/parameter_client.hpp @@ -122,11 +122,12 @@ class AsyncParametersClient using rcl_interfaces::msg::ParameterEvent; return rclcpp::create_subscription< - ParameterEvent, CallbackT, Alloc, ParameterEvent, SubscriptionT>( + ParameterEvent, CallbackT, ResourceStatusEventCallbackType, Alloc, ParameterEvent, SubscriptionT>( this->node_topics_interface_.get(), "parameter_events", std::forward(callback), rmw_qos_profile_default, + {}, nullptr, // group, false, // ignore_local_publications, false, // use_intra_process_comms_, diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 19d2fba4aa..3519987131 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -30,8 +30,9 @@ #include "rosidl_typesupport_cpp/message_type_support.hpp" #include "rcl_interfaces/msg/intra_process_message.hpp" -#include "rclcpp/exceptions.hpp" #include "rclcpp/waitable.hpp" +#include "rclcpp/event.hpp" +#include "rclcpp/exceptions.hpp" #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/allocator/allocator_deleter.hpp" #include "rclcpp/macros.hpp" @@ -187,6 +188,9 @@ class PublisherBase : public Waitable IntraProcessManagerSharedPtr ipm, const rcl_publisher_options_t & intra_process_options); + virtual void + handle_event(ResourceStatusEvent event) const = 0; + protected: std::shared_ptr rcl_node_handle_; @@ -223,13 +227,15 @@ class Publisher : public PublisherBase rclcpp::node_interfaces::NodeBaseInterface * node_base, const std::string & topic, const rcl_publisher_options_t & publisher_options, + ResourceStatusEventCallbackType event_callback, const std::shared_ptr & allocator) : PublisherBase( node_base, topic, *rosidl_typesupport_cpp::get_message_type_support_handle(), publisher_options), - message_allocator_(allocator) + message_allocator_(allocator), + event_callback_(event_callback) { allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get()); } @@ -362,6 +368,11 @@ class Publisher : public PublisherBase return this->publish(serialized_msg.get()); } + void handle_event(ResourceStatusEvent event) const + { + event_callback_(event); + } + std::shared_ptr get_allocator() const { return message_allocator_; @@ -388,8 +399,9 @@ class Publisher : public PublisherBase } std::shared_ptr message_allocator_; - MessageDeleter message_deleter_; + + ResourceStatusEventCallbackType event_callback_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/publisher_factory.hpp b/rclcpp/include/rclcpp/publisher_factory.hpp index 4044d1a6cc..cb59d0c0c6 100644 --- a/rclcpp/include/rclcpp/publisher_factory.hpp +++ b/rclcpp/include/rclcpp/publisher_factory.hpp @@ -73,15 +73,18 @@ struct PublisherFactory }; /// Return a PublisherFactory with functions setup for creating a PublisherT. -template +template PublisherFactory -create_publisher_factory(std::shared_ptr allocator) +create_publisher_factory(EventCallbackT && event_callback, std::shared_ptr allocator) { PublisherFactory factory; + using rclcpp::ResourceStatusEventCallbackType; + ResourceStatusEventCallbackType status_event_callback = event_callback; + // factory function that creates a MessageT specific PublisherT factory.create_typed_publisher = - [allocator]( + [status_event_callback, allocator]( rclcpp::node_interfaces::NodeBaseInterface * node_base, const std::string & topic_name, rcl_publisher_options_t & publisher_options) -> std::shared_ptr @@ -89,7 +92,8 @@ create_publisher_factory(std::shared_ptr allocator) auto message_alloc = std::make_shared(*allocator.get()); publisher_options.allocator = allocator::get_rcl_allocator(*message_alloc.get()); - return std::make_shared(node_base, topic_name, publisher_options, message_alloc); + return std::make_shared(node_base, topic_name, publisher_options, + status_event_callback, message_alloc); }; // function to add a publisher to the intra process manager diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index 698688b6ef..73da334601 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -88,7 +88,7 @@ * - Graph Events (a waitable event object that wakes up when the graph changes): * - rclcpp::Node::get_graph_event() * - rclcpp::Node::wait_for_graph_change() - * - rclcpp::Event + * - rclcpp::GraphEvent * - List topic names and types: * - rclcpp::Node::get_topic_names_and_types() * - Get the number of publishers or subscribers on a topic: diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 35262e5f05..dc019d8277 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -26,6 +26,7 @@ #include "rclcpp/waitable.hpp" #include "rclcpp/any_service_callback.hpp" +#include "rclcpp/event.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/type_support_decl.hpp" @@ -96,6 +97,9 @@ class ServiceBase : public Waitable std::shared_ptr request_header, std::shared_ptr request) = 0; + virtual void + handle_event(ResourceStatusEvent event) const = 0; + protected: RCLCPP_DISABLE_COPY(ServiceBase) @@ -138,9 +142,10 @@ class Service : public ServiceBase Service( std::shared_ptr node_handle, const std::string & service_name, + rcl_service_options_t & service_options, AnyServiceCallback any_callback, - rcl_service_options_t & service_options) - : ServiceBase(node_handle), any_callback_(any_callback) + ResourceStatusEventCallbackType event_callback) + : ServiceBase(node_handle), any_callback_(any_callback), event_callback_(event_callback) { using rosidl_typesupport_cpp::get_service_type_support_handle; auto service_type_support_handle = get_service_type_support_handle(); @@ -189,6 +194,24 @@ class Service : public ServiceBase rclcpp::exceptions::throw_from_rcl_error(ret, "could not create service"); } + + event_handle_ = std::shared_ptr(new rcl_event_t, + [](rcl_event_t * event) + { + if (rcl_event_fini(event) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Error in destruction of rcl event handle: %s", rcl_get_error_string().str); + rcl_reset_error(); + } + delete event; + }); + *event_handle_.get() = rcl_get_zero_initialized_event(); + + ret = rcl_service_event_init(get_event_handle().get(), get_service_handle().get()); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "could not create service event"); + } } Service( @@ -268,10 +291,18 @@ class Service : public ServiceBase } } + void + handle_event(ResourceStatusEvent event) const + { + event_callback_(event); + } + private: RCLCPP_DISABLE_COPY(Service) AnyServiceCallback any_callback_; + + ResourceStatusEventCallbackType event_callback_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 4f55cc19a8..80b4046633 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -31,6 +31,7 @@ #include "rclcpp/waitable.hpp" #include "rclcpp/any_subscription_callback.hpp" +#include "rclcpp/event.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/macros.hpp" @@ -161,6 +162,9 @@ class SubscriptionBase : public Waitable rcl_interfaces::msg::IntraProcessMessage & ipm, const rmw_message_info_t & message_info) = 0; + virtual void + handle_event(ResourceStatusEvent event) const = 0; + const rosidl_message_type_support_t & get_message_type_support_handle() const; @@ -179,14 +183,17 @@ class SubscriptionBase : public Waitable std::shared_ptr subscription_handle_; std::shared_ptr intra_process_subscription_handle_; std::shared_ptr event_handle_; + std::shared_ptr intra_process_event_handle_; size_t wait_set_subscription_index_; size_t wait_set_intra_process_subscription_index_; size_t wait_set_event_index_; + size_t wait_set_intra_process_event_index_; bool subscription_ready_; bool intra_process_subscription_ready_; bool event_ready_; + bool intra_process_event_ready_; using IntraProcessManagerWeakPtr = std::weak_ptr; @@ -234,6 +241,7 @@ class Subscription : public SubscriptionBase const std::string & topic_name, const rcl_subscription_options_t & subscription_options, AnySubscriptionCallback callback, + ResourceStatusEventCallbackType event_callback, typename message_memory_strategy::MessageMemoryStrategy::SharedPtr memory_strategy = message_memory_strategy::MessageMemoryStrategy::create_default()) @@ -246,7 +254,8 @@ class Subscription : public SubscriptionBase any_callback_(callback), message_memory_strategy_(memory_strategy), get_intra_process_message_callback_(nullptr), - matches_any_intra_process_publishers_(nullptr) + matches_any_intra_process_publishers_(nullptr), + event_callback_(event_callback) {} /// Support dynamically setting the message memory strategy. @@ -327,6 +336,11 @@ class Subscription : public SubscriptionBase any_callback_.dispatch_intra_process(msg, message_info); } + void handle_event(ResourceStatusEvent event) const + { + event_callback_(event); + } + using GetMessageCallbackType = std::function; using MatchesAnyPublishersCallbackType = std::function; @@ -342,6 +356,7 @@ class Subscription : public SubscriptionBase const rcl_subscription_options_t & intra_process_options) { std::string intra_process_topic_name = std::string(get_topic_name()) + "/_intra"; + rcl_ret_t ret = rcl_subscription_init( intra_process_subscription_handle_.get(), node_handle_.get(), @@ -362,6 +377,12 @@ class Subscription : public SubscriptionBase rclcpp::exceptions::throw_from_rcl_error(ret, "could not create intra process subscription"); } + ret = rcl_subscription_event_init(get_event_handle().get(), + get_intra_process_subscription_handle().get()); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "could not create subscription event"); + } + intra_process_subscription_id_ = intra_process_subscription_id; get_intra_process_message_callback_ = get_message_callback; matches_any_intra_process_publishers_ = matches_any_publisher_callback; @@ -388,6 +409,8 @@ class Subscription : public SubscriptionBase GetMessageCallbackType get_intra_process_message_callback_; MatchesAnyPublishersCallbackType matches_any_intra_process_publishers_; + + ResourceStatusEventCallbackType event_callback_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/subscription_factory.hpp b/rclcpp/include/rclcpp/subscription_factory.hpp index e2bdec7ef2..8410ef49f6 100644 --- a/rclcpp/include/rclcpp/subscription_factory.hpp +++ b/rclcpp/include/rclcpp/subscription_factory.hpp @@ -69,12 +69,14 @@ struct SubscriptionFactory template< typename MessageT, typename CallbackT, + typename EventCallbackT, typename Alloc, typename CallbackMessageT, typename SubscriptionT> SubscriptionFactory create_subscription_factory( CallbackT && callback, + EventCallbackT && event_callback, typename rclcpp::message_memory_strategy::MessageMemoryStrategy< CallbackMessageT, Alloc>::SharedPtr msg_mem_strat, @@ -86,12 +88,15 @@ create_subscription_factory( AnySubscriptionCallback any_subscription_callback(allocator); any_subscription_callback.set(std::forward(callback)); + using rclcpp::ResourceStatusEventCallbackType; + ResourceStatusEventCallbackType status_event_callback = event_callback; + auto message_alloc = std::make_shared::MessageAlloc>(); // factory function that creates a MessageT specific SubscriptionT factory.create_typed_subscription = - [allocator, msg_mem_strat, any_subscription_callback, message_alloc]( + [allocator, msg_mem_strat, any_subscription_callback, message_alloc, status_event_callback]( rclcpp::node_interfaces::NodeBaseInterface * node_base, const std::string & topic_name, rcl_subscription_options_t & subscription_options @@ -109,6 +114,7 @@ create_subscription_factory( topic_name, subscription_options, any_subscription_callback, + status_event_callback, msg_mem_strat); auto sub_base_ptr = std::dynamic_pointer_cast(sub); return sub_base_ptr; diff --git a/rclcpp/include/rclcpp/time_source.hpp b/rclcpp/include/rclcpp/time_source.hpp index 09a8340383..829c7d60b9 100644 --- a/rclcpp/include/rclcpp/time_source.hpp +++ b/rclcpp/include/rclcpp/time_source.hpp @@ -103,7 +103,7 @@ class TimeSource // Parameter Client pointer std::shared_ptr parameter_client_; - // Parameter Event subscription + // ParameterEvent subscription using ParamMessageT = rcl_interfaces::msg::ParameterEvent; using ParamSubscriptionT = rclcpp::Subscription; std::shared_ptr parameter_subscription_; diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index 570cb25233..fe2152934d 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -41,6 +41,7 @@ ClientBase::ClientBase( context_(node_base->get_context()) { std::weak_ptr weak_node_handle(node_handle_); + rcl_client_t * new_rcl_client = new rcl_client_t; *new_rcl_client = rcl_get_zero_initialized_client(); client_handle_.reset( @@ -62,6 +63,19 @@ ClientBase::ClientBase( } delete client; }); + + event_handle_ = std::shared_ptr(new rcl_event_t, + [](rcl_event_t * event) + { + if (rcl_event_fini(event) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Error in destruction of rcl event handle: %s", rcl_get_error_string().str); + rcl_reset_error(); + } + delete event; + }); + *event_handle_.get() = rcl_get_zero_initialized_event(); } ClientBase::~ClientBase() @@ -163,6 +177,8 @@ ClientBase::execute() if (event_ready_) { // rcl_take_event(); + auto example_event = ResourceStatusEvent::LIVELINESS_CHANGED; + handle_event(example_event); } } diff --git a/rclcpp/src/rclcpp/event.cpp b/rclcpp/src/rclcpp/event.cpp index 6be20a6335..408c087011 100644 --- a/rclcpp/src/rclcpp/event.cpp +++ b/rclcpp/src/rclcpp/event.cpp @@ -17,23 +17,23 @@ namespace rclcpp { -Event::Event() +GraphEvent::GraphEvent() : state_(false) {} bool -Event::set() +GraphEvent::set() { return state_.exchange(true); } bool -Event::check() +GraphEvent::check() { return state_.load(); } bool -Event::check_and_clear() +GraphEvent::check_and_clear() { return state_.exchange(false); } diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 72d4487eb4..ae707bf603 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -279,119 +279,6 @@ Executor::execute_any_executable(AnyExecutable & any_exec) } } -void -Executor::execute_subscription( - rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rmw_message_info_t message_info; - message_info.from_intra_process = false; - - if (subscription->is_serialized()) { - auto serialized_msg = subscription->create_serialized_message(); - auto ret = rcl_take_serialized_message( - subscription->get_subscription_handle().get(), - serialized_msg.get(), &message_info); - if (RCL_RET_OK == ret) { - auto void_serialized_msg = std::static_pointer_cast(serialized_msg); - subscription->handle_message(void_serialized_msg, message_info); - } else if (RCL_RET_SUBSCRIPTION_TAKE_FAILED != ret) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "take_serialized failed for subscription on topic '%s': %s", - subscription->get_topic_name(), rcl_get_error_string().str); - rcl_reset_error(); - } - subscription->return_serialized_message(serialized_msg); - } else { - std::shared_ptr message = subscription->create_message(); - auto ret = rcl_take( - subscription->get_subscription_handle().get(), - message.get(), &message_info); - if (RCL_RET_OK == ret) { - subscription->handle_message(message, message_info); - } else if (RCL_RET_SUBSCRIPTION_TAKE_FAILED != ret) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "could not deserialize serialized message on topic '%s': %s", - subscription->get_topic_name(), rcl_get_error_string().str); - rcl_reset_error(); - } - subscription->return_message(message); - } -} - -void -Executor::execute_intra_process_subscription( - rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rcl_interfaces::msg::IntraProcessMessage ipm; - rmw_message_info_t message_info; - rcl_ret_t status = rcl_take( - subscription->get_intra_process_subscription_handle().get(), - &ipm, - &message_info); - - if (status == RCL_RET_OK) { - message_info.from_intra_process = true; - subscription->handle_intra_process_message(ipm, message_info); - } else if (status != RCL_RET_SUBSCRIPTION_TAKE_FAILED) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "take failed for intra process subscription on topic '%s': %s", - subscription->get_topic_name(), rcl_get_error_string().str); - rcl_reset_error(); - } -} - -void -Executor::execute_timer( - rclcpp::TimerBase::SharedPtr timer) -{ - timer->execute_callback(); -} - -void -Executor::execute_service( - rclcpp::ServiceBase::SharedPtr service) -{ - auto request_header = service->create_request_header(); - std::shared_ptr request = service->create_request(); - rcl_ret_t status = rcl_take_request( - service->get_service_handle().get(), - request_header.get(), - request.get()); - if (status == RCL_RET_OK) { - service->handle_request(request_header, request); - } else if (status != RCL_RET_SERVICE_TAKE_FAILED) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "take request failed for server of service '%s': %s", - service->get_service_name(), rcl_get_error_string().str); - rcl_reset_error(); - } -} - -void -Executor::execute_client( - rclcpp::ClientBase::SharedPtr client) -{ - auto request_header = client->create_request_header(); - std::shared_ptr response = client->create_response(); - rcl_ret_t status = rcl_take_response( - client->get_client_handle().get(), - request_header.get(), - response.get()); - if (status == RCL_RET_OK) { - client->handle_response(request_header, response); - } else if (status != RCL_RET_CLIENT_TAKE_FAILED) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "take response failed for client of service '%s': %s", - client->get_service_name(), rcl_get_error_string().str); - rcl_reset_error(); - } -} - void Executor::wait_for_work(std::chrono::nanoseconds timeout) { diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 8721f93a59..713bc7d39c 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -300,7 +300,7 @@ Node::get_callback_groups() const return node_base_->get_callback_groups(); } -rclcpp::Event::SharedPtr +rclcpp::GraphEvent::SharedPtr Node::get_graph_event() { return node_graph_->get_graph_event(); @@ -308,7 +308,7 @@ Node::get_graph_event() void Node::wait_for_graph_change( - rclcpp::Event::SharedPtr event, + rclcpp::GraphEvent::SharedPtr event, std::chrono::nanoseconds timeout) { node_graph_->wait_for_graph_change(event, timeout); diff --git a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp index d03e781f9d..a660a3f83d 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp @@ -273,7 +273,7 @@ NodeGraph::notify_graph_change() std::remove_if( graph_events_.begin(), graph_events_.end(), - [](const rclcpp::Event::WeakPtr & wptr) { + [](const rclcpp::GraphEvent::WeakPtr & wptr) { return wptr.expired(); }), graph_events_.end()); @@ -298,10 +298,10 @@ NodeGraph::notify_shutdown() graph_cv_.notify_all(); } -rclcpp::Event::SharedPtr +rclcpp::GraphEvent::SharedPtr NodeGraph::get_graph_event() { - auto event = rclcpp::Event::make_shared(); + auto event = rclcpp::GraphEvent::make_shared(); std::lock_guard graph_changed_lock(graph_mutex_); graph_events_.push_back(event); graph_users_count_++; @@ -315,7 +315,7 @@ NodeGraph::get_graph_event() void NodeGraph::wait_for_graph_change( - rclcpp::Event::SharedPtr event, + rclcpp::GraphEvent::SharedPtr event, std::chrono::nanoseconds timeout) { using rclcpp::exceptions::InvalidEventError; diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 8c4d14b94c..1493e51d42 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -55,10 +55,13 @@ NodeParameters::NodeParameters( } if (start_parameter_event_publisher) { - events_publisher_ = rclcpp::create_publisher( + events_publisher_ = rclcpp::create_publisher( node_topics.get(), "parameter_events", parameter_event_qos_profile, + {}, + nullptr, use_intra_process, allocator); } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp index b380e3c2ec..99e73b586a 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp @@ -64,11 +64,19 @@ NodeTopics::create_publisher( void NodeTopics::add_publisher( - rclcpp::PublisherBase::SharedPtr publisher) + rclcpp::PublisherBase::SharedPtr publisher, + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) { - // The publisher is not added to a callback group or anthing like that for now. - // It may be stored within the NodeTopics class or the NodeBase class in the future. - (void)publisher; + // Assign to a group. + if (callback_group) { + if (!node_base_->callback_group_in_node(callback_group)) { + throw std::runtime_error("Cannot create publisher, callback group not in node."); + } + callback_group->add_publisher(publisher); + } else { + node_base_->get_default_callback_group()->add_publisher(publisher); + } + // Notify the executor that a new publisher was created using the parent Node. { auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); diff --git a/rclcpp/src/rclcpp/parameter_client.cpp b/rclcpp/src/rclcpp/parameter_client.cpp index c49f5c2ab2..cdcad3b2e2 100644 --- a/rclcpp/src/rclcpp/parameter_client.cpp +++ b/rclcpp/src/rclcpp/parameter_client.cpp @@ -49,7 +49,8 @@ AsyncParametersClient::AsyncParametersClient( node_base_interface.get(), node_graph_interface, remote_node_name_ + "/" + parameter_service_names::get_parameters, - options); + options, + ResourceStatusEventCallbackType{}); auto get_parameters_base = std::dynamic_pointer_cast(get_parameters_client_); node_services_interface->add_client(get_parameters_base, nullptr); @@ -57,7 +58,8 @@ AsyncParametersClient::AsyncParametersClient( node_base_interface.get(), node_graph_interface, remote_node_name_ + "/" + parameter_service_names::get_parameter_types, - options); + options, + ResourceStatusEventCallbackType{}); auto get_parameter_types_base = std::dynamic_pointer_cast(get_parameter_types_client_); node_services_interface->add_client(get_parameter_types_base, nullptr); @@ -66,7 +68,8 @@ AsyncParametersClient::AsyncParametersClient( node_base_interface.get(), node_graph_interface, remote_node_name_ + "/" + parameter_service_names::set_parameters, - options); + options, + ResourceStatusEventCallbackType{}); auto set_parameters_base = std::dynamic_pointer_cast(set_parameters_client_); node_services_interface->add_client(set_parameters_base, nullptr); @@ -74,7 +77,8 @@ AsyncParametersClient::AsyncParametersClient( Client::make_shared(node_base_interface.get(), node_graph_interface, remote_node_name_ + "/" + parameter_service_names::set_parameters_atomically, - options); + options, + ResourceStatusEventCallbackType{}); auto set_parameters_atomically_base = std::dynamic_pointer_cast( set_parameters_atomically_client_); node_services_interface->add_client(set_parameters_atomically_base, nullptr); @@ -83,7 +87,8 @@ AsyncParametersClient::AsyncParametersClient( node_base_interface.get(), node_graph_interface, remote_node_name_ + "/" + parameter_service_names::list_parameters, - options); + options, + ResourceStatusEventCallbackType{}); auto list_parameters_base = std::dynamic_pointer_cast(list_parameters_client_); node_services_interface->add_client(list_parameters_base, nullptr); @@ -91,7 +96,8 @@ AsyncParametersClient::AsyncParametersClient( node_base_interface.get(), node_graph_interface, remote_node_name_ + "/" + parameter_service_names::describe_parameters, - options); + options, + ResourceStatusEventCallbackType{}); auto describe_parameters_base = std::dynamic_pointer_cast(describe_parameters_client_); node_services_interface->add_client(describe_parameters_base, nullptr); diff --git a/rclcpp/src/rclcpp/parameter_service.cpp b/rclcpp/src/rclcpp/parameter_service.cpp index afecee41c6..f2392eecbd 100644 --- a/rclcpp/src/rclcpp/parameter_service.cpp +++ b/rclcpp/src/rclcpp/parameter_service.cpp @@ -47,7 +47,7 @@ ParameterService::ParameterService( response->values.push_back(param.get_value_message()); } }, - qos_profile, nullptr); + qos_profile, ResourceStatusEventCallbackType{}, nullptr); get_parameter_types_service_ = create_service( node_base, node_services, @@ -63,7 +63,7 @@ ParameterService::ParameterService( return static_cast(type); }); }, - qos_profile, nullptr); + qos_profile, ResourceStatusEventCallbackType{}, nullptr); set_parameters_service_ = create_service( node_base, node_services, @@ -80,7 +80,7 @@ ParameterService::ParameterService( auto results = node_params->set_parameters(pvariants); response->results = results; }, - qos_profile, nullptr); + qos_profile, ResourceStatusEventCallbackType{}, nullptr); set_parameters_atomically_service_ = create_service( node_base, node_services, @@ -99,7 +99,7 @@ ParameterService::ParameterService( auto result = node_params->set_parameters_atomically(pvariants); response->result = result; }, - qos_profile, nullptr); + qos_profile, ResourceStatusEventCallbackType{}, nullptr); describe_parameters_service_ = create_service( node_base, node_services, @@ -112,7 +112,7 @@ ParameterService::ParameterService( auto descriptors = node_params->describe_parameters(request->names); response->descriptors = descriptors; }, - qos_profile, nullptr); + qos_profile, ResourceStatusEventCallbackType{}, nullptr); list_parameters_service_ = create_service( node_base, node_services, @@ -125,5 +125,5 @@ ParameterService::ParameterService( auto result = node_params->list_parameters(request->prefixes, request->depth); response->result = result; }, - qos_profile, nullptr); + qos_profile, ResourceStatusEventCallbackType{}, nullptr); } diff --git a/rclcpp/src/rclcpp/publisher.cpp b/rclcpp/src/rclcpp/publisher.cpp index 7ca781c90a..a2f5d83187 100644 --- a/rclcpp/src/rclcpp/publisher.cpp +++ b/rclcpp/src/rclcpp/publisher.cpp @@ -62,9 +62,27 @@ PublisherBase::PublisherBase( rcl_node_get_name(rcl_node_handle), rcl_node_get_namespace(rcl_node_handle)); } - rclcpp::exceptions::throw_from_rcl_error(ret, "could not create publisher"); } + + event_handle_ = std::shared_ptr(new rcl_event_t, + [](rcl_event_t * event) + { + if (rcl_event_fini(event) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Error in destruction of rcl event handle: %s", rcl_get_error_string().str); + rcl_reset_error(); + } + delete event; + }); + *event_handle_.get() = rcl_get_zero_initialized_event(); + + ret = rcl_publisher_event_init(get_event_handle().get(), &publisher_handle_); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "could not create publisher event"); + } + // Life time of this object is tied to the publisher handle. rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle(&publisher_handle_); if (!publisher_rmw_handle) { @@ -195,6 +213,8 @@ void PublisherBase::execute() { // rcl_take_event(); + auto example_event = ResourceStatusEvent::LIVELINESS_CHANGED; + handle_event(example_event); } size_t diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index 9b428ca224..5c6a2d1203 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -127,6 +127,8 @@ ServiceBase::execute() if (event_ready_) { // rcl_take_event(); + auto example_event = ResourceStatusEvent::LIVELINESS_CHANGED; + handle_event(example_event); } } diff --git a/rclcpp/src/rclcpp/subscription.cpp b/rclcpp/src/rclcpp/subscription.cpp index 7d4b149683..01b2be0fab 100644 --- a/rclcpp/src/rclcpp/subscription.cpp +++ b/rclcpp/src/rclcpp/subscription.cpp @@ -52,13 +52,24 @@ SubscriptionBase::SubscriptionBase( delete rcl_subs; }; + auto custom_event_deleter = [](rcl_event_t * event) + { + if (rcl_event_fini(event) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Error in destruction of rcl event handle: %s", rcl_get_error_string().str); + rcl_reset_error(); + } + delete event; + }; + + subscription_handle_ = std::shared_ptr( new rcl_subscription_t, custom_deletor); *subscription_handle_.get() = rcl_get_zero_initialized_subscription(); - intra_process_subscription_handle_ = std::shared_ptr( - new rcl_subscription_t, custom_deletor); - *intra_process_subscription_handle_.get() = rcl_get_zero_initialized_subscription(); + event_handle_ = std::shared_ptr(new rcl_event_t, custom_event_deleter); + *event_handle_.get() = rcl_get_zero_initialized_event(); rcl_ret_t ret = rcl_subscription_init( subscription_handle_.get(), @@ -79,6 +90,19 @@ SubscriptionBase::SubscriptionBase( rclcpp::exceptions::throw_from_rcl_error(ret, "could not create subscription"); } + + ret = rcl_subscription_event_init(get_event_handle().get(), get_subscription_handle().get()); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "could not create subscription event"); + } + + + intra_process_subscription_handle_ = std::shared_ptr( + new rcl_subscription_t, custom_deletor); + *intra_process_subscription_handle_.get() = rcl_get_zero_initialized_subscription(); + + intra_process_event_handle_ = std::shared_ptr(new rcl_event_t, custom_event_deleter); + *intra_process_event_handle_.get() = rcl_get_zero_initialized_event(); } SubscriptionBase::~SubscriptionBase() @@ -146,7 +170,11 @@ SubscriptionBase::get_number_of_ready_subscriptions() size_t SubscriptionBase::get_number_of_ready_events() { - return 1; + if (use_intra_process_) { + return 2; + } else { + return 1; + } } bool @@ -160,6 +188,13 @@ SubscriptionBase::add_to_wait_set(rcl_wait_set_t * wait_set) return false; } + if (rcl_wait_set_add_event(wait_set, event_handle_.get(), &wait_set_event_index_) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't add subscription event to wait set: %s", rcl_get_error_string().str); + return false; + } + if (use_intra_process_) { if (rcl_wait_set_add_subscription(wait_set, intra_process_subscription_handle_.get(), &wait_set_intra_process_subscription_index_) != RCL_RET_OK) { @@ -168,13 +203,14 @@ SubscriptionBase::add_to_wait_set(rcl_wait_set_t * wait_set) "Couldn't add intra process subscription to wait set: %s", rcl_get_error_string().str); return false; } - } - if (rcl_wait_set_add_event(wait_set, event_handle_.get(), &wait_set_event_index_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't add subscription event to wait set: %s", rcl_get_error_string().str); - return false; + if (rcl_wait_set_add_event(wait_set, intra_process_event_handle_.get(), + &wait_set_intra_process_event_index_) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't add intra process subscription event to wait set: %s", rcl_get_error_string().str); + return false; + } } return true; @@ -189,7 +225,10 @@ SubscriptionBase::is_ready(rcl_wait_set_t * wait_set) (wait_set->subscriptions[wait_set_intra_process_subscription_index_] == intra_process_subscription_handle_.get()); event_ready_ = (wait_set->events[wait_set_event_index_] == event_handle_.get()); - return subscription_ready_ || intra_process_subscription_ready_ || event_ready_; + intra_process_event_ready_ = + (wait_set->events[wait_set_intra_process_event_index_] == intra_process_event_handle_.get()); + return subscription_ready_ || intra_process_subscription_ready_ || + event_ready_ || intra_process_event_ready_; } void @@ -255,6 +294,14 @@ SubscriptionBase::execute() if (event_ready_) { // rcl_take_event(); + auto example_event = ResourceStatusEvent::LIVELINESS_CHANGED; + handle_event(example_event); + } + + if (intra_process_event_ready_) { + // rcl_take_event(); + auto example_event = ResourceStatusEvent::LIVELINESS_CHANGED; + handle_event(example_event); } } diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 173ccf054f..492d2991e3 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -208,12 +208,13 @@ void TimeSource::create_clock_sub() auto cb = std::bind(&TimeSource::clock_cb, this, std::placeholders::_1); clock_subscription_ = rclcpp::create_subscription< - MessageT, decltype(cb), Alloc, MessageT, SubscriptionT + MessageT, decltype(cb), ResourceStatusEventCallbackType, Alloc, MessageT, SubscriptionT >( node_topics_.get(), topic_name, std::move(cb), rmw_qos_profile_default, + {}, group, false, false, From 1a4ccaa9509f48b90455bc3f264ffebea2e48e91 Mon Sep 17 00:00:00 2001 From: Miaofei Date: Mon, 4 Mar 2019 17:29:26 -0800 Subject: [PATCH 04/27] fix some ros2 build issues Signed-off-by: Miaofei --- .../include/rclcpp_lifecycle/lifecycle_node.hpp | 12 ++++++------ .../rclcpp_lifecycle/lifecycle_node_impl.hpp | 16 +++++++++++----- .../rclcpp_lifecycle/lifecycle_publisher.hpp | 3 ++- rclcpp_lifecycle/src/lifecycle_node.cpp | 4 ++-- 4 files changed, 21 insertions(+), 14 deletions(-) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 22b98a98c2..09890f3ffd 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -314,16 +314,16 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, count_subscribers(const std::string & topic_name) const; /// Return a graph event, which will be set anytime a graph change occurs. - /* The graph Event object is a loan which must be returned. - * The Event object is scoped and therefore to return the load just let it go + /* The graph GraphEvent object is a loan which must be returned. + * The GraphEvent object is scoped and therefore to return the load just let it go * out of scope. */ RCLCPP_LIFECYCLE_PUBLIC - rclcpp::Event::SharedPtr + rclcpp::GraphEvent::SharedPtr get_graph_event(); - /// Wait for a graph event to occur by waiting on an Event to become set. - /* The given Event must be acquire through the get_graph_event() method. + /// Wait for a graph event to occur by waiting on an GraphEvent to become set. + /* The given GraphEvent must be acquire through the get_graph_event() method. * * \throws InvalidEventError if the given event is nullptr * \throws EventNotRegisteredError if the given event was not acquired with @@ -332,7 +332,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, RCLCPP_LIFECYCLE_PUBLIC void wait_for_graph_change( - rclcpp::Event::SharedPtr event, + rclcpp::GraphEvent::SharedPtr event, std::chrono::nanoseconds timeout); RCLCPP_LIFECYCLE_PUBLIC diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index a351da7645..4165ad0b27 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -21,6 +21,7 @@ #include "rclcpp/contexts/default_context.hpp" #include "rclcpp/intra_process_manager.hpp" +#include "rclcpp/event.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/create_publisher.hpp" #include "rclcpp/create_service.hpp" @@ -59,10 +60,13 @@ LifecycleNode::create_publisher( using PublisherT = rclcpp_lifecycle::LifecyclePublisher; // create regular publisher in rclcpp::Node - return rclcpp::create_publisher( + return rclcpp::create_publisher( this->node_topics_.get(), topic_name, qos_profile, + {}, + nullptr, use_intra_process_comms_, allocator); } @@ -92,11 +96,13 @@ LifecycleNode::create_subscription( msg_mem_strat = MessageMemoryStrategy::create_default(); } - return rclcpp::create_subscription( + return rclcpp::create_subscription( this->node_topics_.get(), topic_name, std::forward(callback), qos_profile, + {}, group, ignore_local_publications, use_intra_process_comms_, @@ -178,9 +184,9 @@ LifecycleNode::create_service( const rmw_qos_profile_t & qos_profile, rclcpp::callback_group::CallbackGroup::SharedPtr group) { - return rclcpp::create_service( - node_base_, node_services_, - service_name, std::forward(callback), qos_profile, group); + return rclcpp::create_service( + node_base_, node_services_, service_name, std::forward(callback), qos_profile, {}, + group); } template diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp index 867714e600..3bf31142e3 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp @@ -61,9 +61,10 @@ class LifecyclePublisher : public LifecyclePublisherInterface, rclcpp::node_interfaces::NodeBaseInterface * node_base, const std::string & topic, const rcl_publisher_options_t & publisher_options, + rclcpp::ResourceStatusEventCallbackType event_callback, std::shared_ptr allocator) : rclcpp::Publisher( - node_base, topic, publisher_options, allocator), + node_base, topic, publisher_options, event_callback, allocator), enabled_(false), logger_(rclcpp::get_logger("LifecyclePublisher")) { diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 95c1470340..521b7e3128 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -224,7 +224,7 @@ LifecycleNode::get_callback_groups() const return node_base_->get_callback_groups(); } -rclcpp::Event::SharedPtr +rclcpp::GraphEvent::SharedPtr LifecycleNode::get_graph_event() { return node_graph_->get_graph_event(); @@ -232,7 +232,7 @@ LifecycleNode::get_graph_event() void LifecycleNode::wait_for_graph_change( - rclcpp::Event::SharedPtr event, + rclcpp::GraphEvent::SharedPtr event, std::chrono::nanoseconds timeout) { node_graph_->wait_for_graph_change(event, timeout); From e1ad5c28638f2094ae2f175282134c7d73c9a326 Mon Sep 17 00:00:00 2001 From: Miaofei Date: Tue, 12 Mar 2019 00:18:38 -0700 Subject: [PATCH 05/27] update client-facing API Signed-off-by: Miaofei --- rclcpp/CMakeLists.txt | 1 + rclcpp/include/rclcpp/client.hpp | 17 +- rclcpp/include/rclcpp/create_publisher.hpp | 7 +- rclcpp/include/rclcpp/create_service.hpp | 7 +- rclcpp/include/rclcpp/create_subscription.hpp | 19 +- rclcpp/include/rclcpp/event.hpp | 19 +- rclcpp/include/rclcpp/exceptions.hpp | 4 +- rclcpp/include/rclcpp/graph_event.hpp | 28 +++ rclcpp/include/rclcpp/node.hpp | 85 +------- rclcpp/include/rclcpp/node_impl.hpp | 106 ++------- .../rclcpp/node_interfaces/node_graph.hpp | 2 +- .../node_interfaces/node_graph_interface.hpp | 2 +- rclcpp/include/rclcpp/parameter_client.hpp | 4 +- rclcpp/include/rclcpp/publisher.hpp | 74 +++---- rclcpp/include/rclcpp/publisher_factory.hpp | 11 +- rclcpp/include/rclcpp/publisher_options.hpp | 195 +++++++++++++++++ rclcpp/include/rclcpp/qos_event.hpp | 156 +++++++++++++ rclcpp/include/rclcpp/service.hpp | 16 +- .../strategies/allocator_memory_strategy.hpp | 7 +- rclcpp/include/rclcpp/subscription.hpp | 79 +++---- .../include/rclcpp/subscription_factory.hpp | 11 +- .../include/rclcpp/subscription_options.hpp | 205 ++++++++++++++++++ rclcpp/src/rclcpp/client.cpp | 17 +- rclcpp/src/rclcpp/event.cpp | 8 +- .../node_interfaces/node_parameters.cpp | 5 +- rclcpp/src/rclcpp/parameter_client.cpp | 18 +- rclcpp/src/rclcpp/parameter_service.cpp | 12 +- rclcpp/src/rclcpp/publisher.cpp | 64 +----- rclcpp/src/rclcpp/qos_event.cpp | 49 +++++ rclcpp/src/rclcpp/service.cpp | 17 +- rclcpp/src/rclcpp/subscription.cpp | 77 +------ rclcpp/src/rclcpp/time_source.cpp | 7 +- rclcpp/test/test_time_source.cpp | 3 +- rclcpp_action/test/test_client.cpp | 3 +- .../rclcpp_lifecycle/lifecycle_node.hpp | 2 +- .../rclcpp_lifecycle/lifecycle_node_impl.hpp | 15 +- .../rclcpp_lifecycle/lifecycle_publisher.hpp | 4 +- 37 files changed, 835 insertions(+), 521 deletions(-) create mode 100644 rclcpp/include/rclcpp/graph_event.hpp create mode 100644 rclcpp/include/rclcpp/publisher_options.hpp create mode 100644 rclcpp/include/rclcpp/qos_event.hpp create mode 100644 rclcpp/include/rclcpp/subscription_options.hpp create mode 100644 rclcpp/src/rclcpp/qos_event.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 149d49fa20..6bc931b222 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -65,6 +65,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/parameter_map.cpp src/rclcpp/parameter_service.cpp src/rclcpp/publisher.cpp + src/rclcpp/qos_event.cpp src/rclcpp/service.cpp src/rclcpp/signal_handler.cpp src/rclcpp/subscription.cpp diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 721321c57e..23cf8e02a7 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -122,9 +122,6 @@ class ClientBase : public Waitable virtual void handle_response( std::shared_ptr request_header, std::shared_ptr response) = 0; - virtual void - handle_event(ResourceStatusEvent event) const = 0; - protected: RCLCPP_DISABLE_COPY(ClientBase) @@ -179,10 +176,8 @@ class Client : public ClientBase rclcpp::node_interfaces::NodeBaseInterface * node_base, rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, const std::string & service_name, - rcl_client_options_t & client_options, - ResourceStatusEventCallbackType event_callback) - : ClientBase(node_base, node_graph), - event_callback_(event_callback) + rcl_client_options_t & client_options) + : ClientBase(node_base, node_graph) { using rosidl_typesupport_cpp::get_service_type_support_handle; auto service_type_support_handle = @@ -259,12 +254,6 @@ class Client : public ClientBase callback(future); } - void - handle_event(ResourceStatusEvent event) const - { - event_callback_(event); - } - SharedFuture async_send_request(SharedRequest request) { @@ -328,8 +317,6 @@ class Client : public ClientBase std::map> pending_requests_; std::mutex pending_requests_mutex_; - - ResourceStatusEventCallbackType event_callback_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/create_publisher.hpp b/rclcpp/include/rclcpp/create_publisher.hpp index a1b0b5e163..836abf049d 100644 --- a/rclcpp/include/rclcpp/create_publisher.hpp +++ b/rclcpp/include/rclcpp/create_publisher.hpp @@ -25,13 +25,13 @@ namespace rclcpp { -template +template std::shared_ptr create_publisher( rclcpp::node_interfaces::NodeTopicsInterface * node_topics, const std::string & topic_name, const rmw_qos_profile_t & qos_profile, - EventCallbackT && event_callback, + const PublisherEventCallbacks & callbacks, rclcpp::callback_group::CallbackGroup::SharedPtr group, bool use_intra_process_comms, std::shared_ptr allocator) @@ -41,8 +41,7 @@ create_publisher( auto pub = node_topics->create_publisher( topic_name, - rclcpp::create_publisher_factory( - std::forward(event_callback), allocator), + rclcpp::create_publisher_factory(callbacks, allocator), publisher_options, use_intra_process_comms); diff --git a/rclcpp/include/rclcpp/create_service.hpp b/rclcpp/include/rclcpp/create_service.hpp index 2f6e0a0384..d01c0a23eb 100644 --- a/rclcpp/include/rclcpp/create_service.hpp +++ b/rclcpp/include/rclcpp/create_service.hpp @@ -29,7 +29,7 @@ namespace rclcpp /// Create a service with a given type. /// \internal -template +template typename rclcpp::Service::SharedPtr create_service( std::shared_ptr node_base, @@ -37,7 +37,6 @@ create_service( const std::string & service_name, CallbackT && callback, const rmw_qos_profile_t & qos_profile, - EventCallbackT && event_callback, rclcpp::callback_group::CallbackGroup::SharedPtr group) { rclcpp::AnyServiceCallback any_service_callback; @@ -47,9 +46,9 @@ create_service( service_options.qos = qos_profile; auto serv = Service::make_shared( - node_base->get_shared_rcl_node_handle(), - service_name, service_options, any_service_callback, event_callback); + node_base->get_shared_rcl_node_handle(), service_name, service_options, any_service_callback); auto serv_base_ptr = std::dynamic_pointer_cast(serv); + node_services->add_service(serv_base_ptr, group); return serv; diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index dd13506f40..dedef92838 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -29,7 +29,6 @@ namespace rclcpp template< typename MessageT, typename CallbackT, - typename EventCallbackT, typename AllocatorT, typename CallbackMessageT, typename SubscriptionT = rclcpp::Subscription> @@ -39,23 +38,24 @@ create_subscription( const std::string & topic_name, CallbackT && callback, const rmw_qos_profile_t & qos_profile, - EventCallbackT && event_callback, + const SubscriptionEventCallbacks & callbacks, rclcpp::callback_group::CallbackGroup::SharedPtr group, bool ignore_local_publications, bool use_intra_process_comms, - typename rclcpp::message_memory_strategy::MessageMemoryStrategy< - CallbackMessageT, AllocatorT>::SharedPtr - msg_mem_strat, + typename rclcpp::message_memory_strategy::MessageMemoryStrategy::SharedPtr msg_mem_strat, typename std::shared_ptr allocator) { auto subscription_options = rcl_subscription_get_default_options(); subscription_options.qos = qos_profile; subscription_options.ignore_local_publications = ignore_local_publications; - auto factory = rclcpp::create_subscription_factory - ( - std::forward(callback), std::forward(event_callback), - msg_mem_strat, allocator); + auto factory = rclcpp::create_subscription_factory( + std::forward(callback), + callbacks, + msg_mem_strat, + allocator); auto sub = node_topics->create_subscription( topic_name, @@ -63,6 +63,7 @@ create_subscription( subscription_options, use_intra_process_comms); node_topics->add_subscription(sub, group); + return std::dynamic_pointer_cast(sub); } diff --git a/rclcpp/include/rclcpp/event.hpp b/rclcpp/include/rclcpp/event.hpp index 5099d3091a..988dba29e2 100644 --- a/rclcpp/include/rclcpp/event.hpp +++ b/rclcpp/include/rclcpp/event.hpp @@ -17,7 +17,6 @@ #include #include -#include #include "rclcpp/macros.hpp" #include "rclcpp/visibility_control.hpp" @@ -25,23 +24,13 @@ namespace rclcpp { -enum class ResourceStatusEvent -{ - DEADLINE_MISSED, - LIVELINESS_CHANGED, - LIFESPAN_EXPIRED -}; - -using ResourceStatusEventCallbackType = std::function; - - -class GraphEvent +class Event { public: - RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(GraphEvent) + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Event) RCLCPP_PUBLIC - GraphEvent(); + Event(); RCLCPP_PUBLIC bool @@ -56,7 +45,7 @@ class GraphEvent check_and_clear(); private: - RCLCPP_DISABLE_COPY(GraphEvent) + RCLCPP_DISABLE_COPY(Event) std::atomic_bool state_; }; diff --git a/rclcpp/include/rclcpp/exceptions.hpp b/rclcpp/include/rclcpp/exceptions.hpp index 63a76e5ee0..326b51882b 100644 --- a/rclcpp/include/rclcpp/exceptions.hpp +++ b/rclcpp/include/rclcpp/exceptions.hpp @@ -165,7 +165,7 @@ class RCLInvalidArgument : public RCLErrorBase, public std::invalid_argument RCLInvalidArgument(const RCLErrorBase & base_exc, const std::string & prefix); }; -/// Thrown when an invalid rclcpp::GraphEvent object or SharedPtr is encountered. +/// Thrown when an invalid rclcpp::Event object or SharedPtr is encountered. class InvalidEventError : public std::runtime_error { public: @@ -173,7 +173,7 @@ class InvalidEventError : public std::runtime_error : std::runtime_error("event is invalid") {} }; -/// Thrown when an unregistered rclcpp::GraphEvent is encountered where a registered one was expected. +/// Thrown when an unregistered rclcpp::Event is encountered where a registered one was expected. class EventNotRegisteredError : public std::runtime_error { public: diff --git a/rclcpp/include/rclcpp/graph_event.hpp b/rclcpp/include/rclcpp/graph_event.hpp new file mode 100644 index 0000000000..5362354aab --- /dev/null +++ b/rclcpp/include/rclcpp/graph_event.hpp @@ -0,0 +1,28 @@ +// Copyright 2016 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__GRAPH_EVENT_HPP_ +#define RCLCPP__GRAPH_EVENT_HPP_ + +#include "rclcpp/event.hpp" + + +namespace rclcpp +{ + +class GraphEvent : public Event {}; + +} // namespace rclcpp + +#endif // RCLCPP__GRAPH_EVENT_HPP_ diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 2174c87f12..5e99107c29 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -134,87 +134,27 @@ class Node : public std::enable_shared_from_this /// Create and return a Publisher. /** * \param[in] topic_name The topic for this publisher to publish on. - * \param[in] qos_history_depth The depth of the publisher message queue. - * \param[in] allocator Optional custom allocator. + * \param[in] group The callback group for this publisher. NULL for no callback group. + * \param[in] options Additional options to control creation of the publisher. * \return Shared pointer to the created publisher. */ template< typename MessageT, - typename EventCallbackT = ResourceStatusEventCallbackType, typename Alloc = std::allocator, typename PublisherT = ::rclcpp::Publisher> std::shared_ptr create_publisher( const std::string & topic_name, - size_t qos_history_depth, - EventCallbackT && event_callback = {}, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr, - std::shared_ptr allocator = nullptr); - - /// Create and return a Publisher. - /** - * \param[in] topic_name The topic for this publisher to publish on. - * \param[in] qos_profile The quality of service profile to pass on to the rmw implementation. - * \param[in] allocator Optional custom allocator. - * \return Shared pointer to the created publisher. - */ - template< - typename MessageT, - typename EventCallbackT = ResourceStatusEventCallbackType, - typename Alloc = std::allocator, - typename PublisherT = ::rclcpp::Publisher> - std::shared_ptr - create_publisher( - const std::string & topic_name, - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default, - EventCallbackT && event_callback = {}, - rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr, - std::shared_ptr allocator = nullptr); - - /// Create and return a Subscription. - /** - * \param[in] topic_name The topic to subscribe on. - * \param[in] callback The user-defined callback function. - * \param[in] qos_profile The quality of service profile to pass on to the rmw implementation. - * \param[in] group The callback group for this subscription. NULL for no callback group. - * \param[in] ignore_local_publications True to ignore local publications. - * \param[in] msg_mem_strat The message memory strategy to use for allocating messages. - * \param[in] allocator Optional custom allocator. - * \return Shared pointer to the created subscription. - */ - /* TODO(jacquelinekay): - Windows build breaks when static member function passed as default - argument to msg_mem_strat, nullptr is a workaround. - */ - template< - typename MessageT, - typename CallbackT, - typename EventCallbackT = ResourceStatusEventCallbackType, - typename Alloc = std::allocator, - typename SubscriptionT = rclcpp::Subscription< - typename rclcpp::subscription_traits::has_message_type::type, Alloc>> - std::shared_ptr - create_subscription( - const std::string & topic_name, - CallbackT && callback, - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default, - EventCallbackT && event_callback = {}, - rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr, - bool ignore_local_publications = false, - typename rclcpp::message_memory_strategy::MessageMemoryStrategy< - typename rclcpp::subscription_traits::has_message_type::type, Alloc>::SharedPtr - msg_mem_strat = nullptr, - std::shared_ptr allocator = nullptr); + const PublisherOptions & options = PublisherOptions()); /// Create and return a Subscription. /** * \param[in] topic_name The topic to subscribe on. - * \param[in] qos_history_depth The depth of the subscription's incoming message queue. * \param[in] callback The user-defined callback function. * \param[in] group The callback group for this subscription. NULL for no callback group. - * \param[in] ignore_local_publications True to ignore local publications. + * \param[in] options Additional options to control creation of the subscription. * \param[in] msg_mem_strat The message memory strategy to use for allocating messages. - * \param[in] allocator Optional custom allocator. * \return Shared pointer to the created subscription. */ /* TODO(jacquelinekay): @@ -224,7 +164,6 @@ class Node : public std::enable_shared_from_this template< typename MessageT, typename CallbackT, - typename EventCallbackT = ResourceStatusEventCallbackType, typename Alloc = std::allocator, typename SubscriptionT = rclcpp::Subscription< typename rclcpp::subscription_traits::has_message_type::type, Alloc>> @@ -232,14 +171,11 @@ class Node : public std::enable_shared_from_this create_subscription( const std::string & topic_name, CallbackT && callback, - size_t qos_history_depth, - EventCallbackT && event_callback = {}, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr, - bool ignore_local_publications = false, + const SubscriptionOptions & options = SubscriptionOptions(), typename rclcpp::message_memory_strategy::MessageMemoryStrategy< typename rclcpp::subscription_traits::has_message_type::type, Alloc>::SharedPtr - msg_mem_strat = nullptr, - std::shared_ptr allocator = nullptr); + msg_mem_strat = nullptr); /// Create a timer. /** @@ -255,25 +191,20 @@ class Node : public std::enable_shared_from_this rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); /* Create and return a Client. */ - template + template typename rclcpp::Client::SharedPtr create_client( const std::string & service_name, const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, - EventCallbackT && event_callback = {}, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); /* Create and return a Service. */ - template< - typename ServiceT, - typename CallbackT, - typename EventCallbackT = ResourceStatusEventCallbackType> + template typename rclcpp::Service::SharedPtr create_service( const std::string & service_name, CallbackT && callback, const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, - EventCallbackT && event_callback = {}, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); RCLCPP_PUBLIC diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 5382a79837..82ce3426af 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -51,24 +51,6 @@ namespace rclcpp { -template -std::shared_ptr -Node::create_publisher( - const std::string & topic_name, - size_t qos_history_depth, - EventCallbackT && event_callback, - rclcpp::callback_group::CallbackGroup::SharedPtr group, - std::shared_ptr allocator) -{ - if (!allocator) { - allocator = std::make_shared(); - } - rmw_qos_profile_t qos = rmw_qos_profile_default; - qos.depth = qos_history_depth; - return this->create_publisher( - topic_name, qos, std::forward(event_callback), group, allocator); -} - RCLCPP_LOCAL inline std::string @@ -81,50 +63,42 @@ extend_name_with_sub_namespace(const std::string & name, const std::string & sub return name_with_sub_namespace; } -template +template std::shared_ptr Node::create_publisher( const std::string & topic_name, - const rmw_qos_profile_t & qos_profile, - EventCallbackT && event_callback, rclcpp::callback_group::CallbackGroup::SharedPtr group, - std::shared_ptr allocator) + const PublisherOptions & options) { + std::shared_ptr allocator = options.allocator(); if (!allocator) { allocator = std::make_shared(); } - return rclcpp::create_publisher( + return rclcpp::create_publisher( this->node_topics_.get(), extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), - qos_profile, - std::forward(event_callback), + options.publisher_qos_profile(), + options.event_callbacks(), group, this->get_node_options().use_intra_process_comms(), allocator); } -template< - typename MessageT, - typename CallbackT, - typename EventCallbackT, - typename Alloc, - typename SubscriptionT> +template std::shared_ptr Node::create_subscription( const std::string & topic_name, CallbackT && callback, - const rmw_qos_profile_t & qos_profile, - EventCallbackT && event_callback, rclcpp::callback_group::CallbackGroup::SharedPtr group, - bool ignore_local_publications, + const SubscriptionOptions & options, typename rclcpp::message_memory_strategy::MessageMemoryStrategy< typename rclcpp::subscription_traits::has_message_type::type, Alloc>::SharedPtr - msg_mem_strat, - std::shared_ptr allocator) + msg_mem_strat) { using CallbackMessageT = typename rclcpp::subscription_traits::has_message_type::type; + std::shared_ptr allocator = options.allocator(); if (!allocator) { allocator = std::make_shared(); } @@ -134,49 +108,15 @@ Node::create_subscription( msg_mem_strat = MessageMemoryStrategy::create_default(); } - return rclcpp::create_subscription( - this->node_topics_.get(), - extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), - std::forward(callback), - qos_profile, - std::forward(event_callback), - group, - ignore_local_publications, - this->get_node_options().use_intra_process_comms(), - msg_mem_strat, - allocator); -} - -template< - typename MessageT, - typename CallbackT, - typename EventCallbackT, - typename Alloc, - typename SubscriptionT> -std::shared_ptr -Node::create_subscription( - const std::string & topic_name, - CallbackT && callback, - size_t qos_history_depth, - EventCallbackT && event_callback, - rclcpp::callback_group::CallbackGroup::SharedPtr group, - bool ignore_local_publications, - typename rclcpp::message_memory_strategy::MessageMemoryStrategy< - typename rclcpp::subscription_traits::has_message_type::type, Alloc>::SharedPtr - msg_mem_strat, - std::shared_ptr allocator) -{ - rmw_qos_profile_t qos = rmw_qos_profile_default; - qos.depth = qos_history_depth; - - return this->create_subscription( - topic_name, + return rclcpp::create_subscription( + this->node_topics_.get(), + extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), std::forward(callback), - qos, - std::forward(event_callback), + options.subscription_qos_profile(), + options.event_callbacks(), group, - ignore_local_publications, + options.ignore_local_publications(), + this->get_node_options().use_intra_process_comms(), msg_mem_strat, allocator); } @@ -196,12 +136,11 @@ Node::create_wall_timer( return timer; } -template +template typename Client::SharedPtr Node::create_client( const std::string & service_name, const rmw_qos_profile_t & qos_profile, - EventCallbackT && event_callback, rclcpp::callback_group::CallbackGroup::SharedPtr group) { rcl_client_options_t options = rcl_client_get_default_options(); @@ -214,8 +153,7 @@ Node::create_client( node_base_.get(), node_graph_, extend_name_with_sub_namespace(service_name, this->get_sub_namespace()), - options, - event_callback); + options); auto cli_base_ptr = std::dynamic_pointer_cast(cli); node_services_->add_client(cli_base_ptr, group); @@ -223,22 +161,20 @@ Node::create_client( return cli; } -template +template typename rclcpp::Service::SharedPtr Node::create_service( const std::string & service_name, CallbackT && callback, const rmw_qos_profile_t & qos_profile, - EventCallbackT && event_callback, rclcpp::callback_group::CallbackGroup::SharedPtr group) { - return rclcpp::create_service( + return rclcpp::create_service( node_base_, node_services_, extend_name_with_sub_namespace(service_name, this->get_sub_namespace()), std::forward(callback), qos_profile, - std::forward(event_callback), group); } diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp index 06160d3983..cdb88b8a7d 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp @@ -25,7 +25,7 @@ #include "rcl/guard_condition.h" -#include "rclcpp/event.hpp" +#include "rclcpp/graph_event.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/node_interfaces/node_graph_interface.hpp" diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp index f6c91c1bce..f80b28809b 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp @@ -23,7 +23,7 @@ #include "rcl/guard_condition.h" -#include "rclcpp/event.hpp" +#include "rclcpp/graph_event.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/visibility_control.hpp" diff --git a/rclcpp/include/rclcpp/parameter_client.hpp b/rclcpp/include/rclcpp/parameter_client.hpp index 05c69c9746..e8896db35f 100644 --- a/rclcpp/include/rclcpp/parameter_client.hpp +++ b/rclcpp/include/rclcpp/parameter_client.hpp @@ -122,12 +122,12 @@ class AsyncParametersClient using rcl_interfaces::msg::ParameterEvent; return rclcpp::create_subscription< - ParameterEvent, CallbackT, ResourceStatusEventCallbackType, Alloc, ParameterEvent, SubscriptionT>( + ParameterEvent, CallbackT, Alloc, ParameterEvent, SubscriptionT>( this->node_topics_interface_.get(), "parameter_events", std::forward(callback), rmw_qos_profile_default, - {}, + SubscriptionEventCallbacks(), nullptr, // group, false, // ignore_local_publications, false, // use_intra_process_comms_, diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 3519987131..e62df429a9 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -30,14 +30,16 @@ #include "rosidl_typesupport_cpp/message_type_support.hpp" #include "rcl_interfaces/msg/intra_process_message.hpp" +#include "rclcpp/publisher_options.hpp" #include "rclcpp/waitable.hpp" -#include "rclcpp/event.hpp" +#include "rclcpp/qos_event.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/allocator/allocator_deleter.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/visibility_control.hpp" + namespace rclcpp { @@ -57,7 +59,7 @@ namespace intra_process_manager class IntraProcessManager; } -class PublisherBase : public Waitable +class PublisherBase { friend ::rclcpp::node_interfaces::NodeTopicsInterface; @@ -119,29 +121,20 @@ class PublisherBase : public Waitable const rcl_publisher_t * get_publisher_handle() const; - RCLCPP_PUBLIC - std::shared_ptr - get_event_handle(); - - RCLCPP_PUBLIC - std::shared_ptr - get_event_handle() const; - - RCLCPP_PUBLIC - size_t - get_number_of_ready_events() override; - - RCLCPP_PUBLIC - bool - add_to_wait_set(rcl_wait_set_t * wait_set) override; - - RCLCPP_PUBLIC - bool - is_ready(rcl_wait_set_t * wait_set) override; + // RCLCPP_PUBLIC + template void + add_event_handle(const EventCallbackT & callback, const EventTypeEnum event_type) + { + event_handles_.emplace_back(std::make_shared>( + callback, + rcl_publisher_event_init, + &publisher_handle_, + event_type)); + } RCLCPP_PUBLIC - void - execute() override; + const std::vector> & + get_event_handles() const; /// Get subscription count /** \return The number of subscriptions. */ @@ -188,21 +181,18 @@ class PublisherBase : public Waitable IntraProcessManagerSharedPtr ipm, const rcl_publisher_options_t & intra_process_options); - virtual void - handle_event(ResourceStatusEvent event) const = 0; - protected: + using IntraProcessManagerWeakPtr = + std::weak_ptr; + std::shared_ptr rcl_node_handle_; rcl_publisher_t publisher_handle_ = rcl_get_zero_initialized_publisher(); - rcl_publisher_t intra_process_publisher_handle_ = rcl_get_zero_initialized_publisher(); - std::shared_ptr event_handle_; - size_t wait_set_event_index_; + std::vector> event_handles_; - using IntraProcessManagerWeakPtr = - std::weak_ptr; bool use_intra_process_; + rcl_publisher_t intra_process_publisher_handle_ = rcl_get_zero_initialized_publisher(); IntraProcessManagerWeakPtr weak_ipm_; uint64_t intra_process_publisher_id_; StoreMessageCallbackT store_intra_process_message_; @@ -227,17 +217,26 @@ class Publisher : public PublisherBase rclcpp::node_interfaces::NodeBaseInterface * node_base, const std::string & topic, const rcl_publisher_options_t & publisher_options, - ResourceStatusEventCallbackType event_callback, + const PublisherEventCallbacks & event_callbacks, const std::shared_ptr & allocator) : PublisherBase( node_base, topic, *rosidl_typesupport_cpp::get_message_type_support_handle(), publisher_options), - message_allocator_(allocator), - event_callback_(event_callback) + message_allocator_(allocator) { allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get()); + + if (event_callbacks.deadline_callback_) { + this->add_event_handle(event_callbacks.deadline_callback_, RCL_PUBLISHER_DEADLINE); + } + if (event_callbacks.liveliness_callback_) { + this->add_event_handle(event_callbacks.liveliness_callback_, RCL_PUBLISHER_LIVELINESS); + } + // if (event_callbacks.lifespan_callback_) { + // this->add_event_handle(event_callbacks.lifespan_callback_); + // } } virtual ~Publisher() @@ -368,11 +367,6 @@ class Publisher : public PublisherBase return this->publish(serialized_msg.get()); } - void handle_event(ResourceStatusEvent event) const - { - event_callback_(event); - } - std::shared_ptr get_allocator() const { return message_allocator_; @@ -400,8 +394,6 @@ class Publisher : public PublisherBase std::shared_ptr message_allocator_; MessageDeleter message_deleter_; - - ResourceStatusEventCallbackType event_callback_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/publisher_factory.hpp b/rclcpp/include/rclcpp/publisher_factory.hpp index cb59d0c0c6..b494e66b90 100644 --- a/rclcpp/include/rclcpp/publisher_factory.hpp +++ b/rclcpp/include/rclcpp/publisher_factory.hpp @@ -73,18 +73,15 @@ struct PublisherFactory }; /// Return a PublisherFactory with functions setup for creating a PublisherT. -template +template PublisherFactory -create_publisher_factory(EventCallbackT && event_callback, std::shared_ptr allocator) +create_publisher_factory(const PublisherEventCallbacks & event_callbacks, std::shared_ptr allocator) { PublisherFactory factory; - using rclcpp::ResourceStatusEventCallbackType; - ResourceStatusEventCallbackType status_event_callback = event_callback; - // factory function that creates a MessageT specific PublisherT factory.create_typed_publisher = - [status_event_callback, allocator]( + [event_callbacks, allocator]( rclcpp::node_interfaces::NodeBaseInterface * node_base, const std::string & topic_name, rcl_publisher_options_t & publisher_options) -> std::shared_ptr @@ -93,7 +90,7 @@ create_publisher_factory(EventCallbackT && event_callback, std::shared_ptr(*message_alloc.get()); return std::make_shared(node_base, topic_name, publisher_options, - status_event_callback, message_alloc); + event_callbacks, message_alloc); }; // function to add a publisher to the intra process manager diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp new file mode 100644 index 0000000000..ea12617e34 --- /dev/null +++ b/rclcpp/include/rclcpp/publisher_options.hpp @@ -0,0 +1,195 @@ +// 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__PUBLISHER_OPTIONS_HPP_ +#define RCLCPP__PUBLISHER_OPTIONS_HPP_ + +#include +#include +#include + +#include "rclcpp/visibility_control.hpp" +#include "rclcpp/qos_event.hpp" + + +namespace rclcpp +{ + +struct PublisherEventCallbacks +{ + QOSDeadlineEventCallbackType deadline_callback_; + QOSLivelinessEventCallbackType liveliness_callback_; + QOSLifespanEventCallbackType lifespan_callback_; +}; + +template> +class PublisherOptions +{ +public: + /// Create PublisherOptions with default values, optionally specifying the allocator to use. + /** + * Default values for the node options: + * + * - context = rclcpp::contexts::default_context::get_global_default_context() + * - arguments = {} + * - initial_parameters = {} + * - use_global_arguments = true + * - use_intra_process_comms = false + * - start_parameter_services = true + * - start_publisher_publisher = true + * - publisher_qos_profile = rmw_qos_profile_default + * - allocator = rcl_get_default_allocator() + * + * \param[in] allocator allocator to use in construction of PublisherOptions. + */ + RCLCPP_PUBLIC + explicit PublisherOptions() = default; + + /// Destructor. + RCLCPP_PUBLIC + virtual + ~PublisherOptions() = default; + + /// Copy constructor. + RCLCPP_PUBLIC + PublisherOptions(const PublisherOptions & other) = default; + + /// Assignment operator. + RCLCPP_PUBLIC + PublisherOptions & + operator=(const PublisherOptions & other) = default; + + + /// Return a reference to the publisher_qos_profile QoS. + RCLCPP_PUBLIC + const rmw_qos_profile_t & + publisher_qos_profile() const + { + return publisher_qos_profile_; + } + + /// Set the publisher_qos_profile QoS, return this for parameter idiom. + /** + * The QoS settings to be used for the publisher + */ + RCLCPP_PUBLIC + PublisherOptions & + publisher_qos_profile(const rmw_qos_profile_t & publisher_qos_profile) + { + publisher_qos_profile_ = publisher_qos_profile; + return *this; + } + + RCLCPP_PUBLIC + size_t + qos_history_depth() const + { + return publisher_qos_profile_.depth; + } + + RCLCPP_PUBLIC + PublisherOptions & + qos_history_depth(size_t depth) + { + publisher_qos_profile_.depth = depth; + return *this; + } + + + RCLCPP_PUBLIC + const QOSDeadlineEventCallbackType & + deadline_callback() const + { + return callbacks_.deadline_callback_; + } + + RCLCPP_PUBLIC + PublisherOptions & + deadline_callback(const QOSDeadlineEventCallbackType & callback) + { + callbacks_.deadline_callback_ = callback; + return *this; + } + + RCLCPP_PUBLIC + const QOSLivelinessEventCallbackType & + liveliness_callback() const + { + return callbacks_.liveliness_callback_; + } + + RCLCPP_PUBLIC + PublisherOptions & + liveliness_callback(const QOSLivelinessEventCallbackType & callback) + { + callbacks_.liveliness_callback_ = callback; + return *this; + } + + RCLCPP_PUBLIC + const QOSLifespanEventCallbackType & + lifespan_callback() const + { + return callbacks_.lifespan_callback_; + } + + RCLCPP_PUBLIC + PublisherOptions & + lifespan_callback(const QOSLifespanEventCallbackType & callback) + { + callbacks_.lifespan_callback_ = callback; + return *this; + } + + RCLCPP_PUBLIC + const PublisherEventCallbacks & + event_callbacks() const + { + return callbacks_; + } + + + /// Return the rcl_allocator_t to be used. + RCLCPP_PUBLIC + std::shared_ptr + allocator() const + { + return allocator_; + } + + /// Set the rcl_allocator_t to be used, may cause deallocation of existing rcl_publisher_options_t. + /** + * This will cause the internal rcl_publisher_options_t struct to be invalidated. + */ + RCLCPP_PUBLIC + PublisherOptions & + allocator(std::shared_ptr allocator) + { + allocator_ = allocator; + } + +private: + // IMPORTANT: if any of these default values are changed, please update the + // documentation in this class. + + PublisherEventCallbacks callbacks_; + + rmw_qos_profile_t publisher_qos_profile_ = rmw_qos_profile_default; + + std::shared_ptr allocator_ = nullptr; +}; + +} // namespace rclcpp + +#endif // RCLCPP__PUBLISHER_OPTIONS_HPP_ diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp new file mode 100644 index 0000000000..169dcbca78 --- /dev/null +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -0,0 +1,156 @@ +// Copyright 2016 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__QOS_EVENT_HPP_ +#define RCLCPP__QOS_EVENT_HPP_ + +#include + +#include "rcl/error_handling.h" + +#include "rcutils/logging_macros.h" + +#include "rclcpp/waitable.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/function_traits.hpp" + + +namespace rclcpp +{ + +struct QOSDeadlineEventInfo +{ + enum DeadlineEventType { + DEADLINE_MISSED + }; + + DeadlineEventType event_type; + void * other_info; +}; + +struct QOSLivelinessEventInfo +{ + enum LivelinessEventType { + LIVELINESS_CHANGED + }; + + LivelinessEventType event_type; + void * other_info; +}; + +struct QOSLifespanEventInfo +{ + enum LifespanEventType { + LIFESPAN_EXPIRED + }; + + LifespanEventType event_type; + void * other_info; +}; + + +using QOSDeadlineEventCallbackType = std::function; +using QOSLivelinessEventCallbackType = std::function; +using QOSLifespanEventCallbackType = std::function; + + +// struct QOSDeadlineEventConfig { +// using CallbackT = QOSDeadlineEventCallbackType; +// using EventInfoT = QOSDeadlineEventInfo; +// }; + +// struct QOSLivelinessEventConfig { +// using CallbackT = QOSLivelinessEventCallbackType; +// using EventInfoT = QOSLivelinessEventInfo; +// }; + +// struct QOSLifespanEventConfig { +// using CallbackT = QOSLifespanEventCallbackType; +// using EventInfoT = QOSLifespanEventInfo; +// }; + + +class QOSEventBase : public Waitable +{ +public: + /// Get the number of ready events + size_t + get_number_of_ready_events() override; + + /// Add the Waitable to a wait set. + bool + add_to_wait_set(rcl_wait_set_t * wait_set) override; + + /// Check if the Waitable is ready. + bool + is_ready(rcl_wait_set_t * wait_set) override; + +protected: + rcl_event_t event_handle_; + size_t wait_set_event_index_; +}; + + +template +class QOSEvent : public QOSEventBase +{ +public: + template + QOSEvent(const EventCallbackT & callback, InitFuncT init_func, HandleT handle, EventTypeEnum type) + : event_callback_(callback) + { + event_handle_ = rcl_get_zero_initialized_event(); + rcl_ret_t ret = init_func(&event_handle_, handle, type); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "could not create event"); + } + } + + ~QOSEvent() + { + if (rcl_event_fini(&event_handle_) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Error in destruction of rcl event handle: %s", rcl_get_error_string().str); + rcl_reset_error(); + } + } + + /// Execute any entities of the Waitable that are ready. + void + execute() override + { + EventCallbackInfoT callback_info; + + rcl_ret_t ret = rcl_take_event(&event_handle_, callback_info.other_info); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't take event info: %s", rcl_get_error_string().str); + return; + } + + event_callback_(callback_info); + } + +private: + using EventCallbackInfoT = typename std::remove_reference::template argument_type<0>>::type; + + EventCallbackT event_callback_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__QOS_EVENT_HPP_ diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index dc019d8277..f35d2104b4 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -97,9 +97,6 @@ class ServiceBase : public Waitable std::shared_ptr request_header, std::shared_ptr request) = 0; - virtual void - handle_event(ResourceStatusEvent event) const = 0; - protected: RCLCPP_DISABLE_COPY(ServiceBase) @@ -143,9 +140,8 @@ class Service : public ServiceBase std::shared_ptr node_handle, const std::string & service_name, rcl_service_options_t & service_options, - AnyServiceCallback any_callback, - ResourceStatusEventCallbackType event_callback) - : ServiceBase(node_handle), any_callback_(any_callback), event_callback_(event_callback) + AnyServiceCallback any_callback) + : 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(); @@ -291,18 +287,10 @@ class Service : public ServiceBase } } - void - handle_event(ResourceStatusEvent event) const - { - event_callback_(event); - } - private: RCLCPP_DISABLE_COPY(Service) AnyServiceCallback any_callback_; - - ResourceStatusEventCallbackType event_callback_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index f22ad4fe22..42a24dbab1 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -121,13 +121,18 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy for (auto & weak_publisher : group->get_publisher_ptrs()) { auto publisher = weak_publisher.lock(); if (publisher) { - waitable_handles_.push_back(publisher); + for (auto & publisher_event : publisher->get_event_handles()) { + waitable_handles_.push_back(publisher_event); + } } } for (auto & weak_subscription : group->get_subscription_ptrs()) { auto subscription = weak_subscription.lock(); if (subscription) { waitable_handles_.push_back(subscription); + for (auto & subscription_event : subscription->get_event_handles()) { + waitable_handles_.push_back(subscription_event); + } } } for (auto & weak_service : group->get_service_ptrs()) { diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 80b4046633..d266982e9d 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -29,9 +29,10 @@ #include "rcl_interfaces/msg/intra_process_message.hpp" +#include "rclcpp/subscription_options.hpp" #include "rclcpp/waitable.hpp" #include "rclcpp/any_subscription_callback.hpp" -#include "rclcpp/event.hpp" +#include "rclcpp/qos_event.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/macros.hpp" @@ -40,6 +41,7 @@ #include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" + namespace rclcpp { @@ -101,22 +103,25 @@ class SubscriptionBase : public Waitable virtual std::shared_ptr get_intra_process_subscription_handle() const; - RCLCPP_PUBLIC - std::shared_ptr - get_event_handle(); + // RCLCPP_PUBLIC + template void + add_event_handle(const EventCallbackT & callback, const EventTypeEnum event_type) + { + event_handles_.emplace_back(std::make_shared>( + callback, + rcl_subscription_event_init, + get_subscription_handle().get(), + event_type)); + } RCLCPP_PUBLIC - std::shared_ptr - get_event_handle() const; + const std::vector> & + get_event_handles() const; RCLCPP_PUBLIC size_t get_number_of_ready_subscriptions() override; - RCLCPP_PUBLIC - size_t - get_number_of_ready_events() override; - RCLCPP_PUBLIC bool add_to_wait_set(rcl_wait_set_t * wait_set) override; @@ -162,9 +167,6 @@ class SubscriptionBase : public Waitable rcl_interfaces::msg::IntraProcessMessage & ipm, const rmw_message_info_t & message_info) = 0; - virtual void - handle_event(ResourceStatusEvent event) const = 0; - const rosidl_message_type_support_t & get_message_type_support_handle() const; @@ -178,26 +180,21 @@ class SubscriptionBase : public Waitable get_publisher_count() const; protected: + using IntraProcessManagerWeakPtr = + std::weak_ptr; + std::shared_ptr node_handle_; std::shared_ptr subscription_handle_; - std::shared_ptr intra_process_subscription_handle_; - std::shared_ptr event_handle_; - std::shared_ptr intra_process_event_handle_; - size_t wait_set_subscription_index_; - size_t wait_set_intra_process_subscription_index_; - size_t wait_set_event_index_; - size_t wait_set_intra_process_event_index_; - bool subscription_ready_; - bool intra_process_subscription_ready_; - bool event_ready_; - bool intra_process_event_ready_; - using IntraProcessManagerWeakPtr = - std::weak_ptr; + std::vector> event_handles_; + bool use_intra_process_; + std::shared_ptr intra_process_subscription_handle_; + size_t wait_set_intra_process_subscription_index_; + bool intra_process_subscription_ready_; IntraProcessManagerWeakPtr weak_ipm_; uint64_t intra_process_subscription_id_; @@ -241,7 +238,7 @@ class Subscription : public SubscriptionBase const std::string & topic_name, const rcl_subscription_options_t & subscription_options, AnySubscriptionCallback callback, - ResourceStatusEventCallbackType event_callback, + const SubscriptionEventCallbacks & event_callbacks, typename message_memory_strategy::MessageMemoryStrategy::SharedPtr memory_strategy = message_memory_strategy::MessageMemoryStrategy::create_default()) @@ -254,9 +251,18 @@ class Subscription : public SubscriptionBase any_callback_(callback), message_memory_strategy_(memory_strategy), get_intra_process_message_callback_(nullptr), - matches_any_intra_process_publishers_(nullptr), - event_callback_(event_callback) - {} + matches_any_intra_process_publishers_(nullptr) + { + if (event_callbacks.deadline_callback_) { + this->add_event_handle(event_callbacks.deadline_callback_, RCL_SUBSCRIPTION_DEADLINE); + } + if (event_callbacks.liveliness_callback_) { + this->add_event_handle(event_callbacks.liveliness_callback_, RCL_SUBSCRIPTION_LIVELINESS); + } + // if (event_callbacks.lifespan_callback_) { + // this->add_event_handle(event_callbacks.lifespan_callback_); + // } + } /// Support dynamically setting the message memory strategy. /** @@ -336,11 +342,6 @@ class Subscription : public SubscriptionBase any_callback_.dispatch_intra_process(msg, message_info); } - void handle_event(ResourceStatusEvent event) const - { - event_callback_(event); - } - using GetMessageCallbackType = std::function; using MatchesAnyPublishersCallbackType = std::function; @@ -377,12 +378,6 @@ class Subscription : public SubscriptionBase rclcpp::exceptions::throw_from_rcl_error(ret, "could not create intra process subscription"); } - ret = rcl_subscription_event_init(get_event_handle().get(), - get_intra_process_subscription_handle().get()); - if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error(ret, "could not create subscription event"); - } - intra_process_subscription_id_ = intra_process_subscription_id; get_intra_process_message_callback_ = get_message_callback; matches_any_intra_process_publishers_ = matches_any_publisher_callback; @@ -409,8 +404,6 @@ class Subscription : public SubscriptionBase GetMessageCallbackType get_intra_process_message_callback_; MatchesAnyPublishersCallbackType matches_any_intra_process_publishers_; - - ResourceStatusEventCallbackType event_callback_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/subscription_factory.hpp b/rclcpp/include/rclcpp/subscription_factory.hpp index 8410ef49f6..88571ddc09 100644 --- a/rclcpp/include/rclcpp/subscription_factory.hpp +++ b/rclcpp/include/rclcpp/subscription_factory.hpp @@ -69,14 +69,13 @@ struct SubscriptionFactory template< typename MessageT, typename CallbackT, - typename EventCallbackT, typename Alloc, typename CallbackMessageT, typename SubscriptionT> SubscriptionFactory create_subscription_factory( CallbackT && callback, - EventCallbackT && event_callback, + const SubscriptionEventCallbacks & event_callbacks, typename rclcpp::message_memory_strategy::MessageMemoryStrategy< CallbackMessageT, Alloc>::SharedPtr msg_mem_strat, @@ -88,15 +87,12 @@ create_subscription_factory( AnySubscriptionCallback any_subscription_callback(allocator); any_subscription_callback.set(std::forward(callback)); - using rclcpp::ResourceStatusEventCallbackType; - ResourceStatusEventCallbackType status_event_callback = event_callback; - auto message_alloc = std::make_shared::MessageAlloc>(); // factory function that creates a MessageT specific SubscriptionT factory.create_typed_subscription = - [allocator, msg_mem_strat, any_subscription_callback, message_alloc, status_event_callback]( + [allocator, msg_mem_strat, any_subscription_callback, event_callbacks, message_alloc]( rclcpp::node_interfaces::NodeBaseInterface * node_base, const std::string & topic_name, rcl_subscription_options_t & subscription_options @@ -114,9 +110,10 @@ create_subscription_factory( topic_name, subscription_options, any_subscription_callback, - status_event_callback, + event_callbacks, msg_mem_strat); auto sub_base_ptr = std::dynamic_pointer_cast(sub); + return sub_base_ptr; }; diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp new file mode 100644 index 0000000000..076be2b11f --- /dev/null +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -0,0 +1,205 @@ +// 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__SUBSCRIPTION_OPTIONS_HPP_ +#define RCLCPP__SUBSCRIPTION_OPTIONS_HPP_ + +#include +#include +#include + +#include "rclcpp/visibility_control.hpp" +#include "rclcpp/qos_event.hpp" + + +namespace rclcpp +{ + +struct SubscriptionEventCallbacks +{ + QOSDeadlineEventCallbackType deadline_callback_; + QOSLivelinessEventCallbackType liveliness_callback_; + QOSLifespanEventCallbackType lifespan_callback_; +}; + +template> +class SubscriptionOptions +{ +public: + /// Create SubscriptionOptions with default values, optionally specifying the allocator to use. + /** + * Default values for the node options: + * + * - deadline_callback = nullptr + * - liveliness_callback = nullptr + * - lifespan_callback = nullptr + * - subscription_qos_profile = rmw_qos_profile_default + * - ignore_local_publications = false + * - allocator = nullptr + * + * \param[in] allocator allocator to use in construction of SubscriptionOptions. + */ + RCLCPP_PUBLIC + explicit SubscriptionOptions() = default; + + /// Destructor. + RCLCPP_PUBLIC + virtual + ~SubscriptionOptions() = default; + + /// Copy constructor. + RCLCPP_PUBLIC + SubscriptionOptions(const SubscriptionOptions & other) = default; + + /// Assignment operator. + RCLCPP_PUBLIC + SubscriptionOptions & + operator=(const SubscriptionOptions & other) = default; + + + /// Return a reference to the subscription_qos_profile QoS. + RCLCPP_PUBLIC + const rmw_qos_profile_t & + subscription_qos_profile() const + { + return subscription_qos_profile_; + } + + /// Set the subscription_qos_profile QoS, return this for parameter idiom. + /** + * The QoS settings to be used for the subscription + */ + RCLCPP_PUBLIC + SubscriptionOptions & + subscription_qos_profile(const rmw_qos_profile_t & subscription_qos_profile) + { + subscription_qos_profile_ = subscription_qos_profile; + return *this; + } + + RCLCPP_PUBLIC + size_t + qos_history_depth() const + { + return subscription_qos_profile_.depth; + } + + RCLCPP_PUBLIC + SubscriptionOptions & + qos_history_depth(size_t depth) + { + subscription_qos_profile_.depth = depth; + return *this; + } + + + RCLCPP_PUBLIC + const QOSDeadlineEventCallbackType & + deadline_callback() const + { + return callbacks_.deadline_callback_; + } + + RCLCPP_PUBLIC + SubscriptionOptions & + deadline_callback(const QOSDeadlineEventCallbackType & callback) + { + callbacks_.deadline_callback_ = callback; + return *this; + } + + RCLCPP_PUBLIC + const QOSLivelinessEventCallbackType & + liveliness_callback() const + { + return callbacks_.liveliness_callback_; + } + + RCLCPP_PUBLIC + SubscriptionOptions & + liveliness_callback(const QOSLivelinessEventCallbackType & callback) + { + callbacks_.liveliness_callback_ = callback; + return *this; + } + + RCLCPP_PUBLIC + const QOSLifespanEventCallbackType & + lifespan_callback() const + { + return callbacks_.lifespan_callback_; + } + + RCLCPP_PUBLIC + SubscriptionOptions & + lifespan_callback(const QOSLifespanEventCallbackType & callback) + { + callbacks_.lifespan_callback_ = callback; + return *this; + } + + RCLCPP_PUBLIC + const SubscriptionEventCallbacks & + event_callbacks() const + { + return callbacks_; + } + + + RCLCPP_PUBLIC + bool + ignore_local_publications() const + { + return ignore_local_publications_; + } + + RCLCPP_PUBLIC + SubscriptionOptions & + ignore_local_publications(bool ignore) + { + ignore_local_publications_ = ignore; + return *this; + } + + /// Return the std::shared_ptr to be used. + RCLCPP_PUBLIC + std::shared_ptr + allocator() const + { + return allocator_; + } + + RCLCPP_PUBLIC + SubscriptionOptions & + allocator(std::shared_ptr allocator) + { + allocator_ = allocator; + } + +private: + // IMPORTANT: if any of these default values are changed, please update the + // documentation in this class. + + SubscriptionEventCallbacks callbacks_; + + rmw_qos_profile_t subscription_qos_profile_ = rmw_qos_profile_default; + + bool ignore_local_publications_ = false; + + std::shared_ptr allocator_ = nullptr; +}; + +} // namespace rclcpp + +#endif // RCLCPP__SUBSCRIPTION_OPTIONS_HPP_ diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index fe2152934d..bc28bcd24a 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -136,12 +136,14 @@ ClientBase::add_to_wait_set(rcl_wait_set_t * wait_set) return false; } - if (rcl_wait_set_add_event(wait_set, event_handle_.get(), &wait_set_event_index_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't add client event to wait set: %s", rcl_get_error_string().str); - return false; - } + // TODO(mm318): enable QOS event callbacks for clients (currently only for publishers and subscriptions) + wait_set_event_index_ = 0; + // if (rcl_wait_set_add_event(wait_set, event_handle_.get(), &wait_set_event_index_) != RCL_RET_OK) { + // RCUTILS_LOG_ERROR_NAMED( + // "rclcpp", + // "Couldn't add client event to wait set: %s", rcl_get_error_string().str); + // return false; + // } return true; } @@ -177,8 +179,7 @@ ClientBase::execute() if (event_ready_) { // rcl_take_event(); - auto example_event = ResourceStatusEvent::LIVELINESS_CHANGED; - handle_event(example_event); + // handle_event(example_event); } } diff --git a/rclcpp/src/rclcpp/event.cpp b/rclcpp/src/rclcpp/event.cpp index 408c087011..6be20a6335 100644 --- a/rclcpp/src/rclcpp/event.cpp +++ b/rclcpp/src/rclcpp/event.cpp @@ -17,23 +17,23 @@ namespace rclcpp { -GraphEvent::GraphEvent() +Event::Event() : state_(false) {} bool -GraphEvent::set() +Event::set() { return state_.exchange(true); } bool -GraphEvent::check() +Event::check() { return state_.load(); } bool -GraphEvent::check_and_clear() +Event::check_and_clear() { return state_.exchange(false); } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 1493e51d42..345715bd6f 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -55,12 +55,11 @@ NodeParameters::NodeParameters( } if (start_parameter_event_publisher) { - events_publisher_ = rclcpp::create_publisher( + events_publisher_ = rclcpp::create_publisher( node_topics.get(), "parameter_events", parameter_event_qos_profile, - {}, + PublisherEventCallbacks(), nullptr, use_intra_process, allocator); diff --git a/rclcpp/src/rclcpp/parameter_client.cpp b/rclcpp/src/rclcpp/parameter_client.cpp index cdcad3b2e2..c49f5c2ab2 100644 --- a/rclcpp/src/rclcpp/parameter_client.cpp +++ b/rclcpp/src/rclcpp/parameter_client.cpp @@ -49,8 +49,7 @@ AsyncParametersClient::AsyncParametersClient( node_base_interface.get(), node_graph_interface, remote_node_name_ + "/" + parameter_service_names::get_parameters, - options, - ResourceStatusEventCallbackType{}); + options); auto get_parameters_base = std::dynamic_pointer_cast(get_parameters_client_); node_services_interface->add_client(get_parameters_base, nullptr); @@ -58,8 +57,7 @@ AsyncParametersClient::AsyncParametersClient( node_base_interface.get(), node_graph_interface, remote_node_name_ + "/" + parameter_service_names::get_parameter_types, - options, - ResourceStatusEventCallbackType{}); + options); auto get_parameter_types_base = std::dynamic_pointer_cast(get_parameter_types_client_); node_services_interface->add_client(get_parameter_types_base, nullptr); @@ -68,8 +66,7 @@ AsyncParametersClient::AsyncParametersClient( node_base_interface.get(), node_graph_interface, remote_node_name_ + "/" + parameter_service_names::set_parameters, - options, - ResourceStatusEventCallbackType{}); + options); auto set_parameters_base = std::dynamic_pointer_cast(set_parameters_client_); node_services_interface->add_client(set_parameters_base, nullptr); @@ -77,8 +74,7 @@ AsyncParametersClient::AsyncParametersClient( Client::make_shared(node_base_interface.get(), node_graph_interface, remote_node_name_ + "/" + parameter_service_names::set_parameters_atomically, - options, - ResourceStatusEventCallbackType{}); + options); auto set_parameters_atomically_base = std::dynamic_pointer_cast( set_parameters_atomically_client_); node_services_interface->add_client(set_parameters_atomically_base, nullptr); @@ -87,8 +83,7 @@ AsyncParametersClient::AsyncParametersClient( node_base_interface.get(), node_graph_interface, remote_node_name_ + "/" + parameter_service_names::list_parameters, - options, - ResourceStatusEventCallbackType{}); + options); auto list_parameters_base = std::dynamic_pointer_cast(list_parameters_client_); node_services_interface->add_client(list_parameters_base, nullptr); @@ -96,8 +91,7 @@ AsyncParametersClient::AsyncParametersClient( node_base_interface.get(), node_graph_interface, remote_node_name_ + "/" + parameter_service_names::describe_parameters, - options, - ResourceStatusEventCallbackType{}); + options); auto describe_parameters_base = std::dynamic_pointer_cast(describe_parameters_client_); node_services_interface->add_client(describe_parameters_base, nullptr); diff --git a/rclcpp/src/rclcpp/parameter_service.cpp b/rclcpp/src/rclcpp/parameter_service.cpp index f2392eecbd..afecee41c6 100644 --- a/rclcpp/src/rclcpp/parameter_service.cpp +++ b/rclcpp/src/rclcpp/parameter_service.cpp @@ -47,7 +47,7 @@ ParameterService::ParameterService( response->values.push_back(param.get_value_message()); } }, - qos_profile, ResourceStatusEventCallbackType{}, nullptr); + qos_profile, nullptr); get_parameter_types_service_ = create_service( node_base, node_services, @@ -63,7 +63,7 @@ ParameterService::ParameterService( return static_cast(type); }); }, - qos_profile, ResourceStatusEventCallbackType{}, nullptr); + qos_profile, nullptr); set_parameters_service_ = create_service( node_base, node_services, @@ -80,7 +80,7 @@ ParameterService::ParameterService( auto results = node_params->set_parameters(pvariants); response->results = results; }, - qos_profile, ResourceStatusEventCallbackType{}, nullptr); + qos_profile, nullptr); set_parameters_atomically_service_ = create_service( node_base, node_services, @@ -99,7 +99,7 @@ ParameterService::ParameterService( auto result = node_params->set_parameters_atomically(pvariants); response->result = result; }, - qos_profile, ResourceStatusEventCallbackType{}, nullptr); + qos_profile, nullptr); describe_parameters_service_ = create_service( node_base, node_services, @@ -112,7 +112,7 @@ ParameterService::ParameterService( auto descriptors = node_params->describe_parameters(request->names); response->descriptors = descriptors; }, - qos_profile, ResourceStatusEventCallbackType{}, nullptr); + qos_profile, nullptr); list_parameters_service_ = create_service( node_base, node_services, @@ -125,5 +125,5 @@ ParameterService::ParameterService( auto result = node_params->list_parameters(request->prefixes, request->depth); response->result = result; }, - qos_profile, ResourceStatusEventCallbackType{}, nullptr); + qos_profile, nullptr); } diff --git a/rclcpp/src/rclcpp/publisher.cpp b/rclcpp/src/rclcpp/publisher.cpp index a2f5d83187..0f89165134 100644 --- a/rclcpp/src/rclcpp/publisher.cpp +++ b/rclcpp/src/rclcpp/publisher.cpp @@ -37,6 +37,7 @@ using rclcpp::PublisherBase; + PublisherBase::PublisherBase( rclcpp::node_interfaces::NodeBaseInterface * node_base, const std::string & topic, @@ -65,24 +66,6 @@ PublisherBase::PublisherBase( rclcpp::exceptions::throw_from_rcl_error(ret, "could not create publisher"); } - event_handle_ = std::shared_ptr(new rcl_event_t, - [](rcl_event_t * event) - { - if (rcl_event_fini(event) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Error in destruction of rcl event handle: %s", rcl_get_error_string().str); - rcl_reset_error(); - } - delete event; - }); - *event_handle_.get() = rcl_get_zero_initialized_event(); - - ret = rcl_publisher_event_init(get_event_handle().get(), &publisher_handle_); - if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error(ret, "could not create publisher event"); - } - // Life time of this object is tied to the publisher handle. rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle(&publisher_handle_); if (!publisher_rmw_handle) { @@ -172,49 +155,10 @@ PublisherBase::get_publisher_handle() const return &publisher_handle_; } -std::shared_ptr -PublisherBase::get_event_handle() -{ - return event_handle_; -} - -std::shared_ptr -PublisherBase::get_event_handle() const -{ - return event_handle_; -} - -size_t -PublisherBase::get_number_of_ready_events() -{ - return 1; -} - -bool -PublisherBase::add_to_wait_set(rcl_wait_set_t * wait_set) -{ - if (rcl_wait_set_add_event(wait_set, event_handle_.get(), &wait_set_event_index_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't add publisher event to wait set: %s", rcl_get_error_string().str); - return false; - } - - return true; -} - -bool -PublisherBase::is_ready(rcl_wait_set_t * wait_set) -{ - return (wait_set->events[wait_set_event_index_] == event_handle_.get()); -} - -void -PublisherBase::execute() +const std::vector> & +PublisherBase::get_event_handles() const { - // rcl_take_event(); - auto example_event = ResourceStatusEvent::LIVELINESS_CHANGED; - handle_event(example_event); + return event_handles_; } size_t diff --git a/rclcpp/src/rclcpp/qos_event.cpp b/rclcpp/src/rclcpp/qos_event.cpp new file mode 100644 index 0000000000..977d14c2db --- /dev/null +++ b/rclcpp/src/rclcpp/qos_event.cpp @@ -0,0 +1,49 @@ +// Copyright 2016 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/qos_event.hpp" + + +namespace rclcpp +{ + +/// Get the number of ready events +size_t +QOSEventBase::get_number_of_ready_events() +{ + return 1; +} + +/// Add the Waitable to a wait set. +bool +QOSEventBase::add_to_wait_set(rcl_wait_set_t * wait_set) +{ + if (rcl_wait_set_add_event(wait_set, &event_handle_, &wait_set_event_index_) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't add event to wait set: %s", rcl_get_error_string().str); + return false; + } + + return true; +} + +/// Check if the Waitable is ready. +bool +QOSEventBase::is_ready(rcl_wait_set_t * wait_set) +{ + return (wait_set->events[wait_set_event_index_] == &event_handle_); +} + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index 5c6a2d1203..f0fef41d79 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -86,12 +86,14 @@ ServiceBase::add_to_wait_set(rcl_wait_set_t * wait_set) return false; } - if (rcl_wait_set_add_event(wait_set, event_handle_.get(), &wait_set_event_index_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't add service event to wait set: %s", rcl_get_error_string().str); - return false; - } + // TODO(mm318): enable QOS event callbacks for clients (currently only for publishers and subscriptions) + wait_set_event_index_ = 0; + // if (rcl_wait_set_add_event(wait_set, event_handle_.get(), &wait_set_event_index_) != RCL_RET_OK) { + // RCUTILS_LOG_ERROR_NAMED( + // "rclcpp", + // "Couldn't add service event to wait set: %s", rcl_get_error_string().str); + // return false; + // } return true; } @@ -127,8 +129,7 @@ ServiceBase::execute() if (event_ready_) { // rcl_take_event(); - auto example_event = ResourceStatusEvent::LIVELINESS_CHANGED; - handle_event(example_event); + // handle_event(example_event); } } diff --git a/rclcpp/src/rclcpp/subscription.cpp b/rclcpp/src/rclcpp/subscription.cpp index 01b2be0fab..ee5102210f 100644 --- a/rclcpp/src/rclcpp/subscription.cpp +++ b/rclcpp/src/rclcpp/subscription.cpp @@ -52,25 +52,11 @@ SubscriptionBase::SubscriptionBase( delete rcl_subs; }; - auto custom_event_deleter = [](rcl_event_t * event) - { - if (rcl_event_fini(event) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Error in destruction of rcl event handle: %s", rcl_get_error_string().str); - rcl_reset_error(); - } - delete event; - }; - subscription_handle_ = std::shared_ptr( new rcl_subscription_t, custom_deletor); *subscription_handle_.get() = rcl_get_zero_initialized_subscription(); - event_handle_ = std::shared_ptr(new rcl_event_t, custom_event_deleter); - *event_handle_.get() = rcl_get_zero_initialized_event(); - rcl_ret_t ret = rcl_subscription_init( subscription_handle_.get(), node_handle_.get(), @@ -91,18 +77,10 @@ SubscriptionBase::SubscriptionBase( rclcpp::exceptions::throw_from_rcl_error(ret, "could not create subscription"); } - ret = rcl_subscription_event_init(get_event_handle().get(), get_subscription_handle().get()); - if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error(ret, "could not create subscription event"); - } - intra_process_subscription_handle_ = std::shared_ptr( new rcl_subscription_t, custom_deletor); *intra_process_subscription_handle_.get() = rcl_get_zero_initialized_subscription(); - - intra_process_event_handle_ = std::shared_ptr(new rcl_event_t, custom_event_deleter); - *intra_process_event_handle_.get() = rcl_get_zero_initialized_event(); } SubscriptionBase::~SubscriptionBase() @@ -145,16 +123,10 @@ SubscriptionBase::get_intra_process_subscription_handle() const return intra_process_subscription_handle_; } -std::shared_ptr -SubscriptionBase::get_event_handle() -{ - return event_handle_; -} - -std::shared_ptr -SubscriptionBase::get_event_handle() const +const std::vector> & +SubscriptionBase::get_event_handles() const { - return event_handle_; + return event_handles_; } size_t @@ -167,16 +139,6 @@ SubscriptionBase::get_number_of_ready_subscriptions() } } -size_t -SubscriptionBase::get_number_of_ready_events() -{ - if (use_intra_process_) { - return 2; - } else { - return 1; - } -} - bool SubscriptionBase::add_to_wait_set(rcl_wait_set_t * wait_set) { @@ -188,13 +150,6 @@ SubscriptionBase::add_to_wait_set(rcl_wait_set_t * wait_set) return false; } - if (rcl_wait_set_add_event(wait_set, event_handle_.get(), &wait_set_event_index_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't add subscription event to wait set: %s", rcl_get_error_string().str); - return false; - } - if (use_intra_process_) { if (rcl_wait_set_add_subscription(wait_set, intra_process_subscription_handle_.get(), &wait_set_intra_process_subscription_index_) != RCL_RET_OK) { @@ -203,14 +158,6 @@ SubscriptionBase::add_to_wait_set(rcl_wait_set_t * wait_set) "Couldn't add intra process subscription to wait set: %s", rcl_get_error_string().str); return false; } - - if (rcl_wait_set_add_event(wait_set, intra_process_event_handle_.get(), - &wait_set_intra_process_event_index_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't add intra process subscription event to wait set: %s", rcl_get_error_string().str); - return false; - } } return true; @@ -224,11 +171,7 @@ SubscriptionBase::is_ready(rcl_wait_set_t * wait_set) intra_process_subscription_ready_ = use_intra_process_ && (wait_set->subscriptions[wait_set_intra_process_subscription_index_] == intra_process_subscription_handle_.get()); - event_ready_ = (wait_set->events[wait_set_event_index_] == event_handle_.get()); - intra_process_event_ready_ = - (wait_set->events[wait_set_intra_process_event_index_] == intra_process_event_handle_.get()); - return subscription_ready_ || intra_process_subscription_ready_ || - event_ready_ || intra_process_event_ready_; + return subscription_ready_ || intra_process_subscription_ready_; } void @@ -291,18 +234,6 @@ SubscriptionBase::execute() rcl_reset_error(); } } - - if (event_ready_) { - // rcl_take_event(); - auto example_event = ResourceStatusEvent::LIVELINESS_CHANGED; - handle_event(example_event); - } - - if (intra_process_event_ready_) { - // rcl_take_event(); - auto example_event = ResourceStatusEvent::LIVELINESS_CHANGED; - handle_event(example_event); - } } const rosidl_message_type_support_t & diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 492d2991e3..480f869868 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -207,14 +207,13 @@ void TimeSource::create_clock_sub() auto allocator = std::make_shared(); auto cb = std::bind(&TimeSource::clock_cb, this, std::placeholders::_1); - clock_subscription_ = rclcpp::create_subscription< - MessageT, decltype(cb), ResourceStatusEventCallbackType, Alloc, MessageT, SubscriptionT - >( + clock_subscription_ = rclcpp::create_subscription( node_topics_.get(), topic_name, std::move(cb), rmw_qos_profile_default, - {}, + rclcpp::SubscriptionEventCallbacks(), group, false, false, diff --git a/rclcpp/test/test_time_source.cpp b/rclcpp/test/test_time_source.cpp index 99d39eb92c..fd4a915e7e 100644 --- a/rclcpp/test/test_time_source.cpp +++ b/rclcpp/test/test_time_source.cpp @@ -58,8 +58,7 @@ class TestTimeSource : public ::testing::Test void trigger_clock_changes( rclcpp::Node::SharedPtr node) { - auto clock_pub = node->create_publisher("clock", - rmw_qos_profile_default); + auto clock_pub = node->create_publisher("clock"); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node); diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index ee17a4a884..41734eeae0 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -205,7 +205,8 @@ class TestClient : public ::testing::Test action_name, allocator, &status_topic_name); ASSERT_EQ(RCL_RET_OK, ret); status_publisher = server_node->create_publisher( - status_topic_name, rcl_action_qos_profile_status_default); + status_topic_name, nullptr, rclcpp::PublisherOptions<>().publisher_qos_profile( + rcl_action_qos_profile_status_default)); ASSERT_TRUE(status_publisher != nullptr); allocator.deallocate(status_topic_name, allocator.state); server_executor.add_node(server_node); diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 09890f3ffd..deb023cbac 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -32,7 +32,7 @@ #include "rclcpp/client.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/context.hpp" -#include "rclcpp/event.hpp" +#include "rclcpp/graph_event.hpp" #include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/message_memory_strategy.hpp" diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index 4165ad0b27..10eeda9479 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -60,12 +60,11 @@ LifecycleNode::create_publisher( using PublisherT = rclcpp_lifecycle::LifecyclePublisher; // create regular publisher in rclcpp::Node - return rclcpp::create_publisher( + return rclcpp::create_publisher( this->node_topics_.get(), topic_name, qos_profile, - {}, + rclcpp::PublisherEventCallbacks(), nullptr, use_intra_process_comms_, allocator); @@ -96,13 +95,12 @@ LifecycleNode::create_subscription( msg_mem_strat = MessageMemoryStrategy::create_default(); } - return rclcpp::create_subscription( + return rclcpp::create_subscription( this->node_topics_.get(), topic_name, std::forward(callback), qos_profile, - {}, + rclcpp::SubscriptionEventCallbacks(), group, ignore_local_publications, use_intra_process_comms_, @@ -184,9 +182,8 @@ LifecycleNode::create_service( const rmw_qos_profile_t & qos_profile, rclcpp::callback_group::CallbackGroup::SharedPtr group) { - return rclcpp::create_service( - node_base_, node_services_, service_name, std::forward(callback), qos_profile, {}, - group); + return rclcpp::create_service( + node_base_, node_services_, service_name, std::forward(callback), qos_profile, group); } template diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp index 3bf31142e3..10096a84c4 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp @@ -61,10 +61,10 @@ class LifecyclePublisher : public LifecyclePublisherInterface, rclcpp::node_interfaces::NodeBaseInterface * node_base, const std::string & topic, const rcl_publisher_options_t & publisher_options, - rclcpp::ResourceStatusEventCallbackType event_callback, + const rclcpp::PublisherEventCallbacks event_callbacks, std::shared_ptr allocator) : rclcpp::Publisher( - node_base, topic, publisher_options, event_callback, allocator), + node_base, topic, publisher_options, event_callbacks, allocator), enabled_(false), logger_(rclcpp::get_logger("LifecyclePublisher")) { From cc8bb54d27604fe4c9601982b35ae83ef20daefe Mon Sep 17 00:00:00 2001 From: Miaofei Date: Tue, 12 Mar 2019 17:51:11 -0700 Subject: [PATCH 06/27] improve usability of the SubscriptionOptions and PublisherOptions classes Signed-off-by: Miaofei --- rclcpp/include/rclcpp/node_impl.hpp | 4 ++-- rclcpp/include/rclcpp/publisher.hpp | 4 ++-- rclcpp/include/rclcpp/publisher_options.hpp | 19 +++++++++++++---- rclcpp/include/rclcpp/subscription.hpp | 4 ++-- .../include/rclcpp/subscription_options.hpp | 21 ++++++++++++++----- rclcpp_action/test/test_client.cpp | 5 ++--- 6 files changed, 39 insertions(+), 18 deletions(-) diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 82ce3426af..140b2dcf6e 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -78,7 +78,7 @@ Node::create_publisher( return rclcpp::create_publisher( this->node_topics_.get(), extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), - options.publisher_qos_profile(), + options.qos_profile(), options.event_callbacks(), group, this->get_node_options().use_intra_process_comms(), @@ -112,7 +112,7 @@ Node::create_subscription( this->node_topics_.get(), extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), std::forward(callback), - options.subscription_qos_profile(), + options.qos_profile(), options.event_callbacks(), group, options.ignore_local_publications(), diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index e62df429a9..3b13e4d65d 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -122,8 +122,8 @@ class PublisherBase get_publisher_handle() const; // RCLCPP_PUBLIC - template void - add_event_handle(const EventCallbackT & callback, const EventTypeEnum event_type) + template void + add_event_handle(const EventCallbackT & callback, const rcl_publisher_event_type_t event_type) { event_handles_.emplace_back(std::make_shared>( callback, diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp index ea12617e34..f3b7cd1daf 100644 --- a/rclcpp/include/rclcpp/publisher_options.hpp +++ b/rclcpp/include/rclcpp/publisher_options.hpp @@ -54,7 +54,11 @@ class PublisherOptions * \param[in] allocator allocator to use in construction of PublisherOptions. */ RCLCPP_PUBLIC - explicit PublisherOptions() = default; + PublisherOptions() = default; + + RCLCPP_PUBLIC + PublisherOptions(const rmw_qos_profile_t & qos_profile) : + publisher_qos_profile_(qos_profile) {} /// Destructor. RCLCPP_PUBLIC @@ -74,7 +78,14 @@ class PublisherOptions /// Return a reference to the publisher_qos_profile QoS. RCLCPP_PUBLIC const rmw_qos_profile_t & - publisher_qos_profile() const + qos_profile() const + { + return publisher_qos_profile_; + } + + RCLCPP_PUBLIC + rmw_qos_profile_t & + qos_profile() { return publisher_qos_profile_; } @@ -92,8 +103,8 @@ class PublisherOptions } RCLCPP_PUBLIC - size_t - qos_history_depth() const + size_t & + qos_history_depth() { return publisher_qos_profile_.depth; } diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index d266982e9d..546f35d957 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -104,8 +104,8 @@ class SubscriptionBase : public Waitable get_intra_process_subscription_handle() const; // RCLCPP_PUBLIC - template void - add_event_handle(const EventCallbackT & callback, const EventTypeEnum event_type) + template void + add_event_handle(const EventCallbackT & callback, const rcl_subscription_event_type_t event_type) { event_handles_.emplace_back(std::make_shared>( callback, diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index 076be2b11f..338ab0cda4 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -51,7 +51,11 @@ class SubscriptionOptions * \param[in] allocator allocator to use in construction of SubscriptionOptions. */ RCLCPP_PUBLIC - explicit SubscriptionOptions() = default; + SubscriptionOptions() = default; + + RCLCPP_PUBLIC + SubscriptionOptions(const rmw_qos_profile_t & qos_profile) : + subscription_qos_profile_(qos_profile) {} /// Destructor. RCLCPP_PUBLIC @@ -71,7 +75,14 @@ class SubscriptionOptions /// Return a reference to the subscription_qos_profile QoS. RCLCPP_PUBLIC const rmw_qos_profile_t & - subscription_qos_profile() const + qos_profile() const + { + return subscription_qos_profile_; + } + + RCLCPP_PUBLIC + rmw_qos_profile_t & + qos_profile() { return subscription_qos_profile_; } @@ -82,15 +93,15 @@ class SubscriptionOptions */ RCLCPP_PUBLIC SubscriptionOptions & - subscription_qos_profile(const rmw_qos_profile_t & subscription_qos_profile) + qos_profile(const rmw_qos_profile_t & subscription_qos_profile) { subscription_qos_profile_ = subscription_qos_profile; return *this; } RCLCPP_PUBLIC - size_t - qos_history_depth() const + size_t & + qos_history_depth() { return subscription_qos_profile_.depth; } diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index 41734eeae0..4b03a9432a 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -204,9 +204,8 @@ class TestClient : public ::testing::Test ret = rcl_action_get_status_topic_name( action_name, allocator, &status_topic_name); ASSERT_EQ(RCL_RET_OK, ret); - status_publisher = server_node->create_publisher( - status_topic_name, nullptr, rclcpp::PublisherOptions<>().publisher_qos_profile( - rcl_action_qos_profile_status_default)); + status_publisher = server_node->create_publisher(status_topic_name, + nullptr, rclcpp::PublisherOptions<>(rcl_action_qos_profile_status_default)); ASSERT_TRUE(status_publisher != nullptr); allocator.deallocate(status_topic_name, allocator.state); server_executor.add_node(server_node); From afca6f4213e13f4a675827cd1cd48df7d736942b Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 4 Mar 2019 11:11:07 -0800 Subject: [PATCH 07/27] Attempt to fix cppcheck (#646) Signed-off-by: Shane Loretz Signed-off-by: Miaofei --- rclcpp/src/rclcpp/clock.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/rclcpp/src/rclcpp/clock.cpp b/rclcpp/src/rclcpp/clock.cpp index f21c353a1d..0aaa0c86bc 100644 --- a/rclcpp/src/rclcpp/clock.cpp +++ b/rclcpp/src/rclcpp/clock.cpp @@ -130,10 +130,15 @@ Clock::create_jump_callback( rclcpp::Clock::on_time_jump, handler); if (RCL_RET_OK != ret) { delete handler; - handler = NULL; + handler = nullptr; rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to add time jump callback"); } + if (nullptr == handler) { + // imposible to reach here; added to make cppcheck happy + return nullptr; + } + // *INDENT-OFF* // create shared_ptr that removes the callback automatically when all copies are destructed return rclcpp::JumpHandler::SharedPtr(handler, [this](rclcpp::JumpHandler * handler) noexcept { From ec4476e2cbed05e95ab5848da04cf4a512cdab85 Mon Sep 17 00:00:00 2001 From: Michael Jeronimo Date: Wed, 6 Mar 2019 13:12:38 -0800 Subject: [PATCH 08/27] Add a method to the LifecycleNode class to get the logging interface (#652) There are getters for the other interfaces, but the logging interface appears to have been overlooked. Signed-off-by: Michael Jeronimo Signed-off-by: Miaofei --- .../include/rclcpp_lifecycle/lifecycle_node.hpp | 5 +++++ rclcpp_lifecycle/src/lifecycle_node.cpp | 6 ++++++ 2 files changed, 11 insertions(+) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index deb023cbac..77f155b418 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -358,6 +358,11 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, rclcpp::node_interfaces::NodeGraphInterface::SharedPtr get_node_graph_interface(); + /// Return the Node's internal NodeLoggingInterface implementation. + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr + get_node_logging_interface(); + /// Return the Node's internal NodeTimersInterface implementation. RCLCPP_LIFECYCLE_PUBLIC rclcpp::node_interfaces::NodeTimersInterface::SharedPtr diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 521b7e3128..ac508851cf 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -268,6 +268,12 @@ LifecycleNode::get_node_graph_interface() return node_graph_; } +rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr +LifecycleNode::get_node_logging_interface() +{ + return node_logging_; +} + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr LifecycleNode::get_node_timers_interface() { From 40ba3c6d538a69038053b483eb66b62ae93c683b Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 11 Mar 2019 21:12:47 -0700 Subject: [PATCH 09/27] update to use separated action types (#601) * match renamed action types * fix action type casting * rename type/field to use correct term * rename custom GoalID type to avoid naming collision, update types using unique_identifier_msgs * remove obsolete comments * change signature of set_succeeded / set_canceled * change signature of on_terminal_state_(uuid_, result_msg);set_succeeded / set_canceled * change signature of set_aborted * change signature of publish_feedback * update another test Signed-off-by: Miaofei --- .../include/rclcpp_action/client.hpp | 78 ++++++++-------- .../rclcpp_action/client_goal_handle.hpp | 21 +++-- .../rclcpp_action/client_goal_handle_impl.hpp | 10 +- .../include/rclcpp_action/server.hpp | 71 +++++++-------- .../rclcpp_action/server_goal_handle.hpp | 50 +++++----- rclcpp_action/include/rclcpp_action/types.hpp | 18 ++-- rclcpp_action/src/client.cpp | 4 +- rclcpp_action/src/server.cpp | 18 ++-- rclcpp_action/src/types.cpp | 6 +- rclcpp_action/test/test_client.cpp | 74 +++++++-------- rclcpp_action/test/test_server.cpp | 91 +++++++++---------- 11 files changed, 221 insertions(+), 220 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 0ce4b4a948..89db1e39f9 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -148,7 +148,7 @@ class ClientBase : public rclcpp::Waitable /// \internal RCLCPP_ACTION_PUBLIC virtual - GoalID + GoalUUID generate_goal_id(); /// \internal @@ -260,10 +260,10 @@ class Client : public ClientBase using Goal = typename ActionT::Goal; using Feedback = typename ActionT::Feedback; using GoalHandle = ClientGoalHandle; - using Result = typename GoalHandle::Result; + using WrappedResult = typename GoalHandle::WrappedResult; using FeedbackCallback = typename ClientGoalHandle::FeedbackCallback; - using CancelRequest = typename ActionT::CancelGoalService::Request; - using CancelResponse = typename ActionT::CancelGoalService::Response; + using CancelRequest = typename ActionT::Impl::CancelGoalService::Request; + using CancelResponse = typename ActionT::Impl::CancelGoalService::Response; Client( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, @@ -286,26 +286,23 @@ class Client : public ClientBase // Put promise in the heap to move it around. auto promise = std::make_shared>(); std::shared_future future(promise->get_future()); - using GoalRequest = typename ActionT::GoalRequestService::Request; - // auto goal_request = std::make_shared(); - // goal_request->goal_id = this->generate_goal_id(); - // goal_request->goal = goal; - auto goal_request = std::make_shared(goal); - goal_request->action_goal_id.uuid = this->generate_goal_id(); + using GoalRequest = typename ActionT::Impl::SendGoalService::Request; + auto goal_request = std::make_shared(); + goal_request->goal_id.uuid = this->generate_goal_id(); + goal_request->goal = goal; this->send_goal_request( std::static_pointer_cast(goal_request), [this, goal_request, callback, ignore_result, promise]( std::shared_ptr response) mutable { - using GoalResponse = typename ActionT::GoalRequestService::Response; + using GoalResponse = typename ActionT::Impl::SendGoalService::Response; auto goal_response = std::static_pointer_cast(response); if (!goal_response->accepted) { promise->set_value(nullptr); return; } GoalInfo goal_info; - // goal_info.goal_id = goal_request->goal_id; - goal_info.goal_id.uuid = goal_request->action_goal_id.uuid; + goal_info.goal_id.uuid = goal_request->goal_id.uuid; goal_info.stamp = goal_response->stamp; // Do not use std::make_shared as friendship cannot be forwarded. std::shared_ptr goal_handle(new GoalHandle(goal_info, callback)); @@ -324,7 +321,7 @@ class Client : public ClientBase return future; } - std::shared_future + std::shared_future async_get_result(typename GoalHandle::SharedPtr goal_handle) { std::lock_guard lock(goal_handles_mutex_); @@ -406,7 +403,7 @@ class Client : public ClientBase std::shared_ptr create_goal_response() const override { - using GoalResponse = typename ActionT::GoalRequestService::Response; + using GoalResponse = typename ActionT::Impl::SendGoalService::Response; return std::shared_ptr(new GoalResponse()); } @@ -414,7 +411,7 @@ class Client : public ClientBase std::shared_ptr create_result_response() const override { - using GoalResultResponse = typename ActionT::GoalResultService::Response; + using GoalResultResponse = typename ActionT::Impl::GetResultService::Response; return std::shared_ptr(new GoalResultResponse()); } @@ -429,9 +426,8 @@ class Client : public ClientBase std::shared_ptr create_feedback_message() const override { - // using FeedbackMessage = typename ActionT::FeedbackMessage; - // return std::shared_ptr(new FeedbackMessage()); - return std::shared_ptr(new Feedback()); + using FeedbackMessage = typename ActionT::Impl::FeedbackMessage; + return std::shared_ptr(new FeedbackMessage()); } /// \internal @@ -439,13 +435,10 @@ class Client : public ClientBase handle_feedback_message(std::shared_ptr message) override { std::lock_guard guard(goal_handles_mutex_); - // using FeedbackMessage = typename ActionT::FeedbackMessage; - // typename FeedbackMessage::SharedPtr feedback_message = - // std::static_pointer_cast(message); - typename Feedback::SharedPtr feedback_message = - std::static_pointer_cast(message); - // const GoalID & goal_id = feedback_message->goal_id; - const GoalID & goal_id = feedback_message->action_goal_id.uuid; + using FeedbackMessage = typename ActionT::Impl::FeedbackMessage; + typename FeedbackMessage::SharedPtr feedback_message = + std::static_pointer_cast(message); + const GoalUUID & goal_id = feedback_message->goal_id.uuid; if (goal_handles_.count(goal_id) == 0) { RCLCPP_DEBUG( this->get_logger(), @@ -453,15 +446,16 @@ class Client : public ClientBase return; } typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id]; - // goal_handle->call_feedback_callback(goal_handle, feedback_message->feedback); - goal_handle->call_feedback_callback(goal_handle, feedback_message); + auto feedback = std::make_shared(); + *feedback = feedback_message->feedback; + goal_handle->call_feedback_callback(goal_handle, feedback); } /// \internal std::shared_ptr create_status_message() const override { - using GoalStatusMessage = typename ActionT::GoalStatusMessage; + using GoalStatusMessage = typename ActionT::Impl::GoalStatusMessage; return std::shared_ptr(new GoalStatusMessage()); } @@ -470,11 +464,10 @@ class Client : public ClientBase handle_status_message(std::shared_ptr message) override { std::lock_guard guard(goal_handles_mutex_); - using GoalStatusMessage = typename ActionT::GoalStatusMessage; + using GoalStatusMessage = typename ActionT::Impl::GoalStatusMessage; auto status_message = std::static_pointer_cast(message); for (const GoalStatus & status : status_message->status_list) { - // const GoalID & goal_id = status.goal_info.goal_id; - const GoalID & goal_id = status.goal_info.goal_id.uuid; + const GoalUUID & goal_id = status.goal_info.goal_id.uuid; if (goal_handles_.count(goal_id) == 0) { RCLCPP_DEBUG( this->get_logger(), @@ -498,21 +491,22 @@ class Client : public ClientBase void make_result_aware(typename GoalHandle::SharedPtr goal_handle) { - using GoalResultRequest = typename ActionT::GoalResultService::Request; + using GoalResultRequest = typename ActionT::Impl::GetResultService::Request; auto goal_result_request = std::make_shared(); - // goal_result_request.goal_id = goal_handle->get_goal_id(); - goal_result_request->action_goal_id.uuid = goal_handle->get_goal_id(); + goal_result_request->goal_id.uuid = goal_handle->get_goal_id(); this->send_result_request( std::static_pointer_cast(goal_result_request), [goal_handle, this](std::shared_ptr response) mutable { // Wrap the response in a struct with the fields a user cares about - Result result; - using GoalResultResponse = typename ActionT::GoalResultService::Response; - result.response = std::static_pointer_cast(response); - result.goal_id = goal_handle->get_goal_id(); - result.code = static_cast(result.response->action_status); - goal_handle->set_result(result); + WrappedResult wrapped_result; + using GoalResultResponse = typename ActionT::Impl::GetResultService::Response; + auto result_response = std::static_pointer_cast(response); + wrapped_result.result = std::make_shared(); + *wrapped_result.result = result_response->result; + wrapped_result.goal_id = goal_handle->get_goal_id(); + wrapped_result.code = static_cast(result_response->status); + goal_handle->set_result(wrapped_result); std::lock_guard lock(goal_handles_mutex_); goal_handles_.erase(goal_handle->get_goal_id()); }); @@ -536,7 +530,7 @@ class Client : public ClientBase return future; } - std::map goal_handles_; + std::map goal_handles_; std::mutex goal_handles_mutex_; }; } // namespace rclcpp_action diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index 03ae985861..bee86b55c7 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -52,30 +52,31 @@ class ClientGoalHandle RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientGoalHandle) // A wrapper that defines the result of an action - typedef struct Result + typedef struct WrappedResult { /// The unique identifier of the goal - GoalID goal_id; + GoalUUID goal_id; /// A status to indicate if the goal was canceled, aborted, or suceeded ResultCode code; /// User defined fields sent back with an action - typename ActionT::Result::SharedPtr response; - } Result; + typename ActionT::Result::SharedPtr result; + } WrappedResult; using Feedback = typename ActionT::Feedback; using FeedbackCallback = std::function::SharedPtr, const std::shared_ptr)>; + typename ClientGoalHandle::SharedPtr, + const std::shared_ptr)>; virtual ~ClientGoalHandle(); - const GoalID & + const GoalUUID & get_goal_id() const; rclcpp::Time get_goal_stamp() const; - std::shared_future + std::shared_future async_result(); int8_t @@ -108,7 +109,7 @@ class ClientGoalHandle set_status(int8_t status); void - set_result(const Result & result); + set_result(const WrappedResult & wrapped_result); void invalidate(); @@ -116,8 +117,8 @@ class ClientGoalHandle GoalInfo info_; bool is_result_aware_{false}; - std::promise result_promise_; - std::shared_future result_future_; + std::promise result_promise_; + std::shared_future result_future_; FeedbackCallback feedback_callback_{nullptr}; int8_t status_{GoalStatus::STATUS_ACCEPTED}; diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index 3cf7960a33..73f850dd47 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -39,7 +39,7 @@ ClientGoalHandle::~ClientGoalHandle() } template -const GoalID & +const GoalUUID & ClientGoalHandle::get_goal_id() const { // return info_.goal_id; @@ -54,7 +54,7 @@ ClientGoalHandle::get_goal_stamp() const } template -std::shared_future::Result> +std::shared_future::WrappedResult> ClientGoalHandle::async_result() { std::lock_guard guard(handle_mutex_); @@ -66,11 +66,11 @@ ClientGoalHandle::async_result() template void -ClientGoalHandle::set_result(const Result & result) +ClientGoalHandle::set_result(const WrappedResult & wrapped_result) { std::lock_guard guard(handle_mutex_); - status_ = static_cast(result.code); - result_promise_.set_value(result); + status_ = static_cast(wrapped_result.code); + result_promise_.set_value(wrapped_result); } template diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index b1bb47a4f8..4a9cff0b1d 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -146,7 +146,7 @@ class ServerBase : public rclcpp::Waitable RCLCPP_ACTION_PUBLIC virtual std::pair> - call_handle_goal_callback(GoalID &, std::shared_ptr request) = 0; + call_handle_goal_callback(GoalUUID &, std::shared_ptr request) = 0; // ServerBase will determine which goal ids are being cancelled, and then call this function for // each goal id. @@ -155,13 +155,13 @@ class ServerBase : public rclcpp::Waitable RCLCPP_ACTION_PUBLIC virtual CancelResponse - call_handle_cancel_callback(const GoalID & uuid) = 0; + call_handle_cancel_callback(const GoalUUID & uuid) = 0; /// Given a goal request message, return the UUID contained within. /// \internal RCLCPP_ACTION_PUBLIC virtual - GoalID + GoalUUID get_goal_id_from_goal_request(void * message) = 0; /// Create an empty goal request message so it can be taken from a lower layer. @@ -178,13 +178,13 @@ class ServerBase : public rclcpp::Waitable void call_goal_accepted_callback( std::shared_ptr rcl_goal_handle, - GoalID uuid, std::shared_ptr goal_request_message) = 0; + GoalUUID uuid, std::shared_ptr goal_request_message) = 0; /// Given a result request message, return the UUID contained within. /// \internal RCLCPP_ACTION_PUBLIC virtual - GoalID + GoalUUID get_goal_id_from_result_request(void * message) = 0; /// Create an empty goal request message so it can be taken from a lower layer. @@ -214,7 +214,7 @@ class ServerBase : public rclcpp::Waitable /// \internal RCLCPP_ACTION_PUBLIC void - publish_result(const GoalID & uuid, std::shared_ptr result_msg); + publish_result(const GoalUUID & uuid, std::shared_ptr result_msg); /// \internal RCLCPP_ACTION_PUBLIC @@ -272,7 +272,7 @@ class Server : public ServerBase, public std::enable_shared_from_this)>; + const GoalUUID &, std::shared_ptr)>; /// Signature of a callback that accepts or rejects requests to cancel a goal. using CancelCallback = std::function>)>; /// Signature of a callback that is used to notify when the goal has been accepted. @@ -335,16 +335,14 @@ class Server : public ServerBase, public std::enable_shared_from_this> - call_handle_goal_callback(GoalID & uuid, std::shared_ptr message) override + call_handle_goal_callback(GoalUUID & uuid, std::shared_ptr message) override { - // TODO(sloretz) update and remove assert when IDL pipeline allows nesting user's type - static_assert( - std::is_same::value, - "Assuming user fields were merged with goal request fields"); - GoalResponse user_response = handle_goal_( - uuid, std::static_pointer_cast(message)); - - auto ros_response = std::make_shared(); + auto request = std::static_pointer_cast< + typename ActionT::Impl::SendGoalService::Request>(message); + auto goal = std::shared_ptr(request, &request->goal); + GoalResponse user_response = handle_goal_(uuid, goal); + + auto ros_response = std::make_shared(); ros_response->accepted = GoalResponse::ACCEPT_AND_EXECUTE == user_response || GoalResponse::ACCEPT_AND_DEFER == user_response; return std::make_pair(user_response, ros_response); @@ -352,7 +350,7 @@ class Server : public ServerBase, public std::enable_shared_from_this lock(goal_handles_mutex_); CancelResponse resp = CancelResponse::REJECT; @@ -373,13 +371,13 @@ class Server : public ServerBase, public std::enable_shared_from_this rcl_goal_handle, - GoalID uuid, std::shared_ptr goal_request_message) override + GoalUUID uuid, std::shared_ptr goal_request_message) override { std::shared_ptr> goal_handle; std::weak_ptr> weak_this = this->shared_from_this(); - std::function)> on_terminal_state = - [weak_this](const GoalID & uuid, std::shared_ptr result_message) + std::function)> on_terminal_state = + [weak_this](const GoalUUID & uuid, std::shared_ptr result_message) { std::shared_ptr> shared_this = weak_this.lock(); if (!shared_this) { @@ -396,8 +394,8 @@ class Server : public ServerBase, public std::enable_shared_from_thisgoal_handles_.erase(uuid); }; - std::function on_executing = - [weak_this](const GoalID & uuid) + std::function on_executing = + [weak_this](const GoalUUID & uuid) { std::shared_ptr> shared_this = weak_this.lock(); if (!shared_this) { @@ -408,8 +406,8 @@ class Server : public ServerBase, public std::enable_shared_from_thispublish_status(); }; - std::function)> publish_feedback = - [weak_this](std::shared_ptr feedback_msg) + std::function)> publish_feedback = + [weak_this](std::shared_ptr feedback_msg) { std::shared_ptr> shared_this = weak_this.lock(); if (!shared_this) { @@ -418,11 +416,12 @@ class Server : public ServerBase, public std::enable_shared_from_thispublish_feedback(std::static_pointer_cast(feedback_msg)); }; + auto request = std::static_pointer_cast< + const typename ActionT::Impl::SendGoalService::Request>(goal_request_message); + auto goal = std::shared_ptr(request, &request->goal); goal_handle.reset( new ServerGoalHandle( - rcl_goal_handle, uuid, - std::static_pointer_cast(goal_request_message), - on_terminal_state, on_executing, publish_feedback)); + rcl_goal_handle, uuid, goal, on_terminal_state, on_executing, publish_feedback)); { std::lock_guard lock(goal_handles_mutex_); goal_handles_[uuid] = goal_handle; @@ -431,41 +430,41 @@ class Server : public ServerBase, public std::enable_shared_from_this(message)->action_goal_id.uuid; + static_cast(message)->goal_id.uuid; } /// \internal std::shared_ptr create_goal_request() override { - return std::shared_ptr(new typename ActionT::GoalRequestService::Request()); + return std::shared_ptr(new typename ActionT::Impl::SendGoalService::Request()); } /// \internal - GoalID + GoalUUID get_goal_id_from_result_request(void * message) override { return - static_cast(message)->action_goal_id.uuid; + static_cast(message)->goal_id.uuid; } /// \internal std::shared_ptr create_result_request() override { - return std::shared_ptr(new typename ActionT::GoalResultService::Request()); + return std::shared_ptr(new typename ActionT::Impl::GetResultService::Request()); } /// \internal std::shared_ptr create_result_response(decltype(action_msgs::msg::GoalStatus::status) status) override { - auto result = std::make_shared(); - result->action_status = status; + auto result = std::make_shared(); + result->status = status; return std::static_pointer_cast(result); } @@ -480,7 +479,7 @@ class Server : public ServerBase, public std::enable_shared_from_this>; /// A map of goal id to goal handle weak pointers. /// This is used to provide a goal handle to handle_cancel. - std::unordered_map goal_handles_; + std::unordered_map goal_handles_; std::mutex goal_handles_mutex_; }; } // namespace rclcpp_action diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index 5b06fc6059..0534d933f5 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -147,8 +147,10 @@ class ServerGoalHandle : public ServerGoalHandleBase void publish_feedback(std::shared_ptr feedback_msg) { - feedback_msg->action_goal_id.uuid = uuid_; - publish_feedback_(feedback_msg); + auto feedback_message = std::make_shared(); + feedback_message->goal_id.uuid = uuid_; + feedback_message->feedback = *feedback_msg; + publish_feedback_(feedback_message); } // TODO(sloretz) which exception is raised? @@ -165,8 +167,10 @@ class ServerGoalHandle : public ServerGoalHandleBase set_aborted(typename ActionT::Result::SharedPtr result_msg) { _set_aborted(); - result_msg->action_status = action_msgs::msg::GoalStatus::STATUS_ABORTED; - on_terminal_state_(uuid_, result_msg); + auto response = std::make_shared(); + response->status = action_msgs::msg::GoalStatus::STATUS_ABORTED; + response->result = *result_msg; + on_terminal_state_(uuid_, response); } /// Indicate that a goal has been reached. @@ -182,8 +186,10 @@ class ServerGoalHandle : public ServerGoalHandleBase set_succeeded(typename ActionT::Result::SharedPtr result_msg) { _set_succeeded(); - result_msg->action_status = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED; - on_terminal_state_(uuid_, result_msg); + auto response = std::make_shared(); + response->status = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED; + response->result = *result_msg; + on_terminal_state_(uuid_, response); } /// Indicate that a goal has been canceled. @@ -199,8 +205,10 @@ class ServerGoalHandle : public ServerGoalHandleBase set_canceled(typename ActionT::Result::SharedPtr result_msg) { _set_canceled(); - result_msg->action_status = action_msgs::msg::GoalStatus::STATUS_CANCELED; - on_terminal_state_(uuid_, result_msg); + auto response = std::make_shared(); + response->status = action_msgs::msg::GoalStatus::STATUS_CANCELED; + response->result = *result_msg; + on_terminal_state_(uuid_, response); } /// Indicate that the server is starting to execute a goal. @@ -215,7 +223,7 @@ class ServerGoalHandle : public ServerGoalHandleBase on_executing_(uuid_); } - /// Get the original request message describing the goal. + /// Get the user provided message describing the goal. const std::shared_ptr get_goal() const { @@ -223,7 +231,7 @@ class ServerGoalHandle : public ServerGoalHandleBase } /// Get the unique identifier of the goal - const GoalID & + const GoalUUID & get_goal_id() const { return uuid_; @@ -233,8 +241,8 @@ class ServerGoalHandle : public ServerGoalHandleBase { // Cancel goal if handle was allowed to destruct without reaching a terminal state if (try_canceling()) { - auto null_result = std::make_shared(); - null_result->action_status = action_msgs::msg::GoalStatus::STATUS_CANCELED; + auto null_result = std::make_shared(); + null_result->status = action_msgs::msg::GoalStatus::STATUS_CANCELED; on_terminal_state_(uuid_, null_result); } } @@ -243,11 +251,11 @@ class ServerGoalHandle : public ServerGoalHandleBase /// \internal ServerGoalHandle( std::shared_ptr rcl_handle, - GoalID uuid, + GoalUUID uuid, std::shared_ptr goal, - std::function)> on_terminal_state, - std::function on_executing, - std::function)> publish_feedback + std::function)> on_terminal_state, + std::function on_executing, + std::function)> publish_feedback ) : ServerGoalHandleBase(rcl_handle), goal_(goal), uuid_(uuid), on_terminal_state_(on_terminal_state), on_executing_(on_executing), @@ -255,17 +263,17 @@ class ServerGoalHandle : public ServerGoalHandleBase { } - /// The original request message describing the goal. + /// The user provided message describing the goal. const std::shared_ptr goal_; /// A unique id for the goal request. - const GoalID uuid_; + const GoalUUID uuid_; friend Server; - std::function)> on_terminal_state_; - std::function on_executing_; - std::function)> publish_feedback_; + std::function)> on_terminal_state_; + std::function on_executing_; + std::function)> publish_feedback_; }; } // namespace rclcpp_action diff --git a/rclcpp_action/include/rclcpp_action/types.hpp b/rclcpp_action/include/rclcpp_action/types.hpp index badfdbdff3..addf4a8307 100644 --- a/rclcpp_action/include/rclcpp_action/types.hpp +++ b/rclcpp_action/include/rclcpp_action/types.hpp @@ -29,34 +29,34 @@ namespace rclcpp_action { -using GoalID = std::array; +using GoalUUID = std::array; using GoalStatus = action_msgs::msg::GoalStatus; using GoalInfo = action_msgs::msg::GoalInfo; /// Convert a goal id to a human readable string. RCLCPP_ACTION_PUBLIC std::string -to_string(const GoalID & goal_id); +to_string(const GoalUUID & goal_id); // Convert C++ GoalID to rcl_action_goal_info_t RCLCPP_ACTION_PUBLIC void -convert(const GoalID & goal_id, rcl_action_goal_info_t * info); +convert(const GoalUUID & goal_id, rcl_action_goal_info_t * info); // Convert rcl_action_goal_info_t to C++ GoalID RCLCPP_ACTION_PUBLIC void -convert(const rcl_action_goal_info_t & info, GoalID * goal_id); +convert(const rcl_action_goal_info_t & info, GoalUUID * goal_id); } // namespace rclcpp_action namespace std { template<> -struct less +struct less { bool operator()( - const rclcpp_action::GoalID & lhs, - const rclcpp_action::GoalID & rhs) const + const rclcpp_action::GoalUUID & lhs, + const rclcpp_action::GoalUUID & rhs) const { return lhs < rhs; } @@ -64,9 +64,9 @@ struct less /// Hash a goal id so it can be used as a key in std::unordered_map template<> -struct hash +struct hash { - size_t operator()(const rclcpp_action::GoalID & uuid) const noexcept + size_t operator()(const rclcpp_action::GoalUUID & uuid) const noexcept { // TODO(sloretz) Use someone else's hash function and cite it size_t result = 0; diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index e3f1198dcc..3a34f35195 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -366,10 +366,10 @@ ClientBase::send_cancel_request(std::shared_ptr request, ResponseCallback pimpl_->pending_cancel_responses[sequence_number] = callback; } -GoalID +GoalUUID ClientBase::generate_goal_id() { - GoalID goal_id; + GoalUUID goal_id; // TODO(hidmic): Do something better than this for UUID generation. // std::generate( // goal_id.uuid.begin(), goal_id.uuid.end(), diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index ac787a4bcc..025495db54 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -28,7 +28,7 @@ #include using rclcpp_action::ServerBase; -using rclcpp_action::GoalID; +using rclcpp_action::GoalUUID; namespace rclcpp_action { @@ -62,11 +62,11 @@ class ServerBaseImpl bool goal_expired_ = false; // Results to be kept until the goal expires after reaching a terminal state - std::unordered_map> goal_results_; + std::unordered_map> goal_results_; // Requests for results are kept until a result becomes available - std::unordered_map> result_requests_; + std::unordered_map> result_requests_; // rcl goal handles are kept so api to send result doesn't try to access freed memory - std::unordered_map> goal_handles_; + std::unordered_map> goal_handles_; rclcpp::Logger logger_; }; @@ -228,7 +228,7 @@ ServerBase::execute_goal_request_received() rclcpp::exceptions::throw_from_rcl_error(ret); } - GoalID uuid = get_goal_id_from_goal_request(message.get()); + GoalUUID uuid = get_goal_id_from_goal_request(message.get()); convert(uuid, &goal_info); // Call user's callback, getting the user's response and a ros message to send back @@ -339,7 +339,7 @@ ServerBase::execute_cancel_request_received() // For each canceled goal, call cancel callback for (size_t i = 0; i < goals.size; ++i) { const rcl_action_goal_info_t & goal_info = goals.data[i]; - GoalID uuid; + GoalUUID uuid; convert(goal_info, &uuid); auto response_code = call_handle_cancel_callback(uuid); if (CancelResponse::ACCEPT == response_code) { @@ -388,7 +388,7 @@ ServerBase::execute_result_request_received() std::shared_ptr result_response; // check if the goal exists - GoalID uuid = get_goal_id_from_result_request(result_request.get()); + GoalUUID uuid = get_goal_id_from_result_request(result_request.get()); rcl_action_goal_info_t goal_info; convert(uuid, &goal_info); bool goal_exists; @@ -433,7 +433,7 @@ ServerBase::execute_check_expired_goals() rclcpp::exceptions::throw_from_rcl_error(ret); } else if (num_expired) { // A goal expired! - GoalID uuid; + GoalUUID uuid; convert(expired_goals[0], &uuid); RCLCPP_DEBUG(pimpl_->logger_, "Expired goal %s", to_string(uuid).c_str()); pimpl_->goal_results_.erase(uuid); @@ -497,7 +497,7 @@ ServerBase::publish_status() } void -ServerBase::publish_result(const GoalID & uuid, std::shared_ptr result_msg) +ServerBase::publish_result(const GoalUUID & uuid, std::shared_ptr result_msg) { // Check that the goal exists rcl_action_goal_info_t goal_info; diff --git a/rclcpp_action/src/types.cpp b/rclcpp_action/src/types.cpp index cc2779bd18..773702789e 100644 --- a/rclcpp_action/src/types.cpp +++ b/rclcpp_action/src/types.cpp @@ -20,7 +20,7 @@ namespace rclcpp_action { std::string -to_string(const GoalID & goal_id) +to_string(const GoalUUID & goal_id) { std::stringstream stream; stream << std::hex; @@ -31,7 +31,7 @@ to_string(const GoalID & goal_id) } void -convert(const GoalID & goal_id, rcl_action_goal_info_t * info) +convert(const GoalUUID & goal_id, rcl_action_goal_info_t * info) { for (size_t i = 0; i < 16; ++i) { info->goal_id.uuid[i] = goal_id[i]; @@ -39,7 +39,7 @@ convert(const GoalID & goal_id, rcl_action_goal_info_t * info) } void -convert(const rcl_action_goal_info_t & info, GoalID * goal_id) +convert(const rcl_action_goal_info_t & info, GoalUUID * goal_id) { for (size_t i = 0; i < 16; ++i) { (*goal_id)[i] = info.goal_id.uuid[i]; diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index 4b03a9432a..3236d2ec1a 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -24,7 +24,6 @@ #include #include #include -#include #include #include #include @@ -55,17 +54,17 @@ class TestClient : public ::testing::Test using ActionType = test_msgs::action::Fibonacci; using ActionGoal = ActionType::Goal; using ActionGoalHandle = rclcpp_action::ClientGoalHandle; - using ActionGoalRequestService = ActionType::GoalRequestService; + using ActionGoalRequestService = ActionType::Impl::SendGoalService; using ActionGoalRequest = ActionGoalRequestService::Request; using ActionGoalResponse = ActionGoalRequestService::Response; - using ActionGoalResultService = ActionType::GoalResultService; + using ActionGoalResultService = ActionType::Impl::GetResultService; using ActionGoalResultRequest = ActionGoalResultService::Request; using ActionGoalResultResponse = ActionGoalResultService::Response; - using ActionCancelGoalService = ActionType::CancelGoalService; - using ActionCancelGoalRequest = ActionType::CancelGoalService::Request; - using ActionCancelGoalResponse = ActionType::CancelGoalService::Response; - using ActionStatusMessage = ActionType::GoalStatusMessage; - using ActionFeedbackMessage = ActionType::Feedback; + using ActionCancelGoalService = ActionType::Impl::CancelGoalService; + using ActionCancelGoalRequest = ActionType::Impl::CancelGoalService::Request; + using ActionCancelGoalResponse = ActionType::Impl::CancelGoalService::Response; + using ActionStatusMessage = ActionType::Impl::GoalStatusMessage; + using ActionFeedbackMessage = ActionType::Impl::FeedbackMessage; using ActionFeedback = ActionType::Feedback; static void SetUpTestCase() @@ -90,9 +89,9 @@ class TestClient : public ::testing::Test ActionGoalResponse::SharedPtr response) { response->stamp = clock.now(); - response->accepted = (request->order >= 0); + response->accepted = (request->goal.order >= 0); if (response->accepted) { - goals[request->action_goal_id.uuid] = {request, response}; + goals[request->goal_id.uuid] = {request, response}; } }); ASSERT_TRUE(goal_service != nullptr); @@ -108,29 +107,30 @@ class TestClient : public ::testing::Test const ActionGoalResultRequest::SharedPtr request, ActionGoalResultResponse::SharedPtr response) { - if (goals.count(request->action_goal_id.uuid) == 1) { - auto goal_request = goals[request->action_goal_id.uuid].first; - auto goal_response = goals[request->action_goal_id.uuid].second; + if (goals.count(request->goal_id.uuid) == 1) { + auto goal_request = goals[request->goal_id.uuid].first; + auto goal_response = goals[request->goal_id.uuid].second; ActionStatusMessage status_message; rclcpp_action::GoalStatus goal_status; - goal_status.goal_info.goal_id.uuid = goal_request->action_goal_id.uuid; + goal_status.goal_info.goal_id.uuid = goal_request->goal_id.uuid; goal_status.goal_info.stamp = goal_response->stamp; goal_status.status = rclcpp_action::GoalStatus::STATUS_EXECUTING; status_message.status_list.push_back(goal_status); status_publisher->publish(status_message); client_executor.spin_once(); ActionFeedbackMessage feedback_message; - feedback_message.action_goal_id.uuid = goal_request->action_goal_id.uuid; - feedback_message.sequence.push_back(0); + feedback_message.goal_id.uuid = goal_request->goal_id.uuid; + feedback_message.feedback.sequence.push_back(0); feedback_publisher->publish(feedback_message); client_executor.spin_once(); - if (goal_request->order > 0) { - feedback_message.sequence.push_back(1); + if (goal_request->goal.order > 0) { + feedback_message.feedback.sequence.push_back(1); feedback_publisher->publish(feedback_message); client_executor.spin_once(); - for (int i = 1; i < goal_request->order; ++i) { - feedback_message.sequence.push_back( - feedback_message.sequence[i] + feedback_message.sequence[i - 1]); + for (int i = 1; i < goal_request->goal.order; ++i) { + feedback_message.feedback.sequence.push_back( + feedback_message.feedback.sequence[i] + + feedback_message.feedback.sequence[i - 1]); feedback_publisher->publish(feedback_message); client_executor.spin_once(); } @@ -139,11 +139,11 @@ class TestClient : public ::testing::Test status_message.status_list[0] = goal_status; status_publisher->publish(status_message); client_executor.spin_once(); - response->sequence = feedback_message.sequence; - response->action_status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED; - goals.erase(request->action_goal_id.uuid); + response->result.sequence = feedback_message.feedback.sequence; + response->status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED; + goals.erase(request->goal_id.uuid); } else { - response->action_status = rclcpp_action::GoalStatus::STATUS_UNKNOWN; + response->status = rclcpp_action::GoalStatus::STATUS_UNKNOWN; } }); ASSERT_TRUE(result_service != nullptr); @@ -159,7 +159,7 @@ class TestClient : public ::testing::Test const ActionCancelGoalRequest::SharedPtr request, ActionCancelGoalResponse::SharedPtr response) { - rclcpp_action::GoalID zero_uuid; + rclcpp_action::GoalUUID zero_uuid; std::fill(zero_uuid.begin(), zero_uuid.end(), 0u); const rclcpp::Time cancel_stamp = request->goal_info.stamp; bool cancel_all = ( @@ -172,11 +172,11 @@ class TestClient : public ::testing::Test auto goal_response = it->second.second; const rclcpp::Time goal_stamp = goal_response->stamp; bool cancel_this = ( - request->goal_info.goal_id.uuid == goal_request->action_goal_id.uuid || + request->goal_info.goal_id.uuid == goal_request->goal_id.uuid || cancel_stamp > goal_stamp); if (cancel_all || cancel_this) { rclcpp_action::GoalStatus goal_status; - goal_status.goal_info.goal_id.uuid = goal_request->action_goal_id.uuid; + goal_status.goal_info.goal_id.uuid = goal_request->goal_id.uuid; goal_status.goal_info.stamp = goal_response->stamp; goal_status.status = rclcpp_action::GoalStatus::STATUS_CANCELED; status_message.status_list.push_back(goal_status); @@ -252,7 +252,7 @@ class TestClient : public ::testing::Test const char * const action_name{"fibonacci_test"}; std::map< - rclcpp_action::GoalID, + rclcpp_action::GoalUUID, std::pair< typename ActionGoalRequest::SharedPtr, typename ActionGoalResponse::SharedPtr>> goals; @@ -305,11 +305,11 @@ TEST_F(TestClient, async_send_goal_and_ignore_feedback_but_wait_for_result) EXPECT_TRUE(goal_handle->is_result_aware()); auto future_result = goal_handle->async_result(); dual_spin_until_future_complete(future_result); - auto result = future_result.get(); - ASSERT_EQ(6ul, result.response->sequence.size()); - EXPECT_EQ(0, result.response->sequence[0]); - EXPECT_EQ(1, result.response->sequence[1]); - EXPECT_EQ(5, result.response->sequence[5]); + auto wrapped_result = future_result.get(); + ASSERT_EQ(6ul, wrapped_result.result->sequence.size()); + EXPECT_EQ(0, wrapped_result.result->sequence[0]); + EXPECT_EQ(1, wrapped_result.result->sequence[1]); + EXPECT_EQ(5, wrapped_result.result->sequence[5]); } TEST_F(TestClient, async_send_goal_with_feedback_and_result) @@ -337,10 +337,10 @@ TEST_F(TestClient, async_send_goal_with_feedback_and_result) EXPECT_TRUE(goal_handle->is_result_aware()); auto future_result = goal_handle->async_result(); dual_spin_until_future_complete(future_result); - auto result = future_result.get(); + auto wrapped_result = future_result.get(); - ASSERT_EQ(5u, result.response->sequence.size()); - EXPECT_EQ(3, result.response->sequence.back()); + ASSERT_EQ(5u, wrapped_result.result->sequence.size()); + EXPECT_EQ(3, wrapped_result.result->sequence.back()); EXPECT_EQ(5, feedback_count); } diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index c46fd614cb..304bbdda30 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -26,7 +26,7 @@ #include "rclcpp_action/server.hpp" using Fibonacci = test_msgs::action::Fibonacci; -using GoalID = rclcpp_action::GoalID; +using GoalUUID = rclcpp_action::GoalUUID; class TestServer : public ::testing::Test { @@ -36,16 +36,16 @@ class TestServer : public ::testing::Test rclcpp::init(0, nullptr); } - std::shared_ptr - send_goal_request(rclcpp::Node::SharedPtr node, GoalID uuid) + std::shared_ptr + send_goal_request(rclcpp::Node::SharedPtr node, GoalUUID uuid) { - auto client = node->create_client( + auto client = node->create_client( "fibonacci/_action/send_goal"); if (!client->wait_for_service(std::chrono::seconds(20))) { throw std::runtime_error("send goal service didn't become available"); } - auto request = std::make_shared(); - request->action_goal_id.uuid = uuid; + auto request = std::make_shared(); + request->goal_id.uuid = uuid; auto future = client->async_send_request(request); if (rclcpp::executor::FutureReturnCode::SUCCESS != rclcpp::spin_until_future_complete(node, future)) @@ -56,14 +56,14 @@ class TestServer : public ::testing::Test } void - send_cancel_request(rclcpp::Node::SharedPtr node, GoalID uuid) + send_cancel_request(rclcpp::Node::SharedPtr node, GoalUUID uuid) { - auto cancel_client = node->create_client( + auto cancel_client = node->create_client( "fibonacci/_action/cancel_goal"); if (!cancel_client->wait_for_service(std::chrono::seconds(20))) { throw std::runtime_error("cancel goal service didn't become available"); } - auto request = std::make_shared(); + auto request = std::make_shared(); request->goal_info.goal_id.uuid = uuid; auto future = cancel_client->async_send_request(request); if (rclcpp::executor::FutureReturnCode::SUCCESS != @@ -80,7 +80,7 @@ TEST_F(TestServer, construction_and_destruction) using GoalHandle = rclcpp_action::ServerGoalHandle; auto as = rclcpp_action::create_server(node, "fibonacci", - [](const GoalID &, std::shared_ptr) { + [](const GoalUUID &, std::shared_ptr) { return rclcpp_action::GoalResponse::REJECT; }, [](std::shared_ptr) { @@ -93,10 +93,10 @@ TEST_F(TestServer, construction_and_destruction) TEST_F(TestServer, handle_goal_called) { auto node = std::make_shared("handle_goal_node", "/rclcpp_action/handle_goal"); - GoalID received_uuid; + GoalUUID received_uuid; auto handle_goal = [&received_uuid]( - const GoalID & uuid, std::shared_ptr) + const GoalUUID & uuid, std::shared_ptr) { received_uuid = uuid; return rclcpp_action::GoalResponse::REJECT; @@ -114,15 +114,15 @@ TEST_F(TestServer, handle_goal_called) // Create a client that calls the goal request service // Make sure the UUID received is the same as the one sent - auto client = node->create_client( + auto client = node->create_client( "fibonacci/_action/send_goal"); ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(20))); - auto request = std::make_shared(); + auto request = std::make_shared(); - const GoalID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; - request->action_goal_id.uuid = uuid; + const GoalUUID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; + request->goal_id.uuid = uuid; auto future = client->async_send_request(request); ASSERT_EQ( @@ -135,10 +135,10 @@ TEST_F(TestServer, handle_goal_called) TEST_F(TestServer, handle_accepted_called) { auto node = std::make_shared("handle_exec_node", "/rclcpp_action/handle_accepted"); - const GoalID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; + const GoalUUID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; auto handle_goal = []( - const GoalID &, std::shared_ptr) + const GoalUUID &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; @@ -164,16 +164,16 @@ TEST_F(TestServer, handle_accepted_called) ASSERT_TRUE(received_handle); ASSERT_TRUE(received_handle->is_active()); EXPECT_EQ(uuid, received_handle->get_goal_id()); - EXPECT_EQ(*request, *(received_handle->get_goal())); + EXPECT_EQ(request->goal, *(received_handle->get_goal())); } TEST_F(TestServer, handle_cancel_called) { auto node = std::make_shared("handle_cancel_node", "/rclcpp_action/handle_cancel"); - const GoalID uuid{{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; + const GoalUUID uuid{{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; auto handle_goal = []( - const GoalID &, std::shared_ptr) + const GoalUUID &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; @@ -210,10 +210,10 @@ TEST_F(TestServer, handle_cancel_called) TEST_F(TestServer, publish_status_accepted) { auto node = std::make_shared("status_accept_node", "/rclcpp_action/status_accept"); - const GoalID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 100, 110, 120, 13, 14, 15, 16}}; + const GoalUUID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 100, 110, 120, 13, 14, 15, 16}}; auto handle_goal = []( - const GoalID &, std::shared_ptr) + const GoalUUID &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; @@ -271,10 +271,10 @@ TEST_F(TestServer, publish_status_accepted) TEST_F(TestServer, publish_status_canceling) { auto node = std::make_shared("status_cancel_node", "/rclcpp_action/status_cancel"); - const GoalID uuid{{1, 2, 3, 40, 5, 6, 7, 80, 9, 10, 11, 120, 13, 14, 15, 160}}; + const GoalUUID uuid{{1, 2, 3, 40, 5, 6, 7, 80, 9, 10, 11, 120, 13, 14, 15, 160}}; auto handle_goal = []( - const GoalID &, std::shared_ptr) + const GoalUUID &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; @@ -326,10 +326,10 @@ TEST_F(TestServer, publish_status_canceling) TEST_F(TestServer, publish_status_canceled) { auto node = std::make_shared("status_canceled", "/rclcpp_action/status_canceled"); - const GoalID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; + const GoalUUID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; auto handle_goal = []( - const GoalID &, std::shared_ptr) + const GoalUUID &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; @@ -383,10 +383,10 @@ TEST_F(TestServer, publish_status_canceled) TEST_F(TestServer, publish_status_succeeded) { auto node = std::make_shared("status_succeeded", "/rclcpp_action/status_succeeded"); - const GoalID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; + const GoalUUID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; auto handle_goal = []( - const GoalID &, std::shared_ptr) + const GoalUUID &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; @@ -438,10 +438,10 @@ TEST_F(TestServer, publish_status_succeeded) TEST_F(TestServer, publish_status_aborted) { auto node = std::make_shared("status_aborted", "/rclcpp_action/status_aborted"); - const GoalID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; + const GoalUUID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; auto handle_goal = []( - const GoalID &, std::shared_ptr) + const GoalUUID &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; @@ -493,10 +493,10 @@ TEST_F(TestServer, publish_status_aborted) TEST_F(TestServer, publish_feedback) { auto node = std::make_shared("pub_feedback", "/rclcpp_action/pub_feedback"); - const GoalID uuid{{1, 20, 30, 4, 5, 6, 70, 8, 9, 1, 11, 120, 13, 14, 15, 160}}; + const GoalUUID uuid{{1, 20, 30, 4, 5, 6, 70, 8, 9, 1, 11, 120, 13, 14, 15, 160}}; auto handle_goal = []( - const GoalID &, std::shared_ptr) + const GoalUUID &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; @@ -521,7 +521,7 @@ TEST_F(TestServer, publish_feedback) (void)as; // Subscribe to feedback messages - using FeedbackT = Fibonacci::Feedback; + using FeedbackT = Fibonacci::Impl::FeedbackMessage; std::vector received_msgs; auto subscriber = node->create_subscription( "fibonacci/_action/feedback", [&received_msgs](FeedbackT::SharedPtr msg) @@ -531,7 +531,7 @@ TEST_F(TestServer, publish_feedback) send_goal_request(node, uuid); - auto sent_message = std::make_shared(); + auto sent_message = std::make_shared(); sent_message->sequence = {1, 1, 2, 3, 5}; received_handle->publish_feedback(sent_message); @@ -544,17 +544,16 @@ TEST_F(TestServer, publish_feedback) ASSERT_EQ(1u, received_msgs.size()); auto & msg = received_msgs.back(); - ASSERT_EQ(sent_message->sequence, msg->sequence); - ASSERT_EQ(uuid, msg->action_goal_id.uuid); + ASSERT_EQ(sent_message->sequence, msg->feedback.sequence); } TEST_F(TestServer, get_result) { auto node = std::make_shared("get_result", "/rclcpp_action/get_result"); - const GoalID uuid{{1, 2, 3, 4, 5, 6, 7, 80, 90, 10, 11, 12, 13, 14, 15, 160}}; + const GoalUUID uuid{{1, 2, 3, 4, 5, 6, 7, 80, 90, 10, 11, 12, 13, 14, 15, 160}}; auto handle_goal = []( - const GoalID &, std::shared_ptr) + const GoalUUID &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; @@ -581,13 +580,13 @@ TEST_F(TestServer, get_result) send_goal_request(node, uuid); // Send result request - auto result_client = node->create_client( + auto result_client = node->create_client( "fibonacci/_action/get_result"); if (!result_client->wait_for_service(std::chrono::seconds(20))) { throw std::runtime_error("get result service didn't become available"); } - auto request = std::make_shared(); - request->action_goal_id.uuid = uuid; + auto request = std::make_shared(); + request->goal_id.uuid = uuid; auto future = result_client->async_send_request(request); // Send a result @@ -600,17 +599,17 @@ TEST_F(TestServer, get_result) rclcpp::spin_until_future_complete(node, future)); auto response = future.get(); - EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, response->action_status); - EXPECT_EQ(result->sequence, response->sequence); + EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, response->status); + EXPECT_EQ(result->sequence, response->result.sequence); } TEST_F(TestServer, deferred_execution) { auto node = std::make_shared("defer_exec", "/rclcpp_action/defer_exec"); - const GoalID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; + const GoalUUID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; auto handle_goal = []( - const GoalID &, std::shared_ptr) + const GoalUUID &, std::shared_ptr) { return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER; }; From 6821fa61091571c39fec92c981ed095692feb62b Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Mon, 11 Mar 2019 16:31:21 -0700 Subject: [PATCH 10/27] Add Doxyfile for rclcpp_action Signed-off-by: Jacob Perron Signed-off-by: Miaofei --- rclcpp_action/Doxyfile | 35 +++++++++++++++++++++++++++++++++++ 1 file changed, 35 insertions(+) create mode 100644 rclcpp_action/Doxyfile diff --git a/rclcpp_action/Doxyfile b/rclcpp_action/Doxyfile new file mode 100644 index 0000000000..4b97902c6c --- /dev/null +++ b/rclcpp_action/Doxyfile @@ -0,0 +1,35 @@ +# All settings not listed here will use the Doxygen default values. + +PROJECT_NAME = "rclcpp_action" +PROJECT_NUMBER = master +PROJECT_BRIEF = "C++ ROS Action Client Library" + +# Use these lines to include the generated logging.hpp (update install path if needed) +#INPUT = ../../../../install_isolated/rclcpp/include +#STRIP_FROM_PATH = /Users/william/ros2_ws/install_isolated/rclcpp/include +# Otherwise just generate for the local (non-generated header files) +INPUT = ./include + +RECURSIVE = YES +OUTPUT_DIRECTORY = doc_output + +EXTRACT_ALL = YES +SORT_MEMBER_DOCS = NO + +GENERATE_LATEX = NO + +ENABLE_PREPROCESSING = YES +MACRO_EXPANSION = YES +EXPAND_ONLY_PREDEF = YES +PREDEFINED = RCLCPP_PUBLIC= + +# Tag files that do not exist will produce a warning and cross-project linking will not work. +TAGFILES += "../../../../doxygen_tag_files/cppreference-doxygen-web.tag.xml=http://en.cppreference.com/w/" +# Consider changing "latest" to the version you want to reference (e.g. beta1 or 1.0.0) +TAGFILES += "../../../../doxygen_tag_files/rclcpp.tag=http://docs.ros2.org/latest/api/rclcpp/" +TAGFILES += "../../../../doxygen_tag_files/rcl_action.tag=http://docs.ros2.org/latest/api/rcl_action/" +TAGFILES += "../../../../doxygen_tag_files/rcl.tag=http://docs.ros2.org/latest/api/rcl/" +TAGFILES += "../../../../doxygen_tag_files/rmw.tag=http://docs.ros2.org/latest/api/rmw/" +TAGFILES += "../../../../doxygen_tag_files/rcutils.tag=http://docs.ros2.org/latest/api/rcutils/" +# Uncomment to generate tag files for cross-project linking. +# GENERATE_TAGFILE = "../../../../doxygen_tag_files/rclcpp_action.tag" From b7cdf8f1e4d4d9bfabbb9facbfea9092b43c80d8 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Mon, 11 Mar 2019 18:07:05 -0700 Subject: [PATCH 11/27] Add documentation to rclcpp_action Signed-off-by: Jacob Perron Signed-off-by: Miaofei --- .../include/rclcpp_action/client.hpp | 59 ++++++++++++++++++- .../rclcpp_action/client_goal_handle.hpp | 24 ++++++++ .../include/rclcpp_action/create_client.hpp | 7 +++ .../include/rclcpp_action/create_server.hpp | 17 ++++++ .../include/rclcpp_action/rclcpp_action.hpp | 7 ++- .../include/rclcpp_action/server.hpp | 4 +- .../rclcpp_action/server_goal_handle.hpp | 24 ++++---- 7 files changed, 128 insertions(+), 14 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 89db1e39f9..64837011f3 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -245,7 +245,7 @@ class ClientBase : public rclcpp::Waitable /** * This class creates an action client. * - * Create an instance of this server using `rclcpp_action::create_client()`. + * To create an instance of an action client use `rclcpp_action::create_client()`. * * Internally, this class is responsible for: * - coverting between the C++ action type and generic types for `rclcpp_action::ClientBase`, and @@ -265,6 +265,18 @@ class Client : public ClientBase using CancelRequest = typename ActionT::Impl::CancelGoalService::Request; using CancelResponse = typename ActionT::Impl::CancelGoalService::Response; + /// Construct an action client. + /** + * This constructs an action client, but it will not work until it has been added to a node. + * Use `rclcpp_action::create_client()` to both construct and add to a node. + * + * \param[in] node_base A pointer to the base interface of a node. + * \param[in] node_graph A pointer to an interface that allows getting graph information about + * a node. + * \param[in] node_logging A pointer to an interface that allows getting a node's logger. + * \param[in] action_name The action name. + * \param[in] client_options Options to pass to the underlying `rcl_action::rcl_action_client_t`. + */ Client( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, @@ -279,6 +291,20 @@ class Client : public ClientBase { } + /// Send an action goal and asynchronously get the result. + /** + * If the goal is accepted by an action server, the returned future is set to a `ClientGoalHandle`. + * If the goal is rejected by an action server, then the future is set to a `nullptr`. + * + * The goal handle is used to monitor the status of the goal and get the final result. + * + * \param[in] goal The goal request. + * \param[in] callback Optional user callback for feedback associated with the goal. + * \param[in] ignore_result If `true`, then the result for the goal will not be requested and + * therefore inaccessible from the goal handle. + * \return A future that completes when the goal has been accepted or rejected. + * If the goal is rejected, then the result will be a `nullptr`. + */ std::shared_future async_send_goal( const Goal & goal, FeedbackCallback callback = nullptr, bool ignore_result = false) @@ -321,6 +347,13 @@ class Client : public ClientBase return future; } + /// Asynchronously get the result for an active goal. + /** + * \throws exceptions::UnknownGoalHandleError If the goal unknown or already reached a terminal + * state. + * \param[in] goal_handle The goal handle for which to get the result. + * \return A future that is set to the goal result when the goal is finished. + */ std::shared_future async_get_result(typename GoalHandle::SharedPtr goal_handle) { @@ -335,6 +368,13 @@ class Client : public ClientBase return goal_handle->async_result(); } + /// Asynchronously request a goal be canceled. + /** + * \throws exceptions::UnknownGoalHandleError If the goal is unknown or already reached a + * terminal state. + * \param[in] goal_handle The goal handle requesting to be canceled. + * \return A future whose result indicates whether or not the cancel request was accepted. + */ std::shared_future async_cancel_goal(typename GoalHandle::SharedPtr goal_handle) { @@ -364,6 +404,14 @@ class Client : public ClientBase return future; } + /// Asynchronously request for all goals to be canceled. + /** + * \return A future to a CancelResponse message that is set when the request has been + * acknowledged by an action server. + * See + * + * action_msgs/CancelGoal.srv. + */ std::shared_future async_cancel_all_goals() { @@ -375,6 +423,15 @@ class Client : public ClientBase return async_cancel(cancel_request); } + /// Asynchronously request all goals at or before a specified time be canceled. + /** + * \param[in] stamp The timestamp for the cancel goal request. + * \return A future to a CancelResponse message that is set when the request has been + * acknowledged by an action server. + * See + * + * action_msgs/CancelGoal.srv. + */ std::shared_future async_cancel_goals_before(const rclcpp::Time & stamp) { diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index bee86b55c7..c6522f88a7 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -45,6 +45,15 @@ enum class ResultCode : int8_t template class Client; +/// Class for interacting with goals sent from action clients. +/** + * Use this class to check the status of a goal as well as get the result. + * + * This class is not meant to be created by a user, instead it is created when a goal has been + * accepted. + * A `Client` will create an instance and return it to the user (via a future) after calling + * `Client::async_send_goal`. + */ template class ClientGoalHandle { @@ -70,21 +79,36 @@ class ClientGoalHandle virtual ~ClientGoalHandle(); + /// Get the unique ID for the goal. const GoalUUID & get_goal_id() const; + /// Get the time when the goal was accepted. rclcpp::Time get_goal_stamp() const; + /// Get a future to the goal result. + /** + * This method should not be called if the `ignore_result` flag was set when + * sending the original goal request (see Client::async_send_goal). + * + * `is_result_aware()` can be used to check if it is safe to call this method. + * + * \throws exceptions::UnawareGoalHandleError If the the goal handle is unaware of the result. + * \return A future to the result. + */ std::shared_future async_result(); + /// Get the goal status code. int8_t get_status(); + /// Check if an action client has subscribed to feedback for the goal. bool is_feedback_aware(); + /// Check if an action client has requested the result for the goal. bool is_result_aware(); diff --git a/rclcpp_action/include/rclcpp_action/create_client.hpp b/rclcpp_action/include/rclcpp_action/create_client.hpp index 9f1e33a0ad..75eed7d47b 100644 --- a/rclcpp_action/include/rclcpp_action/create_client.hpp +++ b/rclcpp_action/include/rclcpp_action/create_client.hpp @@ -25,6 +25,13 @@ namespace rclcpp_action { +/// Create an action client. +/** + * \param[in] node The action client will be added to this node. + * \param[in] name The action name. + * \param[in] group The action client will be added to this callback group. + * If `nullptr`, then the action client is added to the default callback group. + */ template typename Client::SharedPtr create_client( diff --git a/rclcpp_action/include/rclcpp_action/create_server.hpp b/rclcpp_action/include/rclcpp_action/create_server.hpp index ecd557ca48..0630b0021c 100644 --- a/rclcpp_action/include/rclcpp_action/create_server.hpp +++ b/rclcpp_action/include/rclcpp_action/create_server.hpp @@ -31,6 +31,23 @@ namespace rclcpp_action { +/// Create an action server. +/** + * All provided callback functions must be non-blocking. + * + * \sa Server::Server() for more information. + * + * \param node[in] The action server will be added to this node. + * \param name[in] The action name. + * \param[in] handle_goal A callback that decides if a goal should be accepted or rejected. + * \param[in] handle_cancel A callback that decides if a goal should be attempted to be canceled. + * The return from this callback only indicates if the server will try to cancel a goal. + * It does not indicate if the goal was actually canceled. + * \param[in] handle_accepted A callback that is called to give the user a handle to the goal. + * \param[in] options options to pass to the underlying `rcl_action_server_t`. + * \param group[in] The action server will be added to this callback group. + * If `nullptr`, then the action server is added to the default callback group. + */ template typename Server::SharedPtr create_server( diff --git a/rclcpp_action/include/rclcpp_action/rclcpp_action.hpp b/rclcpp_action/include/rclcpp_action/rclcpp_action.hpp index 8ee34c6e2f..61122e9ff5 100644 --- a/rclcpp_action/include/rclcpp_action/rclcpp_action.hpp +++ b/rclcpp_action/include/rclcpp_action/rclcpp_action.hpp @@ -17,9 +17,14 @@ * `rclcpp_action` provides the canonical C++ API for interacting with ROS Actions. * It consists of these main components: * - * - TODO(jacobperron): Finish docs * - Action Client + * - rclcpp_action/client.hpp + * - rclcpp_action/create_client.hpp + * - rclcpp_action/client_goal_handle.hpp * - Action Server + * - rclcpp_action/server.hpp + * - rclcpp_action/create_server.hpp + * - rclcpp_action/server_goal_handle.hpp */ #ifndef RCLCPP_ACTION__RCLCPP_ACTION_HPP_ diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 4a9cff0b1d..4fbc88c0d2 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -63,7 +63,7 @@ enum class CancelResponse : int8_t /// \internal /** * This class should not be used directly by users writing an action server. - * Instead users should use `rclcpp_action::Server<>`. + * Instead users should use `rclcpp_action::Server`. * * Internally, this class is responsible for interfacing with the `rcl_action` API. */ @@ -288,7 +288,7 @@ class Server : public ServerBase, public std::enable_shared_from_this`. + * The result of a goal should be set using methods on `rclcpp_action::ServerGoalHandle`. * * \param[in] node_base a pointer to the base interface of a node. * \param[in] node_clock a pointer to an interface that allows getting a node's clock. diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index 0534d933f5..f0308da76f 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -123,10 +123,10 @@ class Server; /// Class to interact with goals on a server. /** * Use this class to check the status of a goal as well as set the result. + * * This class is not meant to be created by a user, instead it is created when a goal has been * accepted. - * The class `rclcpp_action::Server<>` will create an instance and give it to the user in their - * `handle_accepted` callback. + * A `Server` will create an instance and give it to the user in their `handle_accepted` callback. * * Internally, this class is responsible for coverting between the C++ action type and generic * types for `rclcpp_action::ServerGoalHandleBase`. @@ -138,9 +138,10 @@ class ServerGoalHandle : public ServerGoalHandleBase /// Send an update about the progress of a goal. /** * This must only be called when the goal is executing. - * If execution of a goal is deferred then `ServerGoalHandle<>::set_executing()` must be called + * If execution of a goal is deferred then `ServerGoalHandle::set_executing()` must be called * first. - * `std::runtime_error` is raised if the goal is in any state besides executing. + * + * \throws std::runtime_error If the goal is in any state besides executing. * * \param[in] feedback_msg the message to publish to clients. */ @@ -153,13 +154,13 @@ class ServerGoalHandle : public ServerGoalHandleBase publish_feedback_(feedback_message); } - // TODO(sloretz) which exception is raised? /// Indicate that a goal could not be reached and has been aborted. /** * Only call this if the goal was executing but cannot be completed. * This is a terminal state, no more methods should be called on a goal handle after this is * called. - * An exception is raised if the goal is in any state besides executing. + * + * \throws rclcpp::exceptions::RCLError If the goal is in any state besides executing. * * \param[in] result_msg the final result to send to clients. */ @@ -173,12 +174,13 @@ class ServerGoalHandle : public ServerGoalHandleBase on_terminal_state_(uuid_, response); } - /// Indicate that a goal has been reached. + /// Indicate that a goal has succeeded. /** * Only call this if the goal is executing and has reached the desired final state. * This is a terminal state, no more methods should be called on a goal handle after this is * called. - * An exception is raised if the goal is in any state besides executing. + * + * \throws rclcpp::exceptions::RCLError If the goal is in any state besides executing. * * \param[in] result_msg the final result to send to clients. */ @@ -197,7 +199,8 @@ class ServerGoalHandle : public ServerGoalHandleBase * Only call this if the goal is executing or pending, but has been canceled. * This is a terminal state, no more methods should be called on a goal handle after this is * called. - * An exception is raised if the goal is in any state besides executing or pending. + * + * \throws rclcpp::exceptions::RCLError If the goal is in any state besides executing. * * \param[in] result_msg the final result to send to clients. */ @@ -214,7 +217,8 @@ class ServerGoalHandle : public ServerGoalHandleBase /// Indicate that the server is starting to execute a goal. /** * Only call this if the goal is pending. - * An exception is raised if the goal is in any state besides pending. + * + * \throws rclcpp::exceptions::RCLError If the goal is in any state besides executing. */ void set_executing() From 22a6484d4fd88d1dc2b33e71e2e27a93fbbb3823 Mon Sep 17 00:00:00 2001 From: Emerson Knapp <537409+emersonknapp@users.noreply.github.com> Date: Tue, 12 Mar 2019 15:32:41 -0700 Subject: [PATCH 12/27] Don't hardcode int64_t for duration type representations (#648) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit In LLVM's `libcxx`, `int64_t` doesn't match chrono literals. See example below. To compile, run `clang++-6.0 -stdlib=libc++ -std=c++14 TEST.cpp` ``` using namespace std::chrono_literals; template bool wait_for_service( std::chrono::duration timeout ) { return timeout == std::chrono::nanoseconds(0); } int main() { wait_for_service(2s); return 0; } ``` Result of compilation ``` TEST.cpp:6:1: note: candidate template ignored: could not match 'long' against 'long long' wait_for_service( ``` Signed-off-by: Emerson Knapp Signed-off-by: Steven! Ragnarök Signed-off-by: Miaofei --- rclcpp/include/rclcpp/client.hpp | 4 ++-- rclcpp/include/rclcpp/executor.hpp | 12 ++++++------ rclcpp/include/rclcpp/executors.hpp | 18 ++++++++++-------- rclcpp/include/rclcpp/node.hpp | 4 ++-- rclcpp/include/rclcpp/node_impl.hpp | 4 ++-- rclcpp/include/rclcpp/parameter_client.hpp | 8 ++++---- rclcpp_action/include/rclcpp_action/client.hpp | 4 ++-- .../rclcpp_lifecycle/lifecycle_node.hpp | 4 ++-- .../rclcpp_lifecycle/lifecycle_node_impl.hpp | 4 ++-- 9 files changed, 32 insertions(+), 30 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 23cf8e02a7..6a9dfabd10 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -107,10 +107,10 @@ class ClientBase : public Waitable bool service_is_ready() const; - template + template bool wait_for_service( - std::chrono::duration timeout = std::chrono::duration(-1)) + std::chrono::duration timeout = std::chrono::duration(-1)) { return wait_for_service_nanoseconds( std::chrono::duration_cast(timeout) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index a1bf7657ea..951163b509 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -151,11 +151,11 @@ class Executor * spin_node_once to block indefinitely (the default behavior). A timeout of 0 causes this * function to be non-blocking. */ - template + template void spin_node_once( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node, - std::chrono::duration timeout = std::chrono::duration(-1)) + std::chrono::duration timeout = std::chrono::duration(-1)) { return spin_node_once_nanoseconds( node, @@ -164,11 +164,11 @@ class Executor } /// Convenience function which takes Node and forwards NodeBaseInterface. - template + template void spin_node_once( std::shared_ptr node, - std::chrono::duration timeout = std::chrono::duration(-1)) + std::chrono::duration timeout = std::chrono::duration(-1)) { return spin_node_once_nanoseconds( node->get_node_base_interface(), @@ -218,11 +218,11 @@ class Executor * code. * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. */ - template + template FutureReturnCode spin_until_future_complete( std::shared_future & future, - std::chrono::duration timeout = std::chrono::duration(-1)) + std::chrono::duration timeout = std::chrono::duration(-1)) { // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete // inside a callback executed by an executor. diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index af0bdedc00..7417098b1c 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -65,13 +65,13 @@ using rclcpp::executors::SingleThreadedExecutor; * If the time spent inside the blocking loop exceeds this timeout, return a `TIMEOUT` return code. * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. */ -template +template rclcpp::executor::FutureReturnCode spin_node_until_future_complete( rclcpp::executor::Executor & executor, rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, std::shared_future & future, - std::chrono::duration timeout = std::chrono::duration(-1)) + std::chrono::duration timeout = std::chrono::duration(-1)) { // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete // inside a callback executed by an executor. @@ -81,13 +81,14 @@ spin_node_until_future_complete( return retcode; } -template +template rclcpp::executor::FutureReturnCode spin_node_until_future_complete( rclcpp::executor::Executor & executor, std::shared_ptr node_ptr, std::shared_future & future, - std::chrono::duration timeout = std::chrono::duration(-1)) + std::chrono::duration timeout = std::chrono::duration(-1)) { return rclcpp::executors::spin_node_until_future_complete( executor, @@ -98,23 +99,24 @@ spin_node_until_future_complete( } // namespace executors -template +template rclcpp::executor::FutureReturnCode spin_until_future_complete( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, std::shared_future & future, - std::chrono::duration timeout = std::chrono::duration(-1)) + std::chrono::duration timeout = std::chrono::duration(-1)) { rclcpp::executors::SingleThreadedExecutor executor; return executors::spin_node_until_future_complete(executor, node_ptr, future, timeout); } -template +template rclcpp::executor::FutureReturnCode spin_until_future_complete( std::shared_ptr node_ptr, std::shared_future & future, - std::chrono::duration timeout = std::chrono::duration(-1)) + std::chrono::duration timeout = std::chrono::duration(-1)) { return rclcpp::spin_until_future_complete(node_ptr->get_node_base_interface(), future, timeout); } diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 5e99107c29..dd1bcf704a 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -183,10 +183,10 @@ class Node : public std::enable_shared_from_this * \param[in] callback User-defined callback function. * \param[in] group Callback group to execute this timer's callback in. */ - template + template typename rclcpp::WallTimer::SharedPtr create_wall_timer( - std::chrono::duration period, + std::chrono::duration period, CallbackT callback, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 140b2dcf6e..e1b2377a51 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -121,10 +121,10 @@ Node::create_subscription( allocator); } -template +template typename rclcpp::WallTimer::SharedPtr Node::create_wall_timer( - std::chrono::duration period, + std::chrono::duration period, CallbackT callback, rclcpp::callback_group::CallbackGroup::SharedPtr group) { diff --git a/rclcpp/include/rclcpp/parameter_client.hpp b/rclcpp/include/rclcpp/parameter_client.hpp index e8896db35f..6b68a793fa 100644 --- a/rclcpp/include/rclcpp/parameter_client.hpp +++ b/rclcpp/include/rclcpp/parameter_client.hpp @@ -139,10 +139,10 @@ class AsyncParametersClient bool service_is_ready() const; - template + template bool wait_for_service( - std::chrono::duration timeout = std::chrono::duration(-1)) + std::chrono::duration timeout = std::chrono::duration(-1)) { return wait_for_service_nanoseconds( std::chrono::duration_cast(timeout) @@ -282,10 +282,10 @@ class SyncParametersClient return async_parameters_client_->service_is_ready(); } - template + template bool wait_for_service( - std::chrono::duration timeout = std::chrono::duration(-1)) + std::chrono::duration timeout = std::chrono::duration(-1)) { return async_parameters_client_->wait_for_service(timeout); } diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 64837011f3..71839208bf 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -66,10 +66,10 @@ class ClientBase : public rclcpp::Waitable action_server_is_ready() const; /// Wait for action_server_is_ready() to become true, or until the given timeout is reached. - template + template bool wait_for_action_server( - std::chrono::duration timeout = std::chrono::duration(-1)) + std::chrono::duration timeout = std::chrono::duration(-1)) { return wait_for_action_server_nanoseconds( std::chrono::duration_cast(timeout) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 77f155b418..da4172ae8f 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -222,10 +222,10 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, * \param[in] callback User-defined callback function. * \param[in] group Callback group to execute this timer's callback in. */ - template + template typename rclcpp::WallTimer::SharedPtr create_wall_timer( - std::chrono::duration period, + std::chrono::duration period, CallbackT callback, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index 10eeda9479..44a5e82489 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -136,10 +136,10 @@ LifecycleNode::create_subscription( allocator); } -template +template typename rclcpp::WallTimer::SharedPtr LifecycleNode::create_wall_timer( - std::chrono::duration period, + std::chrono::duration period, CallbackT callback, rclcpp::callback_group::CallbackGroup::SharedPtr group) { From 80e3d4813ad66cf8a0a1b691d272c6fee5209153 Mon Sep 17 00:00:00 2001 From: Peter Baughman Date: Wed, 13 Mar 2019 10:45:05 -0700 Subject: [PATCH 13/27] Fix test_time_source test (#639) * Fix flakey test Signed-off-by: Pete Baughman * Fix lint and uncrustify issues Signed-off-by: Pete Baughman Signed-off-by: Miaofei --- rclcpp/test/test_time_source.cpp | 148 ++++++++++++++++++++++++------- 1 file changed, 114 insertions(+), 34 deletions(-) diff --git a/rclcpp/test/test_time_source.cpp b/rclcpp/test/test_time_source.cpp index fd4a915e7e..02a7a8e7f2 100644 --- a/rclcpp/test/test_time_source.cpp +++ b/rclcpp/test/test_time_source.cpp @@ -55,15 +55,82 @@ class TestTimeSource : public ::testing::Test rclcpp::Node::SharedPtr node; }; -void trigger_clock_changes( - rclcpp::Node::SharedPtr node) +void spin_until_time( + rclcpp::Clock::SharedPtr clock, + rclcpp::Node::SharedPtr node, + std::chrono::nanoseconds end_time, + bool expect_time_update) { - auto clock_pub = node->create_publisher("clock"); + // Call spin_once on the node until either: + // 1) We see the ros_clock's simulated time change to the expected end_time + // -or- + // 2) 1 second has elapsed in the real world + // If 'expect_time_update' is True, and we timed out waiting for simulated time to + // update, we'll have the test fail + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + + auto start = std::chrono::system_clock::now(); + while (std::chrono::system_clock::now() < (start + 1s)) { + if (!rclcpp::ok()) { + break; // Break for ctrl-c + } + + executor.spin_once(10ms); + + if (clock->now().nanoseconds() == end_time.count()) { + return; + } + } + + if (expect_time_update) { + // If we were expecting ROS clock->now to be updated and we didn't take the early return from + // the loop up above, that's a failure + ASSERT_TRUE(false) << "Timed out waiting for ROS time to update"; + } +} + +void spin_until_ros_time_updated( + rclcpp::Clock::SharedPtr clock, + rclcpp::Node::SharedPtr node, + rclcpp::ParameterValue value) +{ + // Similar to above: Call spin_once until we see the clock's ros_time_is_active method + // match the ParameterValue + // Unlike spin_until_time, there aren't any test cases where we don't expect the value to + // update. In the event that the ParameterValue is not set, we'll pump messages for a full second + // but we don't cause the test to fail rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node); - rclcpp::WallRate loop_rate(50); + auto start = std::chrono::system_clock::now(); + while (std::chrono::system_clock::now() < (start + 1s)) { + if (!rclcpp::ok()) { + break; // Break for ctrl-c + } + + executor.spin_once(10ms); + + // In the case where we didn't intend to change the parameter, we'll still pump + if (value.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) { + continue; + } + + if (clock->ros_time_is_active() == value.get()) { + return; + } + } +} + +void trigger_clock_changes( + rclcpp::Node::SharedPtr node, + std::shared_ptr clock, + bool expect_time_update = true) +{ + auto clock_pub = node->create_publisher("clock"); + for (int i = 0; i < 5; ++i) { if (!rclcpp::ok()) { break; // Break for ctrl-c @@ -72,12 +139,23 @@ void trigger_clock_changes( msg->clock.sec = i; msg->clock.nanosec = 1000; clock_pub->publish(msg); - executor.spin_once(1000000ns); - loop_rate.sleep(); + + // workaround. Long-term, there can be a more elegant fix where we hook a future up + // to a clock change callback and spin until future complete, but that's an upstream + // change + spin_until_time( + clock, + node, + std::chrono::seconds(i) + std::chrono::nanoseconds(1000), + expect_time_update + ); } } -void set_use_sim_time_parameter(rclcpp::Node::SharedPtr node, rclcpp::ParameterValue value) +void set_use_sim_time_parameter( + rclcpp::Node::SharedPtr node, + rclcpp::ParameterValue value, + rclcpp::Clock::SharedPtr clock) { auto parameters_client = std::make_shared(node); @@ -89,10 +167,12 @@ void set_use_sim_time_parameter(rclcpp::Node::SharedPtr node, rclcpp::ParameterV for (auto & result : set_parameters_results) { EXPECT_TRUE(result.successful); } - // SyncParametersClient returns when parameters have been set on the node_parameters interface, - // but it doesn't mean the on_parameter_event subscription in TimeSource has been called. - // Spin some to handle that subscription. - rclcpp::spin_some(node); + + // Same as above - workaround for a little bit of asynchronus behavior. The sim_time paramater + // is set synchronously, but the way the ros clock gets notified involves a pub/sub that happens + // AFTER the synchronous notification that the parameter was set. This may also get fixed + // upstream + spin_until_ros_time_updated(clock, node, value); } TEST_F(TestTimeSource, detachUnattached) { @@ -168,17 +248,17 @@ TEST_F(TestTimeSource, clock) { ts.attachClock(ros_clock); EXPECT_FALSE(ros_clock->ros_time_is_active()); - trigger_clock_changes(node); + trigger_clock_changes(node, ros_clock, false); // Even now that we've recieved a message, ROS time should still not be active since the // parameter has not been explicitly set. EXPECT_FALSE(ros_clock->ros_time_is_active()); // Activate ROS time. - set_use_sim_time_parameter(node, rclcpp::ParameterValue(true)); + set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock); EXPECT_TRUE(ros_clock->ros_time_is_active()); - trigger_clock_changes(node); + trigger_clock_changes(node, ros_clock); auto t_out = ros_clock->now(); @@ -235,7 +315,10 @@ TEST_F(TestTimeSource, callbacks) { ts.attachClock(ros_clock); EXPECT_FALSE(ros_clock->ros_time_is_active()); - trigger_clock_changes(node); + // Last arg below is 'expect_time_update' Since ros_time is not active yet, we don't expect + // the simulated time to be updated by trigger_clock_changes. The method will pump messages + // anyway, but won't fail the test when the simulated time doesn't update + trigger_clock_changes(node, ros_clock, false); auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME); auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME); @@ -244,10 +327,10 @@ TEST_F(TestTimeSource, callbacks) { EXPECT_EQ(0, cbo.last_postcallback_id_); // Activate ROS time. - set_use_sim_time_parameter(node, rclcpp::ParameterValue(true)); + set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock); EXPECT_TRUE(ros_clock->ros_time_is_active()); - trigger_clock_changes(node); + trigger_clock_changes(node, ros_clock); auto t_out = ros_clock->now(); @@ -265,7 +348,7 @@ TEST_F(TestTimeSource, callbacks) { std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 2), jump_threshold); - trigger_clock_changes(node); + trigger_clock_changes(node, ros_clock); EXPECT_EQ(2, cbo.last_precallback_id_); EXPECT_EQ(2, cbo.last_postcallback_id_); @@ -284,7 +367,7 @@ TEST_F(TestTimeSource, callbacks) { std::function(), jump_threshold); - trigger_clock_changes(node); + trigger_clock_changes(node, ros_clock); EXPECT_EQ(3, cbo.last_precallback_id_); EXPECT_EQ(2, cbo.last_postcallback_id_); @@ -294,7 +377,7 @@ TEST_F(TestTimeSource, callbacks) { std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 4), jump_threshold); - trigger_clock_changes(node); + trigger_clock_changes(node, ros_clock); EXPECT_EQ(3, cbo.last_precallback_id_); EXPECT_EQ(4, cbo.last_postcallback_id_); } @@ -329,10 +412,10 @@ TEST_F(TestTimeSource, callback_handler_erasure) { EXPECT_EQ(0, cbo.last_postcallback_id_); // Activate ROS time. - set_use_sim_time_parameter(node, rclcpp::ParameterValue(true)); + set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock); EXPECT_TRUE(ros_clock->ros_time_is_active()); - trigger_clock_changes(node); + trigger_clock_changes(node, ros_clock); auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME); auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME); @@ -356,7 +439,7 @@ TEST_F(TestTimeSource, callback_handler_erasure) { // Remove the last callback in the vector callback_handler2.reset(); - trigger_clock_changes(node); + trigger_clock_changes(node, ros_clock); EXPECT_EQ(2, cbo.last_precallback_id_); EXPECT_EQ(2, cbo.last_postcallback_id_); @@ -370,7 +453,6 @@ TEST_F(TestTimeSource, callback_handler_erasure) { EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds()); } - TEST_F(TestTimeSource, parameter_activation) { rclcpp::TimeSource ts(node); auto ros_clock = std::make_shared(RCL_ROS_TIME); @@ -379,27 +461,25 @@ TEST_F(TestTimeSource, parameter_activation) { ts.attachClock(ros_clock); EXPECT_FALSE(ros_clock->ros_time_is_active()); - set_use_sim_time_parameter(node, rclcpp::ParameterValue(true)); + set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock); EXPECT_TRUE(ros_clock->ros_time_is_active()); - set_use_sim_time_parameter( - node, rclcpp::ParameterValue()); + set_use_sim_time_parameter(node, rclcpp::ParameterValue(), ros_clock); EXPECT_TRUE(ros_clock->ros_time_is_active()); - set_use_sim_time_parameter(node, rclcpp::ParameterValue(false)); + set_use_sim_time_parameter(node, rclcpp::ParameterValue(false), ros_clock); EXPECT_FALSE(ros_clock->ros_time_is_active()); - set_use_sim_time_parameter( - node, rclcpp::ParameterValue()); + set_use_sim_time_parameter(node, rclcpp::ParameterValue(), ros_clock); EXPECT_FALSE(ros_clock->ros_time_is_active()); // If the use_sim_time parameter is not explicitly set to True, this clock's use of sim time // should not be affected by the presence of a clock publisher. - trigger_clock_changes(node); + trigger_clock_changes(node, ros_clock, false); EXPECT_FALSE(ros_clock->ros_time_is_active()); - set_use_sim_time_parameter(node, rclcpp::ParameterValue(false)); + set_use_sim_time_parameter(node, rclcpp::ParameterValue(false), ros_clock); EXPECT_FALSE(ros_clock->ros_time_is_active()); - set_use_sim_time_parameter(node, rclcpp::ParameterValue(true)); + set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock); EXPECT_TRUE(ros_clock->ros_time_is_active()); } @@ -424,7 +504,7 @@ TEST_F(TestTimeSource, no_pre_jump_callback) { ts.attachClock(ros_clock); // Activate ROS time - set_use_sim_time_parameter(node, rclcpp::ParameterValue(true)); + set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock); ASSERT_TRUE(ros_clock->ros_time_is_active()); EXPECT_EQ(0, cbo.last_precallback_id_); From bcabadc369448c34b955bc0a84f0a2fabc657fe1 Mon Sep 17 00:00:00 2001 From: Miaofei Date: Fri, 15 Mar 2019 03:10:36 -0700 Subject: [PATCH 14/27] fix lint errors Signed-off-by: Miaofei --- rclcpp/include/rclcpp/publisher.hpp | 1 + rclcpp/include/rclcpp/publisher_factory.hpp | 4 +++- rclcpp/include/rclcpp/publisher_options.hpp | 6 +++--- rclcpp/include/rclcpp/subscription.hpp | 1 + rclcpp/include/rclcpp/subscription_options.hpp | 2 +- rclcpp/src/rclcpp/client.cpp | 11 ++++++++--- rclcpp/src/rclcpp/executor.cpp | 3 ++- rclcpp/src/rclcpp/publisher.cpp | 1 + rclcpp/src/rclcpp/service.cpp | 12 +++++++++--- rclcpp/src/rclcpp/subscription.cpp | 1 + .../include/rclcpp_lifecycle/lifecycle_node_impl.hpp | 4 ++-- 11 files changed, 32 insertions(+), 14 deletions(-) diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 3b13e4d65d..5bcb8a4e10 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include diff --git a/rclcpp/include/rclcpp/publisher_factory.hpp b/rclcpp/include/rclcpp/publisher_factory.hpp index b494e66b90..a80b2d698f 100644 --- a/rclcpp/include/rclcpp/publisher_factory.hpp +++ b/rclcpp/include/rclcpp/publisher_factory.hpp @@ -75,7 +75,9 @@ struct PublisherFactory /// Return a PublisherFactory with functions setup for creating a PublisherT. template PublisherFactory -create_publisher_factory(const PublisherEventCallbacks & event_callbacks, std::shared_ptr allocator) +create_publisher_factory( + const PublisherEventCallbacks & event_callbacks, + std::shared_ptr allocator) { PublisherFactory factory; diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp index f3b7cd1daf..994c60062f 100644 --- a/rclcpp/include/rclcpp/publisher_options.hpp +++ b/rclcpp/include/rclcpp/publisher_options.hpp @@ -30,7 +30,7 @@ struct PublisherEventCallbacks { QOSDeadlineEventCallbackType deadline_callback_; QOSLivelinessEventCallbackType liveliness_callback_; - QOSLifespanEventCallbackType lifespan_callback_; + QOSLifespanEventCallbackType lifespan_callback_; }; template> @@ -57,7 +57,7 @@ class PublisherOptions PublisherOptions() = default; RCLCPP_PUBLIC - PublisherOptions(const rmw_qos_profile_t & qos_profile) : + explicit PublisherOptions(const rmw_qos_profile_t & qos_profile) : publisher_qos_profile_(qos_profile) {} /// Destructor. @@ -179,7 +179,7 @@ class PublisherOptions return allocator_; } - /// Set the rcl_allocator_t to be used, may cause deallocation of existing rcl_publisher_options_t. + /// Set the rcl_allocator_t to be used, may cause deallocation of existing rcl_publisher_options_t /** * This will cause the internal rcl_publisher_options_t struct to be invalidated. */ diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 546f35d957..876750d6bd 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index 338ab0cda4..0a0f466355 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -54,7 +54,7 @@ class SubscriptionOptions SubscriptionOptions() = default; RCLCPP_PUBLIC - SubscriptionOptions(const rmw_qos_profile_t & qos_profile) : + explicit SubscriptionOptions(const rmw_qos_profile_t & qos_profile) : subscription_qos_profile_(qos_profile) {} /// Destructor. diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index bc28bcd24a..5c9b2a4d5e 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -129,16 +129,21 @@ ClientBase::get_number_of_ready_events() bool ClientBase::add_to_wait_set(rcl_wait_set_t * wait_set) { - if (rcl_wait_set_add_client(wait_set, client_handle_.get(), &wait_set_client_index_) != RCL_RET_OK) { + if ( + rcl_wait_set_add_client(wait_set, client_handle_.get(), &wait_set_client_index_) != RCL_RET_OK) + { RCUTILS_LOG_ERROR_NAMED( "rclcpp", "Couldn't add client to wait set: %s", rcl_get_error_string().str); return false; } - // TODO(mm318): enable QOS event callbacks for clients (currently only for publishers and subscriptions) + // TODO(mm318): enable QOS event callbacks for clients + // (currently only for publishers and subscriptions) wait_set_event_index_ = 0; - // if (rcl_wait_set_add_event(wait_set, event_handle_.get(), &wait_set_event_index_) != RCL_RET_OK) { + // if ( + // rcl_wait_set_add_event(wait_set, event_handle_.get(), &wait_set_event_index_) != RCL_RET_OK) + // { // RCUTILS_LOG_ERROR_NAMED( // "rclcpp", // "Couldn't add client event to wait set: %s", rcl_get_error_string().str); diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index ae707bf603..67cd888a4b 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -60,7 +60,8 @@ Executor::Executor(const ExecutorArgs & args) // Store the context for later use. context_ = args.context; - ret = rcl_wait_set_init(&wait_set_, 0, 2, 0, 0, 0, 0, context_->get_rcl_context().get(), allocator); + ret = rcl_wait_set_init(&wait_set_, 0, 2, 0, 0, 0, 0, + context_->get_rcl_context().get(), allocator); if (RCL_RET_OK != ret) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", diff --git a/rclcpp/src/rclcpp/publisher.cpp b/rclcpp/src/rclcpp/publisher.cpp index 0f89165134..ace6ec9364 100644 --- a/rclcpp/src/rclcpp/publisher.cpp +++ b/rclcpp/src/rclcpp/publisher.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index f0fef41d79..24e00c824f 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -79,16 +79,22 @@ ServiceBase::get_number_of_ready_events() bool ServiceBase::add_to_wait_set(rcl_wait_set_t * wait_set) { - if (rcl_wait_set_add_service(wait_set, service_handle_.get(), &wait_set_service_index_) != RCL_RET_OK) { + if ( + rcl_wait_set_add_service(wait_set, service_handle_.get(), &wait_set_service_index_) != + RCL_RET_OK) + { RCUTILS_LOG_ERROR_NAMED( "rclcpp", "Couldn't add service to wait set: %s", rcl_get_error_string().str); return false; } - // TODO(mm318): enable QOS event callbacks for clients (currently only for publishers and subscriptions) + // TODO(mm318): enable QOS event callbacks for clients + // (currently only for publishers and subscriptions) wait_set_event_index_ = 0; - // if (rcl_wait_set_add_event(wait_set, event_handle_.get(), &wait_set_event_index_) != RCL_RET_OK) { + // if ( + // rcl_wait_set_add_event(wait_set, event_handle_.get(), &wait_set_event_index_) != RCL_RET_OK) + // { // RCUTILS_LOG_ERROR_NAMED( // "rclcpp", // "Couldn't add service event to wait set: %s", rcl_get_error_string().str); diff --git a/rclcpp/src/rclcpp/subscription.cpp b/rclcpp/src/rclcpp/subscription.cpp index ee5102210f..7033fdbc9d 100644 --- a/rclcpp/src/rclcpp/subscription.cpp +++ b/rclcpp/src/rclcpp/subscription.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include "rclcpp/exceptions.hpp" diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index 44a5e82489..af3bbfe3ea 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -182,8 +182,8 @@ LifecycleNode::create_service( const rmw_qos_profile_t & qos_profile, rclcpp::callback_group::CallbackGroup::SharedPtr group) { - return rclcpp::create_service( - node_base_, node_services_, service_name, std::forward(callback), qos_profile, group); + return rclcpp::create_service(node_base_, node_services_, service_name, + std::forward(callback), qos_profile, group); } template From 993788fb378f268f94a108298a88bca1d1c87306 Mon Sep 17 00:00:00 2001 From: Miaofei Date: Fri, 15 Mar 2019 03:18:46 -0700 Subject: [PATCH 15/27] apply uncrustify Signed-off-by: Miaofei --- rclcpp/include/rclcpp/create_subscription.hpp | 4 ++-- rclcpp/include/rclcpp/node.hpp | 2 +- rclcpp/include/rclcpp/node_impl.hpp | 2 +- rclcpp/include/rclcpp/publisher.hpp | 11 +++++----- rclcpp/include/rclcpp/publisher_factory.hpp | 2 +- rclcpp/include/rclcpp/publisher_options.hpp | 4 ++-- rclcpp/include/rclcpp/qos_event.hpp | 19 ++++++++++-------- rclcpp/include/rclcpp/service.hpp | 20 +++++++++---------- rclcpp/include/rclcpp/subscription.hpp | 11 +++++----- .../include/rclcpp/subscription_options.hpp | 4 ++-- rclcpp/src/rclcpp/any_executable.cpp | 2 +- rclcpp/src/rclcpp/client.cpp | 20 +++++++++---------- rclcpp/src/rclcpp/executor.cpp | 2 +- rclcpp/src/rclcpp/qos_event.cpp | 2 +- rclcpp/src/rclcpp/subscription.cpp | 8 +++++--- rclcpp/src/rclcpp/time_source.cpp | 2 +- rclcpp/src/rclcpp/timer.cpp | 2 +- rclcpp_action/test/test_client.cpp | 2 +- .../rclcpp_lifecycle/lifecycle_node_impl.hpp | 2 +- 19 files changed, 64 insertions(+), 57 deletions(-) diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index dedef92838..8510705ecd 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -43,7 +43,7 @@ create_subscription( bool ignore_local_publications, bool use_intra_process_comms, typename rclcpp::message_memory_strategy::MessageMemoryStrategy::SharedPtr msg_mem_strat, + AllocatorT>::SharedPtr msg_mem_strat, typename std::shared_ptr allocator) { auto subscription_options = rcl_subscription_get_default_options(); @@ -51,7 +51,7 @@ create_subscription( subscription_options.ignore_local_publications = ignore_local_publications; auto factory = rclcpp::create_subscription_factory( + CallbackMessageT, SubscriptionT>( std::forward(callback), callbacks, msg_mem_strat, diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index dd1bcf704a..91f82bb68f 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -175,7 +175,7 @@ class Node : public std::enable_shared_from_this const SubscriptionOptions & options = SubscriptionOptions(), typename rclcpp::message_memory_strategy::MessageMemoryStrategy< typename rclcpp::subscription_traits::has_message_type::type, Alloc>::SharedPtr - msg_mem_strat = nullptr); + msg_mem_strat = nullptr); /// Create a timer. /** diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index e1b2377a51..7884b2ea08 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -94,7 +94,7 @@ Node::create_subscription( const SubscriptionOptions & options, typename rclcpp::message_memory_strategy::MessageMemoryStrategy< typename rclcpp::subscription_traits::has_message_type::type, Alloc>::SharedPtr - msg_mem_strat) + msg_mem_strat) { using CallbackMessageT = typename rclcpp::subscription_traits::has_message_type::type; diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 5bcb8a4e10..cdafb890c4 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -123,14 +123,15 @@ class PublisherBase get_publisher_handle() const; // RCLCPP_PUBLIC - template void + template + void add_event_handle(const EventCallbackT & callback, const rcl_publisher_event_type_t event_type) { event_handles_.emplace_back(std::make_shared>( - callback, - rcl_publisher_event_init, - &publisher_handle_, - event_type)); + callback, + rcl_publisher_event_init, + &publisher_handle_, + event_type)); } RCLCPP_PUBLIC diff --git a/rclcpp/include/rclcpp/publisher_factory.hpp b/rclcpp/include/rclcpp/publisher_factory.hpp index a80b2d698f..98ab04dc6a 100644 --- a/rclcpp/include/rclcpp/publisher_factory.hpp +++ b/rclcpp/include/rclcpp/publisher_factory.hpp @@ -92,7 +92,7 @@ create_publisher_factory( publisher_options.allocator = allocator::get_rcl_allocator(*message_alloc.get()); return std::make_shared(node_base, topic_name, publisher_options, - event_callbacks, message_alloc); + event_callbacks, message_alloc); }; // function to add a publisher to the intra process manager diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp index 994c60062f..74e2ea5b28 100644 --- a/rclcpp/include/rclcpp/publisher_options.hpp +++ b/rclcpp/include/rclcpp/publisher_options.hpp @@ -57,8 +57,8 @@ class PublisherOptions PublisherOptions() = default; RCLCPP_PUBLIC - explicit PublisherOptions(const rmw_qos_profile_t & qos_profile) : - publisher_qos_profile_(qos_profile) {} + explicit PublisherOptions(const rmw_qos_profile_t & qos_profile) + : publisher_qos_profile_(qos_profile) {} /// Destructor. RCLCPP_PUBLIC diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index 169dcbca78..502aaffa49 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -31,7 +31,8 @@ namespace rclcpp struct QOSDeadlineEventInfo { - enum DeadlineEventType { + enum DeadlineEventType + { DEADLINE_MISSED }; @@ -41,7 +42,8 @@ struct QOSDeadlineEventInfo struct QOSLivelinessEventInfo { - enum LivelinessEventType { + enum LivelinessEventType + { LIVELINESS_CHANGED }; @@ -51,7 +53,8 @@ struct QOSLivelinessEventInfo struct QOSLifespanEventInfo { - enum LifespanEventType { + enum LifespanEventType + { LIFESPAN_EXPIRED }; @@ -60,9 +63,9 @@ struct QOSLifespanEventInfo }; -using QOSDeadlineEventCallbackType = std::function; -using QOSLivelinessEventCallbackType = std::function; -using QOSLifespanEventCallbackType = std::function; +using QOSDeadlineEventCallbackType = std::function; +using QOSLivelinessEventCallbackType = std::function; +using QOSLifespanEventCallbackType = std::function; // struct QOSDeadlineEventConfig { @@ -108,7 +111,7 @@ class QOSEvent : public QOSEventBase public: template QOSEvent(const EventCallbackT & callback, InitFuncT init_func, HandleT handle, EventTypeEnum type) - : event_callback_(callback) + : event_callback_(callback) { event_handle_ = rcl_get_zero_initialized_event(); rcl_ret_t ret = init_func(&event_handle_, handle, type); @@ -146,7 +149,7 @@ class QOSEvent : public QOSEventBase private: using EventCallbackInfoT = typename std::remove_reference::template argument_type<0>>::type; + rclcpp::function_traits::function_traits::template argument_type<0>>::type; EventCallbackT event_callback_; }; diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index f35d2104b4..c1f172c0a5 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -192,16 +192,16 @@ class Service : public ServiceBase } event_handle_ = std::shared_ptr(new rcl_event_t, - [](rcl_event_t * event) - { - if (rcl_event_fini(event) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Error in destruction of rcl event handle: %s", rcl_get_error_string().str); - rcl_reset_error(); - } - delete event; - }); + [](rcl_event_t * event) + { + if (rcl_event_fini(event) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Error in destruction of rcl event handle: %s", rcl_get_error_string().str); + rcl_reset_error(); + } + delete event; + }); *event_handle_.get() = rcl_get_zero_initialized_event(); ret = rcl_service_event_init(get_event_handle().get(), get_service_handle().get()); diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 876750d6bd..129a39aa1e 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -105,14 +105,15 @@ class SubscriptionBase : public Waitable get_intra_process_subscription_handle() const; // RCLCPP_PUBLIC - template void + template + void add_event_handle(const EventCallbackT & callback, const rcl_subscription_event_type_t event_type) { event_handles_.emplace_back(std::make_shared>( - callback, - rcl_subscription_event_init, - get_subscription_handle().get(), - event_type)); + callback, + rcl_subscription_event_init, + get_subscription_handle().get(), + event_type)); } RCLCPP_PUBLIC diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index 0a0f466355..cafa9dc159 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -54,8 +54,8 @@ class SubscriptionOptions SubscriptionOptions() = default; RCLCPP_PUBLIC - explicit SubscriptionOptions(const rmw_qos_profile_t & qos_profile) : - subscription_qos_profile_(qos_profile) {} + explicit SubscriptionOptions(const rmw_qos_profile_t & qos_profile) + : subscription_qos_profile_(qos_profile) {} /// Destructor. RCLCPP_PUBLIC diff --git a/rclcpp/src/rclcpp/any_executable.cpp b/rclcpp/src/rclcpp/any_executable.cpp index ee9ce7ff9f..07f6ceabab 100644 --- a/rclcpp/src/rclcpp/any_executable.cpp +++ b/rclcpp/src/rclcpp/any_executable.cpp @@ -29,5 +29,5 @@ AnyExecutable::~AnyExecutable() bool AnyExecutable::has_timer() const { - return (waitable->get_number_of_ready_timers() > 0); + return waitable->get_number_of_ready_timers() > 0; } diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index 5c9b2a4d5e..c3bc9923ad 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -65,16 +65,16 @@ ClientBase::ClientBase( }); event_handle_ = std::shared_ptr(new rcl_event_t, - [](rcl_event_t * event) - { - if (rcl_event_fini(event) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Error in destruction of rcl event handle: %s", rcl_get_error_string().str); - rcl_reset_error(); - } - delete event; - }); + [](rcl_event_t * event) + { + if (rcl_event_fini(event) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Error in destruction of rcl event handle: %s", rcl_get_error_string().str); + rcl_reset_error(); + } + delete event; + }); *event_handle_.get() = rcl_get_zero_initialized_event(); } diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 67cd888a4b..4973ab88c8 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -61,7 +61,7 @@ Executor::Executor(const ExecutorArgs & args) context_ = args.context; ret = rcl_wait_set_init(&wait_set_, 0, 2, 0, 0, 0, 0, - context_->get_rcl_context().get(), allocator); + context_->get_rcl_context().get(), allocator); if (RCL_RET_OK != ret) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", diff --git a/rclcpp/src/rclcpp/qos_event.cpp b/rclcpp/src/rclcpp/qos_event.cpp index 977d14c2db..906be4035e 100644 --- a/rclcpp/src/rclcpp/qos_event.cpp +++ b/rclcpp/src/rclcpp/qos_event.cpp @@ -43,7 +43,7 @@ QOSEventBase::add_to_wait_set(rcl_wait_set_t * wait_set) bool QOSEventBase::is_ready(rcl_wait_set_t * wait_set) { - return (wait_set->events[wait_set_event_index_] == &event_handle_); + return wait_set->events[wait_set_event_index_] == &event_handle_; } } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/subscription.cpp b/rclcpp/src/rclcpp/subscription.cpp index 7033fdbc9d..f5ecd2aa39 100644 --- a/rclcpp/src/rclcpp/subscription.cpp +++ b/rclcpp/src/rclcpp/subscription.cpp @@ -144,7 +144,8 @@ bool SubscriptionBase::add_to_wait_set(rcl_wait_set_t * wait_set) { if (rcl_wait_set_add_subscription(wait_set, subscription_handle_.get(), - &wait_set_subscription_index_) != RCL_RET_OK) { + &wait_set_subscription_index_) != RCL_RET_OK) + { RCUTILS_LOG_ERROR_NAMED( "rclcpp", "Couldn't add subscription to wait set: %s", rcl_get_error_string().str); @@ -153,7 +154,8 @@ SubscriptionBase::add_to_wait_set(rcl_wait_set_t * wait_set) if (use_intra_process_) { if (rcl_wait_set_add_subscription(wait_set, intra_process_subscription_handle_.get(), - &wait_set_intra_process_subscription_index_) != RCL_RET_OK) { + &wait_set_intra_process_subscription_index_) != RCL_RET_OK) + { RCUTILS_LOG_ERROR_NAMED( "rclcpp", "Couldn't add intra process subscription to wait set: %s", rcl_get_error_string().str); @@ -171,7 +173,7 @@ SubscriptionBase::is_ready(rcl_wait_set_t * wait_set) (wait_set->subscriptions[wait_set_subscription_index_] == subscription_handle_.get()); intra_process_subscription_ready_ = use_intra_process_ && (wait_set->subscriptions[wait_set_intra_process_subscription_index_] == - intra_process_subscription_handle_.get()); + intra_process_subscription_handle_.get()); return subscription_ready_ || intra_process_subscription_ready_; } diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 480f869868..10be2c5296 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -208,7 +208,7 @@ void TimeSource::create_clock_sub() auto cb = std::bind(&TimeSource::clock_cb, this, std::placeholders::_1); clock_subscription_ = rclcpp::create_subscription( + SubscriptionT>( node_topics_.get(), topic_name, std::move(cb), diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index 731ff8ea5f..a5ed80c1e8 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -135,7 +135,7 @@ TimerBase::add_to_wait_set(rcl_wait_set_t * wait_set) bool TimerBase::is_ready(rcl_wait_set_t * wait_set) { - return (wait_set->timers[wait_set_timer_index_] == timer_handle_.get()); + return wait_set->timers[wait_set_timer_index_] == timer_handle_.get(); } void diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index 3236d2ec1a..6199d29af2 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -205,7 +205,7 @@ class TestClient : public ::testing::Test action_name, allocator, &status_topic_name); ASSERT_EQ(RCL_RET_OK, ret); status_publisher = server_node->create_publisher(status_topic_name, - nullptr, rclcpp::PublisherOptions<>(rcl_action_qos_profile_status_default)); + nullptr, rclcpp::PublisherOptions<>(rcl_action_qos_profile_status_default)); ASSERT_TRUE(status_publisher != nullptr); allocator.deallocate(status_topic_name, allocator.state); server_executor.add_node(server_node); diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index af3bbfe3ea..1ba5d79c63 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -183,7 +183,7 @@ LifecycleNode::create_service( rclcpp::callback_group::CallbackGroup::SharedPtr group) { return rclcpp::create_service(node_base_, node_services_, service_name, - std::forward(callback), qos_profile, group); + std::forward(callback), qos_profile, group); } template From 43f891dac8ff27839079f1c8b39527972ce65fc6 Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Wed, 20 Mar 2019 08:38:07 -0700 Subject: [PATCH 16/27] add section about DCO to CONTRIBUTING.md --- CONTRIBUTING.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 6f63de9e5d..cfba094dac 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -11,3 +11,8 @@ be under the Apache 2 License, as dictated by that the terms of any separate license agreement you may have executed with Licensor regarding such Contributions. ~~~ + +Contributors must sign-off each commit by adding a `Signed-off-by: ...` +line to commit messages to certify that they have the right to submit +the code they are contributing to the project according to the +[Developer Certificate of Origin (DCO)](https://developercertificate.org/). From 787fa6fc65647b5d95420d0270cc4f2af41f6077 Mon Sep 17 00:00:00 2001 From: Miaofei Date: Thu, 21 Mar 2019 13:56:54 -0700 Subject: [PATCH 17/27] update for rcl API changes Signed-off-by: Miaofei --- rclcpp/include/rclcpp/client.hpp | 3 ++- rclcpp/include/rclcpp/publisher.hpp | 12 +++++++++--- rclcpp/include/rclcpp/qos_event.hpp | 11 ++++++++--- rclcpp/include/rclcpp/service.hpp | 3 ++- rclcpp/include/rclcpp/subscription.hpp | 12 +++++++++--- 5 files changed, 30 insertions(+), 11 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 6a9dfabd10..83840c20ab 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -203,7 +203,8 @@ class Client : public ClientBase rclcpp::exceptions::throw_from_rcl_error(ret, "could not create client"); } - ret = rcl_client_event_init(get_event_handle().get(), get_client_handle().get()); + ret = rcl_client_event_init(get_event_handle().get(), get_client_handle().get(), + &client_options, RCL_CLIENT_EVENT_UNIMPLEMENTED); if (ret != RCL_RET_OK) { rclcpp::exceptions::throw_from_rcl_error(ret, "could not create client event"); } diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index cdafb890c4..b0d26670da 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -125,12 +125,16 @@ class PublisherBase // RCLCPP_PUBLIC template void - add_event_handle(const EventCallbackT & callback, const rcl_publisher_event_type_t event_type) + add_event_handle( + const EventCallbackT & callback, + const rcl_publisher_options_t & publisher_options, + const rcl_publisher_event_type_t event_type) { event_handles_.emplace_back(std::make_shared>( callback, rcl_publisher_event_init, &publisher_handle_, + &publisher_options, event_type)); } @@ -231,10 +235,12 @@ class Publisher : public PublisherBase allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get()); if (event_callbacks.deadline_callback_) { - this->add_event_handle(event_callbacks.deadline_callback_, RCL_PUBLISHER_DEADLINE); + this->add_event_handle(event_callbacks.deadline_callback_, + publisher_options, RCL_PUBLISHER_DEADLINE); } if (event_callbacks.liveliness_callback_) { - this->add_event_handle(event_callbacks.liveliness_callback_, RCL_PUBLISHER_LIVELINESS); + this->add_event_handle(event_callbacks.liveliness_callback_, + publisher_options, RCL_PUBLISHER_LIVELINESS); } // if (event_callbacks.lifespan_callback_) { // this->add_event_handle(event_callbacks.lifespan_callback_); diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index 502aaffa49..68479ed5ed 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -109,12 +109,17 @@ template class QOSEvent : public QOSEventBase { public: - template - QOSEvent(const EventCallbackT & callback, InitFuncT init_func, HandleT handle, EventTypeEnum type) + template + QOSEvent( + const EventCallbackT & callback, + InitFuncT init_func, + HandleT handle, + OptionsT options, + EventTypeEnum type) : event_callback_(callback) { event_handle_ = rcl_get_zero_initialized_event(); - rcl_ret_t ret = init_func(&event_handle_, handle, type); + rcl_ret_t ret = init_func(&event_handle_, handle, options, type); if (ret != RCL_RET_OK) { rclcpp::exceptions::throw_from_rcl_error(ret, "could not create event"); } diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index c1f172c0a5..f89e70d44b 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -204,7 +204,8 @@ class Service : public ServiceBase }); *event_handle_.get() = rcl_get_zero_initialized_event(); - ret = rcl_service_event_init(get_event_handle().get(), get_service_handle().get()); + ret = rcl_service_event_init(get_event_handle().get(), get_service_handle().get(), + &service_options, RCL_SERVICE_EVENT_UNIMPLEMENTED); if (ret != RCL_RET_OK) { rclcpp::exceptions::throw_from_rcl_error(ret, "could not create service event"); } diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 129a39aa1e..55beb7f5d8 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -107,12 +107,16 @@ class SubscriptionBase : public Waitable // RCLCPP_PUBLIC template void - add_event_handle(const EventCallbackT & callback, const rcl_subscription_event_type_t event_type) + add_event_handle( + const EventCallbackT & callback, + const rcl_subscription_options_t & subscription_options, + const rcl_subscription_event_type_t event_type) { event_handles_.emplace_back(std::make_shared>( callback, rcl_subscription_event_init, get_subscription_handle().get(), + &subscription_options, event_type)); } @@ -256,10 +260,12 @@ class Subscription : public SubscriptionBase matches_any_intra_process_publishers_(nullptr) { if (event_callbacks.deadline_callback_) { - this->add_event_handle(event_callbacks.deadline_callback_, RCL_SUBSCRIPTION_DEADLINE); + this->add_event_handle(event_callbacks.deadline_callback_, + subscription_options, RCL_SUBSCRIPTION_DEADLINE); } if (event_callbacks.liveliness_callback_) { - this->add_event_handle(event_callbacks.liveliness_callback_, RCL_SUBSCRIPTION_LIVELINESS); + this->add_event_handle(event_callbacks.liveliness_callback_, + subscription_options, RCL_SUBSCRIPTION_LIVELINESS); } // if (event_callbacks.lifespan_callback_) { // this->add_event_handle(event_callbacks.lifespan_callback_); From 71ba5f3fe7cb9782b1daa6eaaa2af39a9aee350b Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Thu, 21 Mar 2019 14:50:09 -0700 Subject: [PATCH 18/27] Fix lint and build warnings and API inconsistency Signed-off-by: Emerson Knapp --- rclcpp/include/rclcpp/publisher_options.hpp | 3 ++- rclcpp/include/rclcpp/subscription_options.hpp | 2 ++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp index 74e2ea5b28..6c6ae24eda 100644 --- a/rclcpp/include/rclcpp/publisher_options.hpp +++ b/rclcpp/include/rclcpp/publisher_options.hpp @@ -96,7 +96,7 @@ class PublisherOptions */ RCLCPP_PUBLIC PublisherOptions & - publisher_qos_profile(const rmw_qos_profile_t & publisher_qos_profile) + qos_profile(const rmw_qos_profile_t & publisher_qos_profile) { publisher_qos_profile_ = publisher_qos_profile; return *this; @@ -188,6 +188,7 @@ class PublisherOptions allocator(std::shared_ptr allocator) { allocator_ = allocator; + return *this; } private: diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index cafa9dc159..a175ebd8db 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -160,6 +160,7 @@ class SubscriptionOptions return *this; } + RCLCPP_PUBLIC const SubscriptionEventCallbacks & event_callbacks() const @@ -196,6 +197,7 @@ class SubscriptionOptions allocator(std::shared_ptr allocator) { allocator_ = allocator; + return *this; } private: From 0a44344f4332f3a8818291dc46c5491192b41c78 Mon Sep 17 00:00:00 2001 From: Marko Durkovic Date: Sat, 23 Mar 2019 08:18:43 +0100 Subject: [PATCH 19/27] Avoid race that triggers timer too often (#621) The two distinct operations of acquiring and subsequent checking of a timer have to be protected by one lock_guard against races with other threads. The releasing of a timer has to be protected by the same lock. Given this requirement there is no use for a second mutex. Signed-off-by: Marko Durkovic --- rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp | 1 - rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp | 3 +-- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp index eb41a93e11..72df5dd82a 100644 --- a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp @@ -78,7 +78,6 @@ class MultiThreadedExecutor : public executor::Executor size_t number_of_threads_; bool yield_before_execute_; - std::mutex scheduled_timers_mutex_; std::set scheduled_timers_; }; diff --git a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp index 08318534b2..6c04d81315 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -82,7 +82,6 @@ MultiThreadedExecutor::run(size_t) } if (any_exec.timer) { // Guard against multiple threads getting the same timer. - std::lock_guard lock(scheduled_timers_mutex_); if (scheduled_timers_.count(any_exec.timer) != 0) { continue; } @@ -96,7 +95,7 @@ MultiThreadedExecutor::run(size_t) execute_any_executable(any_exec); if (any_exec.timer) { - std::lock_guard lock(scheduled_timers_mutex_); + std::lock_guard wait_lock(wait_mutex_); auto it = scheduled_timers_.find(any_exec.timer); if (it != scheduled_timers_.end()) { scheduled_timers_.erase(it); From 2462a942468417c6e2ffb5b8daafadc642f433a4 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Fri, 22 Mar 2019 13:56:12 -0700 Subject: [PATCH 20/27] Back out Waitable and GraphEvent-related changes Signed-off-by: Emerson Knapp --- rclcpp/include/rclcpp/any_executable.hpp | 12 +- rclcpp/include/rclcpp/client.hpp | 24 +- rclcpp/include/rclcpp/create_service.hpp | 5 +- rclcpp/include/rclcpp/create_subscription.hpp | 12 +- rclcpp/include/rclcpp/executor.hpp | 22 ++ .../executors/multi_threaded_executor.hpp | 2 +- rclcpp/include/rclcpp/graph_event.hpp | 28 --- rclcpp/include/rclcpp/memory_strategy.hpp | 15 ++ rclcpp/include/rclcpp/node.hpp | 12 +- rclcpp/include/rclcpp/node_impl.hpp | 7 +- .../rclcpp/node_interfaces/node_graph.hpp | 8 +- .../node_interfaces/node_graph_interface.hpp | 16 +- rclcpp/include/rclcpp/publisher.hpp | 13 +- rclcpp/include/rclcpp/rclcpp.hpp | 2 +- rclcpp/include/rclcpp/service.hpp | 27 +- .../strategies/allocator_memory_strategy.hpp | 231 ++++++++++++++++-- rclcpp/include/rclcpp/subscription.hpp | 26 +- .../include/rclcpp/subscription_factory.hpp | 1 - rclcpp/include/rclcpp/time_source.hpp | 2 +- rclcpp/include/rclcpp/timer.hpp | 22 +- rclcpp/src/rclcpp/any_executable.cpp | 16 +- rclcpp/src/rclcpp/client.cpp | 75 ------ rclcpp/src/rclcpp/executor.cpp | 153 +++++++++++- .../executors/multi_threaded_executor.cpp | 10 +- rclcpp/src/rclcpp/node.cpp | 4 +- .../src/rclcpp/node_interfaces/node_graph.cpp | 8 +- rclcpp/src/rclcpp/publisher.cpp | 3 +- rclcpp/src/rclcpp/service.cpp | 75 ------ rclcpp/src/rclcpp/subscription.cpp | 130 +--------- rclcpp/src/rclcpp/time_source.cpp | 5 +- rclcpp/src/rclcpp/timer.cpp | 31 --- .../rclcpp_lifecycle/lifecycle_node.hpp | 14 +- .../rclcpp_lifecycle/lifecycle_node_impl.hpp | 5 +- rclcpp_lifecycle/src/lifecycle_node.cpp | 4 +- 34 files changed, 498 insertions(+), 522 deletions(-) delete mode 100644 rclcpp/include/rclcpp/graph_event.hpp diff --git a/rclcpp/include/rclcpp/any_executable.hpp b/rclcpp/include/rclcpp/any_executable.hpp index a8741bdca8..7b9bac00aa 100644 --- a/rclcpp/include/rclcpp/any_executable.hpp +++ b/rclcpp/include/rclcpp/any_executable.hpp @@ -35,16 +35,18 @@ namespace executor struct AnyExecutable { RCLCPP_PUBLIC - AnyExecutable() = default; + AnyExecutable(); RCLCPP_PUBLIC virtual ~AnyExecutable(); - bool - has_timer() const; - + // Only one of the following pointers will be set. + rclcpp::SubscriptionBase::SharedPtr subscription; + rclcpp::SubscriptionBase::SharedPtr subscription_intra_process; + rclcpp::TimerBase::SharedPtr timer; + rclcpp::ServiceBase::SharedPtr service; + rclcpp::ClientBase::SharedPtr client; rclcpp::Waitable::SharedPtr waitable; - // These are used to keep the scope on the containing items rclcpp::callback_group::CallbackGroup::SharedPtr callback_group; rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base; diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 83840c20ab..fd2454f80e 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -27,7 +27,6 @@ #include "rcl/error_handling.h" #include "rcl/wait.h" -#include "rclcpp/waitable.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/function_traits.hpp" #include "rclcpp/macros.hpp" @@ -50,7 +49,7 @@ namespace node_interfaces class NodeBaseInterface; } // namespace node_interfaces -class ClientBase : public Waitable +class ClientBase { public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientBase) @@ -83,26 +82,6 @@ class ClientBase : public Waitable std::shared_ptr get_event_handle() const; - RCLCPP_PUBLIC - size_t - get_number_of_ready_clients() override; - - RCLCPP_PUBLIC - size_t - get_number_of_ready_events() override; - - RCLCPP_PUBLIC - bool - add_to_wait_set(rcl_wait_set_t * wait_set) override; - - RCLCPP_PUBLIC - bool - is_ready(rcl_wait_set_t * wait_set) override; - - RCLCPP_PUBLIC - void - execute() override; - RCLCPP_PUBLIC bool service_is_ready() const; @@ -182,7 +161,6 @@ class Client : public ClientBase using rosidl_typesupport_cpp::get_service_type_support_handle; auto service_type_support_handle = get_service_type_support_handle(); - rcl_ret_t ret = rcl_client_init( this->get_client_handle().get(), this->get_rcl_node_handle(), diff --git a/rclcpp/include/rclcpp/create_service.hpp b/rclcpp/include/rclcpp/create_service.hpp index d01c0a23eb..ac7846239d 100644 --- a/rclcpp/include/rclcpp/create_service.hpp +++ b/rclcpp/include/rclcpp/create_service.hpp @@ -46,11 +46,10 @@ create_service( service_options.qos = qos_profile; auto serv = Service::make_shared( - node_base->get_shared_rcl_node_handle(), service_name, service_options, any_service_callback); + node_base->get_shared_rcl_node_handle(), + service_name, any_service_callback, service_options); auto serv_base_ptr = std::dynamic_pointer_cast(serv); - node_services->add_service(serv_base_ptr, group); - return serv; } diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index 8510705ecd..f5a102d2a4 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -42,18 +42,19 @@ create_subscription( rclcpp::callback_group::CallbackGroup::SharedPtr group, bool ignore_local_publications, bool use_intra_process_comms, - typename rclcpp::message_memory_strategy::MessageMemoryStrategy::SharedPtr msg_mem_strat, + typename rclcpp::message_memory_strategy::MessageMemoryStrategy< + CallbackMessageT, AllocatorT>::SharedPtr + msg_mem_strat, typename std::shared_ptr allocator) { auto subscription_options = rcl_subscription_get_default_options(); subscription_options.qos = qos_profile; subscription_options.ignore_local_publications = ignore_local_publications; - auto factory = rclcpp::create_subscription_factory( + auto factory = rclcpp::create_subscription_factory + ( std::forward(callback), - callbacks, + callbacks, msg_mem_strat, allocator); @@ -63,7 +64,6 @@ create_subscription( subscription_options, use_intra_process_comms); node_topics->add_subscription(sub, group); - return std::dynamic_pointer_cast(sub); } diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 951163b509..a87a2d0b1a 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -298,6 +298,28 @@ class Executor void execute_any_executable(AnyExecutable & any_exec); + RCLCPP_PUBLIC + static void + execute_subscription( + rclcpp::SubscriptionBase::SharedPtr subscription); + + RCLCPP_PUBLIC + static void + execute_intra_process_subscription( + rclcpp::SubscriptionBase::SharedPtr subscription); + + RCLCPP_PUBLIC + static void + execute_timer(rclcpp::TimerBase::SharedPtr timer); + + RCLCPP_PUBLIC + static void + execute_service(rclcpp::ServiceBase::SharedPtr service); + + RCLCPP_PUBLIC + static void + execute_client(rclcpp::ClientBase::SharedPtr client); + RCLCPP_PUBLIC void wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); diff --git a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp index 709c71190f..eb41a93e11 100644 --- a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp @@ -79,7 +79,7 @@ class MultiThreadedExecutor : public executor::Executor bool yield_before_execute_; std::mutex scheduled_timers_mutex_; - std::set scheduled_timers_; + std::set scheduled_timers_; }; } // namespace executors diff --git a/rclcpp/include/rclcpp/graph_event.hpp b/rclcpp/include/rclcpp/graph_event.hpp deleted file mode 100644 index 5362354aab..0000000000 --- a/rclcpp/include/rclcpp/graph_event.hpp +++ /dev/null @@ -1,28 +0,0 @@ -// Copyright 2016 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__GRAPH_EVENT_HPP_ -#define RCLCPP__GRAPH_EVENT_HPP_ - -#include "rclcpp/event.hpp" - - -namespace rclcpp -{ - -class GraphEvent : public Event {}; - -} // namespace rclcpp - -#endif // RCLCPP__GRAPH_EVENT_HPP_ diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp index 429b53f90c..70db12237a 100644 --- a/rclcpp/include/rclcpp/memory_strategy.hpp +++ b/rclcpp/include/rclcpp/memory_strategy.hpp @@ -64,6 +64,21 @@ class RCLCPP_PUBLIC MemoryStrategy virtual void remove_guard_condition(const rcl_guard_condition_t * guard_condition) = 0; + virtual void + get_next_subscription( + rclcpp::executor::AnyExecutable & any_exec, + const WeakNodeVector & weak_nodes) = 0; + + virtual void + get_next_service( + rclcpp::executor::AnyExecutable & any_exec, + const WeakNodeVector & weak_nodes) = 0; + + virtual void + get_next_client( + rclcpp::executor::AnyExecutable & any_exec, + const WeakNodeVector & weak_nodes) = 0; + virtual void get_next_waitable( rclcpp::executor::AnyExecutable & any_exec, diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index b8ec5a4241..bbc038e00b 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -366,17 +366,17 @@ class Node : public std::enable_shared_from_this count_subscribers(const std::string & topic_name) const; /// Return a graph event, which will be set anytime a graph change occurs. - /* The graph GraphEvent object is a loan which must be returned. - * The GraphEvent object is scoped and therefore to return the loan just let it go + /* The graph Event object is a loan which must be returned. + * The Event object is scoped and therefore to return the loan just let it go * out of scope. */ RCLCPP_PUBLIC - rclcpp::GraphEvent::SharedPtr + rclcpp::Event::SharedPtr get_graph_event(); - /// Wait for a graph event to occur by waiting on an GraphEvent to become set. + /// Wait for a graph event to occur by waiting on an Event to become set. /** - * The given GraphEvent must be acquire through the get_graph_event() method. + * The given Event must be acquire through the get_graph_event() method. * * \throws InvalidEventError if the given event is nullptr * \throws EventNotRegisteredError if the given event was not acquired with @@ -385,7 +385,7 @@ class Node : public std::enable_shared_from_this RCLCPP_PUBLIC void wait_for_graph_change( - rclcpp::GraphEvent::SharedPtr event, + rclcpp::Event::SharedPtr event, std::chrono::nanoseconds timeout); RCLCPP_PUBLIC diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 7884b2ea08..7fb4e177e1 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -85,7 +85,11 @@ Node::create_publisher( allocator); } -template +template< + typename MessageT, + typename CallbackT, + typename Alloc, + typename SubscriptionT> std::shared_ptr Node::create_subscription( const std::string & topic_name, @@ -157,7 +161,6 @@ Node::create_client( auto cli_base_ptr = std::dynamic_pointer_cast(cli); node_services_->add_client(cli_base_ptr, group); - return cli; } diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp index cdb88b8a7d..8ecaf074f2 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp @@ -25,7 +25,7 @@ #include "rcl/guard_condition.h" -#include "rclcpp/graph_event.hpp" +#include "rclcpp/event.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/node_interfaces/node_graph_interface.hpp" @@ -102,14 +102,14 @@ class NodeGraph : public NodeGraphInterface RCLCPP_PUBLIC virtual - rclcpp::GraphEvent::SharedPtr + rclcpp::Event::SharedPtr get_graph_event(); RCLCPP_PUBLIC virtual void wait_for_graph_change( - rclcpp::GraphEvent::SharedPtr event, + rclcpp::Event::SharedPtr event, std::chrono::nanoseconds timeout); RCLCPP_PUBLIC @@ -133,7 +133,7 @@ class NodeGraph : public NodeGraphInterface /// For notifying waiting threads (wait_for_graph_change()) on changes (notify_graph_change()). std::condition_variable graph_cv_; /// Weak references to graph events out on loan. - std::vector graph_events_; + std::vector graph_events_; /// Number of graph events out on loan, used to determine if the graph should be monitored. /** graph_users_count_ is atomic so that it can be accessed without acquiring the graph_mutex_ */ std::atomic_size_t graph_users_count_; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp index f80b28809b..022984dd47 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp @@ -23,7 +23,7 @@ #include "rcl/guard_condition.h" -#include "rclcpp/graph_event.hpp" +#include "rclcpp/event.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/visibility_control.hpp" @@ -99,7 +99,7 @@ class NodeGraphInterface /** * Affects threads waiting on the notify guard condition, see: * get_notify_guard_condition(), as well as the threads waiting on graph - * changes using a graph GraphEvent, see: wait_for_graph_change(). + * changes using a graph Event, see: wait_for_graph_change(). * * This is typically only used by the rclcpp::graph_listener::GraphListener. * @@ -118,18 +118,18 @@ class NodeGraphInterface /// Return a graph event, which will be set anytime a graph change occurs. /** - * The graph GraphEvent object is a loan which must be returned. - * The GraphEvent object is scoped and therefore to return the load just let it go + * The graph Event object is a loan which must be returned. + * The Event object is scoped and therefore to return the load just let it go * out of scope. */ RCLCPP_PUBLIC virtual - rclcpp::GraphEvent::SharedPtr + rclcpp::Event::SharedPtr get_graph_event() = 0; - /// Wait for a graph event to occur by waiting on an GraphEvent to become set. + /// Wait for a graph event to occur by waiting on an Event to become set. /** - * The given GraphEvent must be acquire through the get_graph_event() method. + * The given Event must be acquire through the get_graph_event() method. * * \throws InvalidEventError if the given event is nullptr * \throws EventNotRegisteredError if the given event was not acquired with @@ -139,7 +139,7 @@ class NodeGraphInterface virtual void wait_for_graph_change( - rclcpp::GraphEvent::SharedPtr event, + rclcpp::Event::SharedPtr event, std::chrono::nanoseconds timeout) = 0; /// Return the number of on loan graph events, see get_graph_event(). diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index b0d26670da..b0b346ba43 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -40,7 +40,6 @@ #include "rclcpp/macros.hpp" #include "rclcpp/visibility_control.hpp" - namespace rclcpp { @@ -188,17 +187,16 @@ class PublisherBase const rcl_publisher_options_t & intra_process_options); protected: - using IntraProcessManagerWeakPtr = - std::weak_ptr; - std::shared_ptr rcl_node_handle_; rcl_publisher_t publisher_handle_ = rcl_get_zero_initialized_publisher(); + rcl_publisher_t intra_process_publisher_handle_ = rcl_get_zero_initialized_publisher(); + +std::vector> event_handles_; - std::vector> event_handles_; - + using IntraProcessManagerWeakPtr = + std::weak_ptr; bool use_intra_process_; - rcl_publisher_t intra_process_publisher_handle_ = rcl_get_zero_initialized_publisher(); IntraProcessManagerWeakPtr weak_ipm_; uint64_t intra_process_publisher_id_; StoreMessageCallbackT store_intra_process_message_; @@ -401,6 +399,7 @@ class Publisher : public PublisherBase } std::shared_ptr message_allocator_; + MessageDeleter message_deleter_; }; diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index 73da334601..698688b6ef 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -88,7 +88,7 @@ * - Graph Events (a waitable event object that wakes up when the graph changes): * - rclcpp::Node::get_graph_event() * - rclcpp::Node::wait_for_graph_change() - * - rclcpp::GraphEvent + * - rclcpp::Event * - List topic names and types: * - rclcpp::Node::get_topic_names_and_types() * - Get the number of publishers or subscribers on a topic: diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index f89e70d44b..c79ef707bd 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -24,7 +24,6 @@ #include "rcl/error_handling.h" #include "rcl/service.h" -#include "rclcpp/waitable.hpp" #include "rclcpp/any_service_callback.hpp" #include "rclcpp/event.hpp" #include "rclcpp/exceptions.hpp" @@ -39,7 +38,7 @@ namespace rclcpp { -class ServiceBase : public Waitable +class ServiceBase { public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ServiceBase) @@ -71,26 +70,6 @@ class ServiceBase : public Waitable std::shared_ptr get_event_handle() const; - RCLCPP_PUBLIC - size_t - get_number_of_ready_services() override; - - RCLCPP_PUBLIC - size_t - get_number_of_ready_events() override; - - RCLCPP_PUBLIC - bool - add_to_wait_set(rcl_wait_set_t * wait_set) override; - - RCLCPP_PUBLIC - bool - is_ready(rcl_wait_set_t * wait_set) override; - - RCLCPP_PUBLIC - void - execute() override; - virtual std::shared_ptr create_request() = 0; virtual std::shared_ptr create_request_header() = 0; virtual void handle_request( @@ -139,8 +118,8 @@ class Service : public ServiceBase Service( std::shared_ptr node_handle, const std::string & service_name, - rcl_service_options_t & service_options, - AnyServiceCallback any_callback) + AnyServiceCallback any_callback, + rcl_service_options_t & service_options) : ServiceBase(node_handle), any_callback_(any_callback) { using rosidl_typesupport_cpp::get_service_type_support_handle; diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 42a24dbab1..6c76cad643 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -83,6 +83,10 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy void clear_handles() { + subscription_handles_.clear(); + service_handles_.clear(); + client_handles_.clear(); + timer_handles_.clear(); waitable_handles_.clear(); } @@ -91,12 +95,55 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy // TODO(jacobperron): Check if wait set sizes are what we expect them to be? // e.g. wait_set->size_of_clients == client_handles_.size() + // Important to use subscription_handles_.size() instead of wait set's size since + // there may be more subscriptions in the wait set due to Waitables added to the end. + // The same logic applies for other entities. + for (size_t i = 0; i < subscription_handles_.size(); ++i) { + if (!wait_set->subscriptions[i]) { + subscription_handles_[i].reset(); + } + } + for (size_t i = 0; i < service_handles_.size(); ++i) { + if (!wait_set->services[i]) { + service_handles_[i].reset(); + } + } + for (size_t i = 0; i < client_handles_.size(); ++i) { + if (!wait_set->clients[i]) { + client_handles_[i].reset(); + } + } + for (size_t i = 0; i < timer_handles_.size(); ++i) { + if (!wait_set->timers[i]) { + timer_handles_[i].reset(); + } + } for (size_t i = 0; i < waitable_handles_.size(); ++i) { if (!waitable_handles_[i]->is_ready(wait_set)) { waitable_handles_[i].reset(); } } + subscription_handles_.erase( + std::remove(subscription_handles_.begin(), subscription_handles_.end(), nullptr), + subscription_handles_.end() + ); + + service_handles_.erase( + std::remove(service_handles_.begin(), service_handles_.end(), nullptr), + service_handles_.end() + ); + + client_handles_.erase( + std::remove(client_handles_.begin(), client_handles_.end(), nullptr), + client_handles_.end() + ); + + timer_handles_.erase( + std::remove(timer_handles_.begin(), timer_handles_.end(), nullptr), + timer_handles_.end() + ); + waitable_handles_.erase( std::remove(waitable_handles_.begin(), waitable_handles_.end(), nullptr), waitable_handles_.end() @@ -117,40 +164,32 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy if (!group || !group->can_be_taken_from().load()) { continue; } - - for (auto & weak_publisher : group->get_publisher_ptrs()) { - auto publisher = weak_publisher.lock(); - if (publisher) { - for (auto & publisher_event : publisher->get_event_handles()) { - waitable_handles_.push_back(publisher_event); - } - } - } for (auto & weak_subscription : group->get_subscription_ptrs()) { auto subscription = weak_subscription.lock(); if (subscription) { - waitable_handles_.push_back(subscription); - for (auto & subscription_event : subscription->get_event_handles()) { - waitable_handles_.push_back(subscription_event); + subscription_handles_.push_back(subscription->get_subscription_handle()); + if (subscription->get_intra_process_subscription_handle()) { + subscription_handles_.push_back( + subscription->get_intra_process_subscription_handle()); } } } for (auto & weak_service : group->get_service_ptrs()) { auto service = weak_service.lock(); if (service) { - waitable_handles_.push_back(service); + service_handles_.push_back(service->get_service_handle()); } } for (auto & weak_client : group->get_client_ptrs()) { auto client = weak_client.lock(); if (client) { - waitable_handles_.push_back(client); + client_handles_.push_back(client->get_client_handle()); } } for (auto & weak_timer : group->get_timer_ptrs()) { auto timer = weak_timer.lock(); if (timer) { - waitable_handles_.push_back(timer); + timer_handles_.push_back(timer->get_timer_handle()); } } for (auto & weak_waitable : group->get_waitable_ptrs()) { @@ -166,6 +205,42 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy bool add_handles_to_wait_set(rcl_wait_set_t * wait_set) { + for (auto subscription : subscription_handles_) { + if (rcl_wait_set_add_subscription(wait_set, subscription.get(), NULL) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't add subscription to wait set: %s", rcl_get_error_string().str); + return false; + } + } + + for (auto client : client_handles_) { + if (rcl_wait_set_add_client(wait_set, client.get(), NULL) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't add client to wait set: %s", rcl_get_error_string().str); + return false; + } + } + + for (auto service : service_handles_) { + if (rcl_wait_set_add_service(wait_set, service.get(), NULL) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't add service to wait set: %s", rcl_get_error_string().str); + return false; + } + } + + for (auto timer : timer_handles_) { + if (rcl_wait_set_add_timer(wait_set, timer.get(), NULL) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't add timer to wait set: %s", rcl_get_error_string().str); + return false; + } + } + for (auto guard_condition : guard_conditions_) { if (rcl_wait_set_add_guard_condition(wait_set, guard_condition, NULL) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( @@ -187,6 +262,118 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy return true; } + virtual void + get_next_subscription( + executor::AnyExecutable & any_exec, + const WeakNodeVector & weak_nodes) + { + auto it = subscription_handles_.begin(); + while (it != subscription_handles_.end()) { + auto subscription = get_subscription_by_handle(*it, weak_nodes); + if (subscription) { + // Figure out if this is for intra-process or not. + bool is_intra_process = false; + if (subscription->get_intra_process_subscription_handle()) { + is_intra_process = subscription->get_intra_process_subscription_handle() == *it; + } + // Find the group for this handle and see if it can be serviced + auto group = get_group_by_subscription(subscription, weak_nodes); + if (!group) { + // Group was not found, meaning the subscription is not valid... + // Remove it from the ready list and continue looking + it = subscription_handles_.erase(it); + continue; + } + if (!group->can_be_taken_from().load()) { + // Group is mutually exclusive and is being used, so skip it for now + // Leave it to be checked next time, but continue searching + ++it; + continue; + } + // Otherwise it is safe to set and return the any_exec + if (is_intra_process) { + any_exec.subscription_intra_process = subscription; + } else { + any_exec.subscription = subscription; + } + any_exec.callback_group = group; + any_exec.node_base = get_node_by_group(group, weak_nodes); + subscription_handles_.erase(it); + return; + } + // Else, the subscription is no longer valid, remove it and continue + it = subscription_handles_.erase(it); + } + } + + virtual void + get_next_service( + executor::AnyExecutable & any_exec, + const WeakNodeVector & weak_nodes) + { + auto it = service_handles_.begin(); + while (it != service_handles_.end()) { + auto service = get_service_by_handle(*it, weak_nodes); + if (service) { + // Find the group for this handle and see if it can be serviced + auto group = get_group_by_service(service, weak_nodes); + if (!group) { + // Group was not found, meaning the service is not valid... + // Remove it from the ready list and continue looking + it = service_handles_.erase(it); + continue; + } + if (!group->can_be_taken_from().load()) { + // Group is mutually exclusive and is being used, so skip it for now + // Leave it to be checked next time, but continue searching + ++it; + continue; + } + // Otherwise it is safe to set and return the any_exec + any_exec.service = service; + any_exec.callback_group = group; + any_exec.node_base = get_node_by_group(group, weak_nodes); + service_handles_.erase(it); + return; + } + // Else, the service is no longer valid, remove it and continue + it = service_handles_.erase(it); + } + } + + virtual void + get_next_client(executor::AnyExecutable & any_exec, const WeakNodeVector & weak_nodes) + { + auto it = client_handles_.begin(); + while (it != client_handles_.end()) { + auto client = get_client_by_handle(*it, weak_nodes); + if (client) { + // Find the group for this handle and see if it can be serviced + auto group = get_group_by_client(client, weak_nodes); + if (!group) { + // Group was not found, meaning the service is not valid... + // Remove it from the ready list and continue looking + it = client_handles_.erase(it); + continue; + } + if (!group->can_be_taken_from().load()) { + // Group is mutually exclusive and is being used, so skip it for now + // Leave it to be checked next time, but continue searching + ++it; + continue; + } + // Otherwise it is safe to set and return the any_exec + any_exec.client = client; + any_exec.callback_group = group; + any_exec.node_base = get_node_by_group(group, weak_nodes); + client_handles_.erase(it); + return; + } + // Else, the service is no longer valid, remove it and continue + it = client_handles_.erase(it); + } + } + virtual void get_next_waitable(executor::AnyExecutable & any_exec, const WeakNodeVector & weak_nodes) { @@ -227,7 +414,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy size_t number_of_ready_subscriptions() const { - size_t number_of_subscriptions = 0; + size_t number_of_subscriptions = subscription_handles_.size(); for (auto waitable : waitable_handles_) { number_of_subscriptions += waitable->get_number_of_ready_subscriptions(); } @@ -236,7 +423,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy size_t number_of_ready_services() const { - size_t number_of_services = 0; + size_t number_of_services = service_handles_.size(); for (auto waitable : waitable_handles_) { number_of_services += waitable->get_number_of_ready_services(); } @@ -252,9 +439,10 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy return number_of_events; } + size_t number_of_ready_clients() const { - size_t number_of_clients = 0; + size_t number_of_clients = client_handles_.size(); for (auto waitable : waitable_handles_) { number_of_clients += waitable->get_number_of_ready_clients(); } @@ -272,7 +460,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy size_t number_of_ready_timers() const { - size_t number_of_timers = 0; + size_t number_of_timers = timer_handles_.size(); for (auto waitable : waitable_handles_) { number_of_timers += waitable->get_number_of_ready_timers(); } @@ -290,6 +478,11 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy std::vector::template rebind_alloc>; VectorRebind guard_conditions_; + + VectorRebind> subscription_handles_; + VectorRebind> service_handles_; + VectorRebind> client_handles_; + VectorRebind> timer_handles_; VectorRebind> waitable_handles_; std::shared_ptr allocator_; diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 55beb7f5d8..3890e29ab3 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -42,7 +42,6 @@ #include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" - namespace rclcpp { @@ -62,7 +61,7 @@ class IntraProcessManager; /// Virtual base class for subscriptions. This pattern allows us to iterate over different template /// specializations of Subscription, among other things. -class SubscriptionBase : public Waitable +class SubscriptionBase { public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(SubscriptionBase) @@ -97,11 +96,11 @@ class SubscriptionBase : public Waitable get_subscription_handle(); RCLCPP_PUBLIC - std::shared_ptr + const std::shared_ptr get_subscription_handle() const; RCLCPP_PUBLIC - virtual std::shared_ptr + virtual const std::shared_ptr get_intra_process_subscription_handle() const; // RCLCPP_PUBLIC @@ -124,22 +123,6 @@ class SubscriptionBase : public Waitable const std::vector> & get_event_handles() const; - RCLCPP_PUBLIC - size_t - get_number_of_ready_subscriptions() override; - - RCLCPP_PUBLIC - bool - add_to_wait_set(rcl_wait_set_t * wait_set) override; - - RCLCPP_PUBLIC - bool - is_ready(rcl_wait_set_t * wait_set) override; - - RCLCPP_PUBLIC - void - execute() override; - /// Borrow a new message. /** \return Shared pointer to the fresh message. */ virtual std::shared_ptr @@ -365,7 +348,6 @@ class Subscription : public SubscriptionBase const rcl_subscription_options_t & intra_process_options) { std::string intra_process_topic_name = std::string(get_topic_name()) + "/_intra"; - rcl_ret_t ret = rcl_subscription_init( intra_process_subscription_handle_.get(), node_handle_.get(), @@ -394,7 +376,7 @@ class Subscription : public SubscriptionBase } /// Implemenation detail. - std::shared_ptr + const std::shared_ptr get_intra_process_subscription_handle() const { if (!get_intra_process_message_callback_) { diff --git a/rclcpp/include/rclcpp/subscription_factory.hpp b/rclcpp/include/rclcpp/subscription_factory.hpp index 88571ddc09..905e10d3de 100644 --- a/rclcpp/include/rclcpp/subscription_factory.hpp +++ b/rclcpp/include/rclcpp/subscription_factory.hpp @@ -113,7 +113,6 @@ create_subscription_factory( event_callbacks, msg_mem_strat); auto sub_base_ptr = std::dynamic_pointer_cast(sub); - return sub_base_ptr; }; diff --git a/rclcpp/include/rclcpp/time_source.hpp b/rclcpp/include/rclcpp/time_source.hpp index 829c7d60b9..09a8340383 100644 --- a/rclcpp/include/rclcpp/time_source.hpp +++ b/rclcpp/include/rclcpp/time_source.hpp @@ -103,7 +103,7 @@ class TimeSource // Parameter Client pointer std::shared_ptr parameter_client_; - // ParameterEvent subscription + // Parameter Event subscription using ParamMessageT = rcl_interfaces::msg::ParameterEvent; using ParamSubscriptionT = rclcpp::Subscription; std::shared_ptr parameter_subscription_; diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index 2f47d206e1..34eac08348 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -24,7 +24,6 @@ #include #include "rclcpp/clock.hpp" -#include "rclcpp/waitable.hpp" #include "rclcpp/context.hpp" #include "rclcpp/function_traits.hpp" #include "rclcpp/macros.hpp" @@ -41,7 +40,7 @@ namespace rclcpp { -class TimerBase : public Waitable +class TimerBase { public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(TimerBase) @@ -71,22 +70,6 @@ class TimerBase : public Waitable std::shared_ptr get_timer_handle(); - RCLCPP_PUBLIC - size_t - get_number_of_ready_timers() override; - - RCLCPP_PUBLIC - bool - add_to_wait_set(rcl_wait_set_t * wait_set) override; - - RCLCPP_PUBLIC - bool - is_ready(rcl_wait_set_t * wait_set) override; - - RCLCPP_PUBLIC - void - execute() override; - /// Check how long the timer has until its next scheduled callback. /** \return A std::chrono::duration representing the relative time until the next callback. */ RCLCPP_PUBLIC @@ -108,10 +91,7 @@ class TimerBase : public Waitable protected: Clock::SharedPtr clock_; - std::shared_ptr timer_handle_; - size_t wait_set_timer_index_; - bool timer_expired_; }; diff --git a/rclcpp/src/rclcpp/any_executable.cpp b/rclcpp/src/rclcpp/any_executable.cpp index 07f6ceabab..256033bbad 100644 --- a/rclcpp/src/rclcpp/any_executable.cpp +++ b/rclcpp/src/rclcpp/any_executable.cpp @@ -16,6 +16,16 @@ using rclcpp::executor::AnyExecutable; +AnyExecutable::AnyExecutable() +: subscription(nullptr), + subscription_intra_process(nullptr), + timer(nullptr), + service(nullptr), + client(nullptr), + callback_group(nullptr), + node_base(nullptr) +{} + AnyExecutable::~AnyExecutable() { // Make sure that discarded (taken but not executed) AnyExecutable's have @@ -25,9 +35,3 @@ AnyExecutable::~AnyExecutable() callback_group->can_be_taken_from().store(true); } } - -bool -AnyExecutable::has_timer() const -{ - return waitable->get_number_of_ready_timers() > 0; -} diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index 6189893dd5..6cbcbeb70f 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -41,7 +41,6 @@ ClientBase::ClientBase( context_(node_base->get_context()) { std::weak_ptr weak_node_handle(node_handle_); - rcl_client_t * new_rcl_client = new rcl_client_t; *new_rcl_client = rcl_get_zero_initialized_client(); client_handle_.reset( @@ -114,80 +113,6 @@ ClientBase::get_event_handle() const return event_handle_; } -size_t -ClientBase::get_number_of_ready_clients() -{ - return 1; -} - -size_t -ClientBase::get_number_of_ready_events() -{ - return 1; -} - -bool -ClientBase::add_to_wait_set(rcl_wait_set_t * wait_set) -{ - if ( - rcl_wait_set_add_client(wait_set, client_handle_.get(), &wait_set_client_index_) != RCL_RET_OK) - { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't add client to wait set: %s", rcl_get_error_string().str); - return false; - } - - // TODO(mm318): enable QOS event callbacks for clients - // (currently only for publishers and subscriptions) - wait_set_event_index_ = 0; - // if ( - // rcl_wait_set_add_event(wait_set, event_handle_.get(), &wait_set_event_index_) != RCL_RET_OK) - // { - // RCUTILS_LOG_ERROR_NAMED( - // "rclcpp", - // "Couldn't add client event to wait set: %s", rcl_get_error_string().str); - // return false; - // } - - return true; -} - -bool -ClientBase::is_ready(rcl_wait_set_t * wait_set) -{ - client_ready_ = (wait_set->clients[wait_set_client_index_] == client_handle_.get()); - event_ready_ = (wait_set->events[wait_set_event_index_] == event_handle_.get()); - return client_ready_ || event_ready_; -} - -void -ClientBase::execute() -{ - if (client_ready_) { - auto request_header = create_request_header(); - std::shared_ptr response = create_response(); - rcl_ret_t status = rcl_take_response( - get_client_handle().get(), - request_header.get(), - response.get()); - if (status == RCL_RET_OK) { - handle_response(request_header, response); - } else if (status != RCL_RET_CLIENT_TAKE_FAILED) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "take response failed for client of service '%s': %s", - get_service_name(), rcl_get_error_string().str); - rcl_reset_error(); - } - } - - if (event_ready_) { - // rcl_take_event(); - // handle_event(example_event); - } -} - bool ClientBase::service_is_ready() const { diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 4973ab88c8..f833bcf0a3 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -267,7 +267,21 @@ Executor::execute_any_executable(AnyExecutable & any_exec) if (!spinning.load()) { return; } - + if (any_exec.timer) { + execute_timer(any_exec.timer); + } + if (any_exec.subscription) { + execute_subscription(any_exec.subscription); + } + if (any_exec.subscription_intra_process) { + execute_intra_process_subscription(any_exec.subscription_intra_process); + } + if (any_exec.service) { + execute_service(any_exec.service); + } + if (any_exec.client) { + execute_client(any_exec.client); + } if (any_exec.waitable) { any_exec.waitable->execute(); } @@ -280,6 +294,119 @@ Executor::execute_any_executable(AnyExecutable & any_exec) } } +void +Executor::execute_subscription( + rclcpp::SubscriptionBase::SharedPtr subscription) +{ + rmw_message_info_t message_info; + message_info.from_intra_process = false; + + if (subscription->is_serialized()) { + auto serialized_msg = subscription->create_serialized_message(); + auto ret = rcl_take_serialized_message( + subscription->get_subscription_handle().get(), + serialized_msg.get(), &message_info); + if (RCL_RET_OK == ret) { + auto void_serialized_msg = std::static_pointer_cast(serialized_msg); + subscription->handle_message(void_serialized_msg, message_info); + } else if (RCL_RET_SUBSCRIPTION_TAKE_FAILED != ret) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "take_serialized failed for subscription on topic '%s': %s", + subscription->get_topic_name(), rcl_get_error_string().str); + rcl_reset_error(); + } + subscription->return_serialized_message(serialized_msg); + } else { + std::shared_ptr message = subscription->create_message(); + auto ret = rcl_take( + subscription->get_subscription_handle().get(), + message.get(), &message_info); + if (RCL_RET_OK == ret) { + subscription->handle_message(message, message_info); + } else if (RCL_RET_SUBSCRIPTION_TAKE_FAILED != ret) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "could not deserialize serialized message on topic '%s': %s", + subscription->get_topic_name(), rcl_get_error_string().str); + rcl_reset_error(); + } + subscription->return_message(message); + } +} + +void +Executor::execute_intra_process_subscription( + rclcpp::SubscriptionBase::SharedPtr subscription) +{ + rcl_interfaces::msg::IntraProcessMessage ipm; + rmw_message_info_t message_info; + rcl_ret_t status = rcl_take( + subscription->get_intra_process_subscription_handle().get(), + &ipm, + &message_info); + + if (status == RCL_RET_OK) { + message_info.from_intra_process = true; + subscription->handle_intra_process_message(ipm, message_info); + } else if (status != RCL_RET_SUBSCRIPTION_TAKE_FAILED) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "take failed for intra process subscription on topic '%s': %s", + subscription->get_topic_name(), rcl_get_error_string().str); + rcl_reset_error(); + } +} + +void +Executor::execute_timer( + rclcpp::TimerBase::SharedPtr timer) +{ + timer->execute_callback(); +} + +void +Executor::execute_service( + rclcpp::ServiceBase::SharedPtr service) +{ + auto request_header = service->create_request_header(); + std::shared_ptr request = service->create_request(); + rcl_ret_t status = rcl_take_request( + service->get_service_handle().get(), + request_header.get(), + request.get()); + if (status == RCL_RET_OK) { + service->handle_request(request_header, request); + } else if (status != RCL_RET_SERVICE_TAKE_FAILED) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "take request failed for server of service '%s': %s", + service->get_service_name(), rcl_get_error_string().str); + rcl_reset_error(); + } +} + +void +Executor::execute_client( + rclcpp::ClientBase::SharedPtr client) +{ + auto request_header = client->create_request_header(); + std::shared_ptr response = client->create_response(); + rcl_ret_t status = rcl_take_response( + client->get_client_handle().get(), + request_header.get(), + response.get()); + if (status == RCL_RET_OK) { + client->handle_response(request_header, response); + } else if (status != RCL_RET_CLIENT_TAKE_FAILED) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "take response failed for client of service '%s': %s", + client->get_service_name(), rcl_get_error_string().str); + rcl_reset_error(); + } +} + void Executor::wait_for_work(std::chrono::nanoseconds timeout) { @@ -395,7 +522,7 @@ Executor::get_next_timer(AnyExecutable & any_exec) for (auto & timer_ref : group->get_timer_ptrs()) { auto timer = timer_ref.lock(); if (timer && timer->is_ready()) { - any_exec.waitable = timer; + any_exec.timer = timer; any_exec.callback_group = group; node = get_node_by_group(group); return; @@ -408,12 +535,32 @@ Executor::get_next_timer(AnyExecutable & any_exec) bool Executor::get_next_ready_executable(AnyExecutable & any_executable) { + // Check the timers to see if there are any that are ready, if so return + get_next_timer(any_executable); + if (any_executable.timer) { + return true; + } + // Check the subscriptions to see if there are any that are ready + memory_strategy_->get_next_subscription(any_executable, weak_nodes_); + if (any_executable.subscription || any_executable.subscription_intra_process) { + return true; + } + // Check the services to see if there are any that are ready + memory_strategy_->get_next_service(any_executable, weak_nodes_); + if (any_executable.service) { + return true; + } + // Check the clients to see if there are any that are ready + memory_strategy_->get_next_client(any_executable, weak_nodes_); + if (any_executable.client) { + return true; + } // Check the waitables to see if there are any that are ready memory_strategy_->get_next_waitable(any_executable, weak_nodes_); if (any_executable.waitable) { return true; } - // If there is no ready executable, return false + // If there is no ready executable, return a null ptr return false; } diff --git a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp index b0d916a8c7..08318534b2 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -80,13 +80,13 @@ MultiThreadedExecutor::run(size_t) if (!get_next_executable(any_exec)) { continue; } - if (any_exec.has_timer()) { + if (any_exec.timer) { // Guard against multiple threads getting the same timer. std::lock_guard lock(scheduled_timers_mutex_); - if (scheduled_timers_.count(any_exec.waitable) != 0) { + if (scheduled_timers_.count(any_exec.timer) != 0) { continue; } - scheduled_timers_.insert(any_exec.waitable); + scheduled_timers_.insert(any_exec.timer); } } if (yield_before_execute_) { @@ -95,9 +95,9 @@ MultiThreadedExecutor::run(size_t) execute_any_executable(any_exec); - if (any_exec.has_timer()) { + if (any_exec.timer) { std::lock_guard lock(scheduled_timers_mutex_); - auto it = scheduled_timers_.find(any_exec.waitable); + auto it = scheduled_timers_.find(any_exec.timer); if (it != scheduled_timers_.end()) { scheduled_timers_.erase(it); } diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 5eaf3bd544..7935a8caf0 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -306,7 +306,7 @@ Node::get_callback_groups() const return node_base_->get_callback_groups(); } -rclcpp::GraphEvent::SharedPtr +rclcpp::Event::SharedPtr Node::get_graph_event() { return node_graph_->get_graph_event(); @@ -314,7 +314,7 @@ Node::get_graph_event() void Node::wait_for_graph_change( - rclcpp::GraphEvent::SharedPtr event, + rclcpp::Event::SharedPtr event, std::chrono::nanoseconds timeout) { node_graph_->wait_for_graph_change(event, timeout); diff --git a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp index a660a3f83d..d03e781f9d 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp @@ -273,7 +273,7 @@ NodeGraph::notify_graph_change() std::remove_if( graph_events_.begin(), graph_events_.end(), - [](const rclcpp::GraphEvent::WeakPtr & wptr) { + [](const rclcpp::Event::WeakPtr & wptr) { return wptr.expired(); }), graph_events_.end()); @@ -298,10 +298,10 @@ NodeGraph::notify_shutdown() graph_cv_.notify_all(); } -rclcpp::GraphEvent::SharedPtr +rclcpp::Event::SharedPtr NodeGraph::get_graph_event() { - auto event = rclcpp::GraphEvent::make_shared(); + auto event = rclcpp::Event::make_shared(); std::lock_guard graph_changed_lock(graph_mutex_); graph_events_.push_back(event); graph_users_count_++; @@ -315,7 +315,7 @@ NodeGraph::get_graph_event() void NodeGraph::wait_for_graph_change( - rclcpp::GraphEvent::SharedPtr event, + rclcpp::Event::SharedPtr event, std::chrono::nanoseconds timeout) { using rclcpp::exceptions::InvalidEventError; diff --git a/rclcpp/src/rclcpp/publisher.cpp b/rclcpp/src/rclcpp/publisher.cpp index ace6ec9364..8a57aee012 100644 --- a/rclcpp/src/rclcpp/publisher.cpp +++ b/rclcpp/src/rclcpp/publisher.cpp @@ -38,7 +38,6 @@ using rclcpp::PublisherBase; - PublisherBase::PublisherBase( rclcpp::node_interfaces::NodeBaseInterface * node_base, const std::string & topic, @@ -64,9 +63,9 @@ PublisherBase::PublisherBase( rcl_node_get_name(rcl_node_handle), rcl_node_get_namespace(rcl_node_handle)); } + rclcpp::exceptions::throw_from_rcl_error(ret, "could not create publisher"); } - // Life time of this object is tied to the publisher handle. rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle(&publisher_handle_); if (!publisher_rmw_handle) { diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index 24e00c824f..352f82a43b 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -64,81 +64,6 @@ ServiceBase::get_event_handle() const return event_handle_; } -size_t -ServiceBase::get_number_of_ready_services() -{ - return 1; -} - -size_t -ServiceBase::get_number_of_ready_events() -{ - return 1; -} - -bool -ServiceBase::add_to_wait_set(rcl_wait_set_t * wait_set) -{ - if ( - rcl_wait_set_add_service(wait_set, service_handle_.get(), &wait_set_service_index_) != - RCL_RET_OK) - { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't add service to wait set: %s", rcl_get_error_string().str); - return false; - } - - // TODO(mm318): enable QOS event callbacks for clients - // (currently only for publishers and subscriptions) - wait_set_event_index_ = 0; - // if ( - // rcl_wait_set_add_event(wait_set, event_handle_.get(), &wait_set_event_index_) != RCL_RET_OK) - // { - // RCUTILS_LOG_ERROR_NAMED( - // "rclcpp", - // "Couldn't add service event to wait set: %s", rcl_get_error_string().str); - // return false; - // } - - return true; -} - -bool -ServiceBase::is_ready(rcl_wait_set_t * wait_set) -{ - service_ready_ = (wait_set->services[wait_set_service_index_] == service_handle_.get()); - event_ready_ = (wait_set->events[wait_set_event_index_] == event_handle_.get()); - return service_ready_ || event_ready_; -} - -void -ServiceBase::execute() -{ - if (service_ready_) { - auto request_header = create_request_header(); - std::shared_ptr request = create_request(); - rcl_ret_t status = rcl_take_request( - get_service_handle().get(), - request_header.get(), - request.get()); - if (status == RCL_RET_OK) { - handle_request(request_header, request); - } else if (status != RCL_RET_SERVICE_TAKE_FAILED) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "take request failed for server of service '%s': %s", - get_service_name(), rcl_get_error_string().str); - rcl_reset_error(); - } - } - - if (event_ready_) { - // rcl_take_event(); - // handle_event(example_event); - } -} - rcl_node_t * ServiceBase::get_rcl_node_handle() { diff --git a/rclcpp/src/rclcpp/subscription.cpp b/rclcpp/src/rclcpp/subscription.cpp index f5ecd2aa39..ed26acaf17 100644 --- a/rclcpp/src/rclcpp/subscription.cpp +++ b/rclcpp/src/rclcpp/subscription.cpp @@ -16,7 +16,6 @@ #include #include -#include #include #include "rclcpp/exceptions.hpp" @@ -53,11 +52,14 @@ SubscriptionBase::SubscriptionBase( delete rcl_subs; }; - subscription_handle_ = std::shared_ptr( new rcl_subscription_t, custom_deletor); *subscription_handle_.get() = rcl_get_zero_initialized_subscription(); + intra_process_subscription_handle_ = std::shared_ptr( + new rcl_subscription_t, custom_deletor); + *intra_process_subscription_handle_.get() = rcl_get_zero_initialized_subscription(); + rcl_ret_t ret = rcl_subscription_init( subscription_handle_.get(), node_handle_.get(), @@ -77,11 +79,6 @@ SubscriptionBase::SubscriptionBase( rclcpp::exceptions::throw_from_rcl_error(ret, "could not create subscription"); } - - - intra_process_subscription_handle_ = std::shared_ptr( - new rcl_subscription_t, custom_deletor); - *intra_process_subscription_handle_.get() = rcl_get_zero_initialized_subscription(); } SubscriptionBase::~SubscriptionBase() @@ -112,133 +109,18 @@ SubscriptionBase::get_subscription_handle() return subscription_handle_; } -std::shared_ptr +const std::shared_ptr SubscriptionBase::get_subscription_handle() const { return subscription_handle_; } -std::shared_ptr +const std::shared_ptr SubscriptionBase::get_intra_process_subscription_handle() const { return intra_process_subscription_handle_; } -const std::vector> & -SubscriptionBase::get_event_handles() const -{ - return event_handles_; -} - -size_t -SubscriptionBase::get_number_of_ready_subscriptions() -{ - if (use_intra_process_) { - return 2; - } else { - return 1; - } -} - -bool -SubscriptionBase::add_to_wait_set(rcl_wait_set_t * wait_set) -{ - if (rcl_wait_set_add_subscription(wait_set, subscription_handle_.get(), - &wait_set_subscription_index_) != RCL_RET_OK) - { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't add subscription to wait set: %s", rcl_get_error_string().str); - return false; - } - - if (use_intra_process_) { - if (rcl_wait_set_add_subscription(wait_set, intra_process_subscription_handle_.get(), - &wait_set_intra_process_subscription_index_) != RCL_RET_OK) - { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't add intra process subscription to wait set: %s", rcl_get_error_string().str); - return false; - } - } - - return true; -} - -bool -SubscriptionBase::is_ready(rcl_wait_set_t * wait_set) -{ - subscription_ready_ = - (wait_set->subscriptions[wait_set_subscription_index_] == subscription_handle_.get()); - intra_process_subscription_ready_ = use_intra_process_ && - (wait_set->subscriptions[wait_set_intra_process_subscription_index_] == - intra_process_subscription_handle_.get()); - return subscription_ready_ || intra_process_subscription_ready_; -} - -void -SubscriptionBase::execute() -{ - if (subscription_ready_) { - rmw_message_info_t message_info; - message_info.from_intra_process = false; - - if (is_serialized()) { - auto serialized_msg = create_serialized_message(); - auto ret = rcl_take_serialized_message( - get_subscription_handle().get(), - serialized_msg.get(), &message_info); - if (RCL_RET_OK == ret) { - auto void_serialized_msg = std::static_pointer_cast(serialized_msg); - handle_message(void_serialized_msg, message_info); - } else if (RCL_RET_SUBSCRIPTION_TAKE_FAILED != ret) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "take_serialized failed for subscription on topic '%s': %s", - get_topic_name(), rcl_get_error_string().str); - rcl_reset_error(); - } - return_serialized_message(serialized_msg); - } else { - std::shared_ptr message = create_message(); - auto ret = rcl_take( - get_subscription_handle().get(), - message.get(), &message_info); - if (RCL_RET_OK == ret) { - handle_message(message, message_info); - } else if (RCL_RET_SUBSCRIPTION_TAKE_FAILED != ret) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "could not deserialize serialized message on topic '%s': %s", - get_topic_name(), rcl_get_error_string().str); - rcl_reset_error(); - } - return_message(message); - } - } - - if (intra_process_subscription_ready_) { - rcl_interfaces::msg::IntraProcessMessage ipm; - rmw_message_info_t message_info; - rcl_ret_t status = rcl_take( - get_intra_process_subscription_handle().get(), - &ipm, - &message_info); - - if (status == RCL_RET_OK) { - message_info.from_intra_process = true; - handle_intra_process_message(ipm, message_info); - } else if (status != RCL_RET_SUBSCRIPTION_TAKE_FAILED) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "take failed for intra process subscription on topic '%s': %s", - get_topic_name(), rcl_get_error_string().str); - rcl_reset_error(); - } - } -} - const rosidl_message_type_support_t & SubscriptionBase::get_message_type_support_handle() const { diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 10be2c5296..8ea80664da 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -207,8 +207,9 @@ void TimeSource::create_clock_sub() auto allocator = std::make_shared(); auto cb = std::bind(&TimeSource::clock_cb, this, std::placeholders::_1); - clock_subscription_ = rclcpp::create_subscription( + clock_subscription_ = rclcpp::create_subscription< + MessageT, decltype(cb), Alloc, MessageT, SubscriptionT + >( node_topics_.get(), topic_name, std::move(cb), diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index a5ed80c1e8..b46b3fd423 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -112,34 +112,3 @@ TimerBase::get_timer_handle() { return timer_handle_; } - -size_t -TimerBase::get_number_of_ready_timers() -{ - return 1; -} - -bool -TimerBase::add_to_wait_set(rcl_wait_set_t * wait_set) -{ - if (rcl_wait_set_add_timer(wait_set, timer_handle_.get(), &wait_set_timer_index_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't add timer to wait set: %s", rcl_get_error_string().str); - return false; - } - - return true; -} - -bool -TimerBase::is_ready(rcl_wait_set_t * wait_set) -{ - return wait_set->timers[wait_set_timer_index_] == timer_handle_.get(); -} - -void -TimerBase::execute() -{ - execute_callback(); -} diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index da4172ae8f..2e368dac8c 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -32,7 +32,7 @@ #include "rclcpp/client.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/context.hpp" -#include "rclcpp/graph_event.hpp" +#include "rclcpp/event.hpp" #include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/message_memory_strategy.hpp" @@ -314,16 +314,16 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, count_subscribers(const std::string & topic_name) const; /// Return a graph event, which will be set anytime a graph change occurs. - /* The graph GraphEvent object is a loan which must be returned. - * The GraphEvent object is scoped and therefore to return the load just let it go + /* The graph Event object is a loan which must be returned. + * The Event object is scoped and therefore to return the load just let it go * out of scope. */ RCLCPP_LIFECYCLE_PUBLIC - rclcpp::GraphEvent::SharedPtr + rclcpp::Event::SharedPtr get_graph_event(); - /// Wait for a graph event to occur by waiting on an GraphEvent to become set. - /* The given GraphEvent must be acquire through the get_graph_event() method. + /// Wait for a graph event to occur by waiting on an Event to become set. + /* The given Event must be acquire through the get_graph_event() method. * * \throws InvalidEventError if the given event is nullptr * \throws EventNotRegisteredError if the given event was not acquired with @@ -332,7 +332,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, RCLCPP_LIFECYCLE_PUBLIC void wait_for_graph_change( - rclcpp::GraphEvent::SharedPtr event, + rclcpp::Event::SharedPtr event, std::chrono::nanoseconds timeout); RCLCPP_LIFECYCLE_PUBLIC diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index 1ba5d79c63..4776b88c40 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -182,8 +182,9 @@ LifecycleNode::create_service( const rmw_qos_profile_t & qos_profile, rclcpp::callback_group::CallbackGroup::SharedPtr group) { - return rclcpp::create_service(node_base_, node_services_, service_name, - std::forward(callback), qos_profile, group); + return rclcpp::create_service( + node_base_, node_services_, + service_name, std::forward(callback), qos_profile, group); } template diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index ac508851cf..63d1dfbcba 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -224,7 +224,7 @@ LifecycleNode::get_callback_groups() const return node_base_->get_callback_groups(); } -rclcpp::GraphEvent::SharedPtr +rclcpp::Event::SharedPtr LifecycleNode::get_graph_event() { return node_graph_->get_graph_event(); @@ -232,7 +232,7 @@ LifecycleNode::get_graph_event() void LifecycleNode::wait_for_graph_change( - rclcpp::GraphEvent::SharedPtr event, + rclcpp::Event::SharedPtr event, std::chrono::nanoseconds timeout) { node_graph_->wait_for_graph_change(event, timeout); From 142b4d6f52d66decd68e41bda7e4b922fa89567f Mon Sep 17 00:00:00 2001 From: Miaofei Date: Mon, 25 Mar 2019 23:38:37 -0700 Subject: [PATCH 21/27] add publisher and subscription events to AllocatorMemoryStrategy --- .../rclcpp/strategies/allocator_memory_strategy.hpp | 12 +++++++++++- rclcpp/src/rclcpp/subscription.cpp | 6 ++++++ 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 6c76cad643..0f3d263d4c 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -164,6 +164,14 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy if (!group || !group->can_be_taken_from().load()) { continue; } + for (auto & weak_publisher : group->get_publisher_ptrs()) { + auto publisher = weak_publisher.lock(); + if (publisher) { + for (auto & publisher_event : publisher->get_event_handles()) { + waitable_handles_.push_back(publisher_event); + } + } + } for (auto & weak_subscription : group->get_subscription_ptrs()) { auto subscription = weak_subscription.lock(); if (subscription) { @@ -172,6 +180,9 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy subscription_handles_.push_back( subscription->get_intra_process_subscription_handle()); } + for (auto & subscription_event : subscription->get_event_handles()) { + waitable_handles_.push_back(subscription_event); + } } } for (auto & weak_service : group->get_service_ptrs()) { @@ -439,7 +450,6 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy return number_of_events; } - size_t number_of_ready_clients() const { size_t number_of_clients = client_handles_.size(); diff --git a/rclcpp/src/rclcpp/subscription.cpp b/rclcpp/src/rclcpp/subscription.cpp index ed26acaf17..89f6a3c9c5 100644 --- a/rclcpp/src/rclcpp/subscription.cpp +++ b/rclcpp/src/rclcpp/subscription.cpp @@ -121,6 +121,12 @@ SubscriptionBase::get_intra_process_subscription_handle() const return intra_process_subscription_handle_; } +const std::vector> & +SubscriptionBase::get_event_handles() const +{ + return event_handles_; +} + const rosidl_message_type_support_t & SubscriptionBase::get_message_type_support_handle() const { From 42b7cbe406aebd497b28af25f01ae6f12e165414 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Tue, 26 Mar 2019 12:53:27 -0700 Subject: [PATCH 22/27] Add stub API for assert_liveliness --- rclcpp/include/rclcpp/node.hpp | 12 ++++++++++++ rclcpp/include/rclcpp/publisher.hpp | 11 +++++++++++ 2 files changed, 23 insertions(+) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index bbc038e00b..3f0cde52e5 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -549,6 +549,18 @@ class Node : public std::enable_shared_from_this const NodeOptions & get_node_options() const; + + /// Manually assert that this Node is alive (for RMW_QOS_POLICY_MANUAL_BY_NODE) + /** + * If the rmw Liveliness policy is set to RMW_QOS_POLICY_MANUAL_BY_NODE, the creator of this + * node must manually call `assert_liveliness` on a regular basis to signal to the rest of the + * system that this Node is still alive. + * This function must be called at least as often as the qos_profile's liveliness_lease_duration + */ + RCLCPP_PUBLIC + void + assert_liveliness() {} + protected: /// Construct a sub-node, which will extend the namespace of all entities created with it. /** diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index b0b346ba43..09a477e301 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -153,6 +153,17 @@ class PublisherBase size_t get_intra_process_subscription_count() const; + /// Manually assert that this Publisher is alive (for RMW_QOS_POLICY_MANUAL_BY_TOPIC) + /** + * If the rmw Liveliness policy is set to RMW_QOS_POLICY_MANUAL_BY_TOPIC, the creator of this + * Publisher must manually call `assert_liveliness` on a regular basis to signal to the rest of + * the system that this Node is still alive. + * This function must be called at least as often as the qos_profile's liveliness_lease_duration + */ + RCLCPP_PUBLIC + void + assert_liveliness() {} + /// Compare this publisher to a gid. /** * Note that this function calls the next function. From b352d450319e84dd28aed78b800ab40938cec5c2 Mon Sep 17 00:00:00 2001 From: Vinnam Kim Date: Wed, 27 Mar 2019 08:24:20 +0900 Subject: [PATCH 23/27] Fix use_sim_time issue on LifeCycleNode (#651) Signed-off-by: vinnamkim --- .../include/rclcpp_lifecycle/lifecycle_node.hpp | 7 +++++++ rclcpp_lifecycle/src/lifecycle_node.cpp | 16 ++++++++++++++++ 2 files changed, 23 insertions(+) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 2e368dac8c..c93837ab9e 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -43,6 +43,7 @@ #include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/node_interfaces/node_parameters_interface.hpp" #include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/node_interfaces/node_time_source_interface.hpp" #include "rclcpp/node_interfaces/node_timers_interface.hpp" #include "rclcpp/node_interfaces/node_topics_interface.hpp" #include "rclcpp/node_interfaces/node_waitables_interface.hpp" @@ -383,6 +384,11 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, rclcpp::node_interfaces::NodeParametersInterface::SharedPtr get_node_parameters_interface(); + /// Return the Node's internal NodeParametersInterface implementation. + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr + get_node_time_source_interface(); + /// Return the Node's internal NodeWaitablesInterface implementation. RCLCPP_LIFECYCLE_PUBLIC rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr @@ -513,6 +519,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_; rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_; rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; + rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr node_time_source_; rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_; bool use_intra_process_comms_; diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 63d1dfbcba..b9a1665f4b 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -33,6 +33,7 @@ #include "rclcpp/node_interfaces/node_logging.hpp" #include "rclcpp/node_interfaces/node_parameters.hpp" #include "rclcpp/node_interfaces/node_services.hpp" +#include "rclcpp/node_interfaces/node_time_source.hpp" #include "rclcpp/node_interfaces/node_timers.hpp" #include "rclcpp/node_interfaces/node_topics.hpp" #include "rclcpp/node_interfaces/node_waitables.hpp" @@ -81,6 +82,15 @@ LifecycleNode::LifecycleNode( options.start_parameter_event_publisher(), options.parameter_event_qos_profile() )), + node_time_source_(new rclcpp::node_interfaces::NodeTimeSource( + node_base_, + node_topics_, + node_graph_, + node_services_, + node_logging_, + node_clock_, + node_parameters_ + )), node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())), use_intra_process_comms_(options.use_intra_process_comms()), impl_(new LifecycleNodeInterfaceImpl(node_base_, node_services_)) @@ -274,6 +284,12 @@ LifecycleNode::get_node_logging_interface() return node_logging_; } +rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr +LifecycleNode::get_node_time_source_interface() +{ + return node_time_source_; +} + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr LifecycleNode::get_node_timers_interface() { From ad2ba8d094b18a4af4c86af6fa2982d15b10a068 Mon Sep 17 00:00:00 2001 From: Miaofei Date: Wed, 27 Mar 2019 13:08:18 -0700 Subject: [PATCH 24/27] revert changes to client and services address PR comments Signed-off-by: Miaofei --- rclcpp/include/rclcpp/client.hpp | 21 ------ rclcpp/include/rclcpp/create_publisher.hpp | 4 +- rclcpp/include/rclcpp/create_subscription.hpp | 4 +- rclcpp/include/rclcpp/publisher.hpp | 44 ++++++------ rclcpp/include/rclcpp/publisher_options.hpp | 28 ++------ rclcpp/include/rclcpp/qos_event.hpp | 70 ++++--------------- rclcpp/include/rclcpp/service.hpp | 36 +--------- .../strategies/allocator_memory_strategy.hpp | 4 +- rclcpp/include/rclcpp/subscription.hpp | 44 ++++++------ .../include/rclcpp/subscription_options.hpp | 29 ++------ rclcpp/src/rclcpp/client.cpp | 25 ------- rclcpp/src/rclcpp/publisher.cpp | 6 +- rclcpp/src/rclcpp/qos_event.cpp | 6 +- rclcpp/src/rclcpp/service.cpp | 12 ---- rclcpp/src/rclcpp/subscription.cpp | 6 +- 15 files changed, 81 insertions(+), 258 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index fd2454f80e..46467400fe 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -74,14 +74,6 @@ class ClientBase std::shared_ptr get_client_handle() const; - RCLCPP_PUBLIC - std::shared_ptr - get_event_handle(); - - RCLCPP_PUBLIC - std::shared_ptr - get_event_handle() const; - RCLCPP_PUBLIC bool service_is_ready() const; @@ -121,13 +113,6 @@ class ClientBase std::shared_ptr context_; std::shared_ptr client_handle_; - std::shared_ptr event_handle_; - - size_t wait_set_client_index_; - size_t wait_set_event_index_; - - bool client_ready_; - bool event_ready_; }; template @@ -180,12 +165,6 @@ class Client : public ClientBase } rclcpp::exceptions::throw_from_rcl_error(ret, "could not create client"); } - - ret = rcl_client_event_init(get_event_handle().get(), get_client_handle().get(), - &client_options, RCL_CLIENT_EVENT_UNIMPLEMENTED); - if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error(ret, "could not create client event"); - } } virtual ~Client() diff --git a/rclcpp/include/rclcpp/create_publisher.hpp b/rclcpp/include/rclcpp/create_publisher.hpp index 836abf049d..e21b00687b 100644 --- a/rclcpp/include/rclcpp/create_publisher.hpp +++ b/rclcpp/include/rclcpp/create_publisher.hpp @@ -31,7 +31,7 @@ create_publisher( rclcpp::node_interfaces::NodeTopicsInterface * node_topics, const std::string & topic_name, const rmw_qos_profile_t & qos_profile, - const PublisherEventCallbacks & callbacks, + const PublisherEventCallbacks & event_callbacks, rclcpp::callback_group::CallbackGroup::SharedPtr group, bool use_intra_process_comms, std::shared_ptr allocator) @@ -41,7 +41,7 @@ create_publisher( auto pub = node_topics->create_publisher( topic_name, - rclcpp::create_publisher_factory(callbacks, allocator), + rclcpp::create_publisher_factory(event_callbacks, allocator), publisher_options, use_intra_process_comms); diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index f5a102d2a4..aa1f547039 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -38,7 +38,7 @@ create_subscription( const std::string & topic_name, CallbackT && callback, const rmw_qos_profile_t & qos_profile, - const SubscriptionEventCallbacks & callbacks, + const SubscriptionEventCallbacks & event_callbacks, rclcpp::callback_group::CallbackGroup::SharedPtr group, bool ignore_local_publications, bool use_intra_process_comms, @@ -54,7 +54,7 @@ create_subscription( auto factory = rclcpp::create_subscription_factory ( std::forward(callback), - callbacks, + event_callbacks, msg_mem_strat, allocator); diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 09a477e301..4a0a9be6af 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -121,25 +121,9 @@ class PublisherBase const rcl_publisher_t * get_publisher_handle() const; - // RCLCPP_PUBLIC - template - void - add_event_handle( - const EventCallbackT & callback, - const rcl_publisher_options_t & publisher_options, - const rcl_publisher_event_type_t event_type) - { - event_handles_.emplace_back(std::make_shared>( - callback, - rcl_publisher_event_init, - &publisher_handle_, - &publisher_options, - event_type)); - } - RCLCPP_PUBLIC - const std::vector> & - get_event_handles() const; + const std::vector> & + get_event_handlers() const; /// Get subscription count /** \return The number of subscriptions. */ @@ -198,12 +182,27 @@ class PublisherBase const rcl_publisher_options_t & intra_process_options); protected: + template + void + add_event_handler( + const EventCallbackT & callback, + const rcl_publisher_options_t & publisher_options, + const rcl_publisher_event_type_t event_type) + { + event_handlers_.emplace_back(std::make_shared>( + callback, + rcl_publisher_event_init, + &publisher_handle_, + &publisher_options, + event_type)); + } + std::shared_ptr rcl_node_handle_; rcl_publisher_t publisher_handle_ = rcl_get_zero_initialized_publisher(); rcl_publisher_t intra_process_publisher_handle_ = rcl_get_zero_initialized_publisher(); -std::vector> event_handles_; + std::vector> event_handlers_; using IntraProcessManagerWeakPtr = std::weak_ptr; @@ -244,16 +243,13 @@ class Publisher : public PublisherBase allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get()); if (event_callbacks.deadline_callback_) { - this->add_event_handle(event_callbacks.deadline_callback_, + this->add_event_handler(event_callbacks.deadline_callback_, publisher_options, RCL_PUBLISHER_DEADLINE); } if (event_callbacks.liveliness_callback_) { - this->add_event_handle(event_callbacks.liveliness_callback_, + this->add_event_handler(event_callbacks.liveliness_callback_, publisher_options, RCL_PUBLISHER_LIVELINESS); } - // if (event_callbacks.lifespan_callback_) { - // this->add_event_handle(event_callbacks.lifespan_callback_); - // } } virtual ~Publisher() diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp index 6c6ae24eda..1b87f6c4c2 100644 --- a/rclcpp/include/rclcpp/publisher_options.hpp +++ b/rclcpp/include/rclcpp/publisher_options.hpp @@ -28,9 +28,8 @@ namespace rclcpp struct PublisherEventCallbacks { - QOSDeadlineEventCallbackType deadline_callback_; - QOSLivelinessEventCallbackType liveliness_callback_; - QOSLifespanEventCallbackType lifespan_callback_; + QOSDeadlineOfferedCallbackType deadline_callback_; + QOSLivelinessLostCallbackType liveliness_callback_; }; template> @@ -119,7 +118,7 @@ class PublisherOptions RCLCPP_PUBLIC - const QOSDeadlineEventCallbackType & + const QOSDeadlineOfferedCallbackType & deadline_callback() const { return callbacks_.deadline_callback_; @@ -127,14 +126,14 @@ class PublisherOptions RCLCPP_PUBLIC PublisherOptions & - deadline_callback(const QOSDeadlineEventCallbackType & callback) + deadline_callback(const QOSDeadlineOfferedCallbackType & callback) { callbacks_.deadline_callback_ = callback; return *this; } RCLCPP_PUBLIC - const QOSLivelinessEventCallbackType & + const QOSLivelinessLostCallbackType & liveliness_callback() const { return callbacks_.liveliness_callback_; @@ -142,27 +141,12 @@ class PublisherOptions RCLCPP_PUBLIC PublisherOptions & - liveliness_callback(const QOSLivelinessEventCallbackType & callback) + liveliness_callback(const QOSLivelinessLostCallbackType & callback) { callbacks_.liveliness_callback_ = callback; return *this; } - RCLCPP_PUBLIC - const QOSLifespanEventCallbackType & - lifespan_callback() const - { - return callbacks_.lifespan_callback_; - } - - RCLCPP_PUBLIC - PublisherOptions & - lifespan_callback(const QOSLifespanEventCallbackType & callback) - { - callbacks_.lifespan_callback_ = callback; - return *this; - } - RCLCPP_PUBLIC const PublisherEventCallbacks & event_callbacks() const diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index 68479ed5ed..6682cecd35 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -29,62 +29,18 @@ namespace rclcpp { -struct QOSDeadlineEventInfo -{ - enum DeadlineEventType - { - DEADLINE_MISSED - }; - - DeadlineEventType event_type; - void * other_info; -}; - -struct QOSLivelinessEventInfo -{ - enum LivelinessEventType - { - LIVELINESS_CHANGED - }; - - LivelinessEventType event_type; - void * other_info; -}; - -struct QOSLifespanEventInfo -{ - enum LifespanEventType - { - LIFESPAN_EXPIRED - }; - - LifespanEventType event_type; - void * other_info; -}; - - -using QOSDeadlineEventCallbackType = std::function; -using QOSLivelinessEventCallbackType = std::function; -using QOSLifespanEventCallbackType = std::function; - - -// struct QOSDeadlineEventConfig { -// using CallbackT = QOSDeadlineEventCallbackType; -// using EventInfoT = QOSDeadlineEventInfo; -// }; - -// struct QOSLivelinessEventConfig { -// using CallbackT = QOSLivelinessEventCallbackType; -// using EventInfoT = QOSLivelinessEventInfo; -// }; +using QOSDeadlineRequestedInfo = rmw_requested_deadline_missed_status_t; +using QOSDeadlineOfferedInfo = rmw_offered_deadline_missed_t; +using QOSLivelinessChangedInfo = rmw_liveliness_changed_status_t; +using QOSLivelinessLostInfo = rmw_liveliness_lost_t; -// struct QOSLifespanEventConfig { -// using CallbackT = QOSLifespanEventCallbackType; -// using EventInfoT = QOSLifespanEventInfo; -// }; +using QOSDeadlineRequestedCallbackType = std::function; +using QOSDeadlineOfferedCallbackType = std::function; +using QOSLivelinessChangedCallbackType = std::function; +using QOSLivelinessLostCallbackType = std::function; -class QOSEventBase : public Waitable +class QOSEventHandlerBase : public Waitable { public: /// Get the number of ready events @@ -106,11 +62,11 @@ class QOSEventBase : public Waitable template -class QOSEvent : public QOSEventBase +class QOSEventHandler : public QOSEventHandlerBase { public: template - QOSEvent( + QOSEventHandler( const EventCallbackT & callback, InitFuncT init_func, HandleT handle, @@ -125,7 +81,7 @@ class QOSEvent : public QOSEventBase } } - ~QOSEvent() + ~QOSEventHandler() { if (rcl_event_fini(&event_handle_) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( @@ -141,7 +97,7 @@ class QOSEvent : public QOSEventBase { EventCallbackInfoT callback_info; - rcl_ret_t ret = rcl_take_event(&event_handle_, callback_info.other_info); + rcl_ret_t ret = rcl_take_event(&event_handle_, &callback_info); if (ret != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index c79ef707bd..9dc8d027b0 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -25,7 +25,6 @@ #include "rcl/service.h" #include "rclcpp/any_service_callback.hpp" -#include "rclcpp/event.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/type_support_decl.hpp" @@ -62,14 +61,6 @@ class ServiceBase std::shared_ptr get_service_handle() const; - RCLCPP_PUBLIC - std::shared_ptr - get_event_handle(); - - RCLCPP_PUBLIC - std::shared_ptr - get_event_handle() const; - virtual std::shared_ptr create_request() = 0; virtual std::shared_ptr create_request_header() = 0; virtual void handle_request( @@ -90,13 +81,7 @@ class ServiceBase std::shared_ptr node_handle_; std::shared_ptr service_handle_; - std::shared_ptr event_handle_; - - size_t wait_set_service_index_; - size_t wait_set_event_index_; - - bool service_ready_; - bool event_ready_; + bool owns_rcl_handle_ = true; }; template @@ -169,25 +154,6 @@ class Service : public ServiceBase rclcpp::exceptions::throw_from_rcl_error(ret, "could not create service"); } - - event_handle_ = std::shared_ptr(new rcl_event_t, - [](rcl_event_t * event) - { - if (rcl_event_fini(event) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Error in destruction of rcl event handle: %s", rcl_get_error_string().str); - rcl_reset_error(); - } - delete event; - }); - *event_handle_.get() = rcl_get_zero_initialized_event(); - - ret = rcl_service_event_init(get_event_handle().get(), get_service_handle().get(), - &service_options, RCL_SERVICE_EVENT_UNIMPLEMENTED); - if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error(ret, "could not create service event"); - } } Service( diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 0f3d263d4c..1e73d1ccbf 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -167,7 +167,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy for (auto & weak_publisher : group->get_publisher_ptrs()) { auto publisher = weak_publisher.lock(); if (publisher) { - for (auto & publisher_event : publisher->get_event_handles()) { + for (auto & publisher_event : publisher->get_event_handlers()) { waitable_handles_.push_back(publisher_event); } } @@ -180,7 +180,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy subscription_handles_.push_back( subscription->get_intra_process_subscription_handle()); } - for (auto & subscription_event : subscription->get_event_handles()) { + for (auto & subscription_event : subscription->get_event_handlers()) { waitable_handles_.push_back(subscription_event); } } diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 3890e29ab3..df4464326a 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -103,25 +103,9 @@ class SubscriptionBase virtual const std::shared_ptr get_intra_process_subscription_handle() const; - // RCLCPP_PUBLIC - template - void - add_event_handle( - const EventCallbackT & callback, - const rcl_subscription_options_t & subscription_options, - const rcl_subscription_event_type_t event_type) - { - event_handles_.emplace_back(std::make_shared>( - callback, - rcl_subscription_event_init, - get_subscription_handle().get(), - &subscription_options, - event_type)); - } - RCLCPP_PUBLIC - const std::vector> & - get_event_handles() const; + const std::vector> & + get_event_handlers() const; /// Borrow a new message. /** \return Shared pointer to the fresh message. */ @@ -169,6 +153,21 @@ class SubscriptionBase get_publisher_count() const; protected: + template + void + add_event_handler( + const EventCallbackT & callback, + const rcl_subscription_options_t & subscription_options, + const rcl_subscription_event_type_t event_type) + { + event_handlers_.emplace_back(std::make_shared>( + callback, + rcl_subscription_event_init, + get_subscription_handle().get(), + &subscription_options, + event_type)); + } + using IntraProcessManagerWeakPtr = std::weak_ptr; @@ -178,7 +177,7 @@ class SubscriptionBase size_t wait_set_subscription_index_; bool subscription_ready_; - std::vector> event_handles_; + std::vector> event_handlers_; bool use_intra_process_; std::shared_ptr intra_process_subscription_handle_; @@ -243,16 +242,13 @@ class Subscription : public SubscriptionBase matches_any_intra_process_publishers_(nullptr) { if (event_callbacks.deadline_callback_) { - this->add_event_handle(event_callbacks.deadline_callback_, + this->add_event_handler(event_callbacks.deadline_callback_, subscription_options, RCL_SUBSCRIPTION_DEADLINE); } if (event_callbacks.liveliness_callback_) { - this->add_event_handle(event_callbacks.liveliness_callback_, + this->add_event_handler(event_callbacks.liveliness_callback_, subscription_options, RCL_SUBSCRIPTION_LIVELINESS); } - // if (event_callbacks.lifespan_callback_) { - // this->add_event_handle(event_callbacks.lifespan_callback_); - // } } /// Support dynamically setting the message memory strategy. diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index a175ebd8db..6d6dc9fda4 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -28,9 +28,8 @@ namespace rclcpp struct SubscriptionEventCallbacks { - QOSDeadlineEventCallbackType deadline_callback_; - QOSLivelinessEventCallbackType liveliness_callback_; - QOSLifespanEventCallbackType lifespan_callback_; + QOSDeadlineRequestedCallbackType deadline_callback_; + QOSLivelinessChangedCallbackType liveliness_callback_; }; template> @@ -43,7 +42,6 @@ class SubscriptionOptions * * - deadline_callback = nullptr * - liveliness_callback = nullptr - * - lifespan_callback = nullptr * - subscription_qos_profile = rmw_qos_profile_default * - ignore_local_publications = false * - allocator = nullptr @@ -116,7 +114,7 @@ class SubscriptionOptions RCLCPP_PUBLIC - const QOSDeadlineEventCallbackType & + const QOSDeadlineRequestedCallbackType & deadline_callback() const { return callbacks_.deadline_callback_; @@ -124,14 +122,14 @@ class SubscriptionOptions RCLCPP_PUBLIC SubscriptionOptions & - deadline_callback(const QOSDeadlineEventCallbackType & callback) + deadline_callback(const QOSDeadlineRequestedCallbackType & callback) { callbacks_.deadline_callback_ = callback; return *this; } RCLCPP_PUBLIC - const QOSLivelinessEventCallbackType & + const QOSLivelinessChangedCallbackType & liveliness_callback() const { return callbacks_.liveliness_callback_; @@ -139,27 +137,12 @@ class SubscriptionOptions RCLCPP_PUBLIC SubscriptionOptions & - liveliness_callback(const QOSLivelinessEventCallbackType & callback) + liveliness_callback(const QOSLivelinessChangedCallbackType & callback) { callbacks_.liveliness_callback_ = callback; return *this; } - RCLCPP_PUBLIC - const QOSLifespanEventCallbackType & - lifespan_callback() const - { - return callbacks_.lifespan_callback_; - } - - RCLCPP_PUBLIC - SubscriptionOptions & - lifespan_callback(const QOSLifespanEventCallbackType & callback) - { - callbacks_.lifespan_callback_ = callback; - return *this; - } - RCLCPP_PUBLIC const SubscriptionEventCallbacks & diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index 6cbcbeb70f..c2f64e06f7 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -62,19 +62,6 @@ ClientBase::ClientBase( } delete client; }); - - event_handle_ = std::shared_ptr(new rcl_event_t, - [](rcl_event_t * event) - { - if (rcl_event_fini(event) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Error in destruction of rcl event handle: %s", rcl_get_error_string().str); - rcl_reset_error(); - } - delete event; - }); - *event_handle_.get() = rcl_get_zero_initialized_event(); } ClientBase::~ClientBase() @@ -101,18 +88,6 @@ ClientBase::get_client_handle() const return client_handle_; } -std::shared_ptr -ClientBase::get_event_handle() -{ - return event_handle_; -} - -std::shared_ptr -ClientBase::get_event_handle() const -{ - return event_handle_; -} - bool ClientBase::service_is_ready() const { diff --git a/rclcpp/src/rclcpp/publisher.cpp b/rclcpp/src/rclcpp/publisher.cpp index 8a57aee012..addbcf114b 100644 --- a/rclcpp/src/rclcpp/publisher.cpp +++ b/rclcpp/src/rclcpp/publisher.cpp @@ -155,10 +155,10 @@ PublisherBase::get_publisher_handle() const return &publisher_handle_; } -const std::vector> & -PublisherBase::get_event_handles() const +const std::vector> & +PublisherBase::get_event_handlers() const { - return event_handles_; + return event_handlers_; } size_t diff --git a/rclcpp/src/rclcpp/qos_event.cpp b/rclcpp/src/rclcpp/qos_event.cpp index 906be4035e..d3058f99f4 100644 --- a/rclcpp/src/rclcpp/qos_event.cpp +++ b/rclcpp/src/rclcpp/qos_event.cpp @@ -20,14 +20,14 @@ namespace rclcpp /// Get the number of ready events size_t -QOSEventBase::get_number_of_ready_events() +QOSEventHandlerBase::get_number_of_ready_events() { return 1; } /// Add the Waitable to a wait set. bool -QOSEventBase::add_to_wait_set(rcl_wait_set_t * wait_set) +QOSEventHandlerBase::add_to_wait_set(rcl_wait_set_t * wait_set) { if (rcl_wait_set_add_event(wait_set, &event_handle_, &wait_set_event_index_) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( @@ -41,7 +41,7 @@ QOSEventBase::add_to_wait_set(rcl_wait_set_t * wait_set) /// Check if the Waitable is ready. bool -QOSEventBase::is_ready(rcl_wait_set_t * wait_set) +QOSEventHandlerBase::is_ready(rcl_wait_set_t * wait_set) { return wait_set->events[wait_set_event_index_] == &event_handle_; } diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index 352f82a43b..c7f8ff9449 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -52,18 +52,6 @@ ServiceBase::get_service_handle() const return service_handle_; } -std::shared_ptr -ServiceBase::get_event_handle() -{ - return event_handle_; -} - -std::shared_ptr -ServiceBase::get_event_handle() const -{ - return event_handle_; -} - rcl_node_t * ServiceBase::get_rcl_node_handle() { diff --git a/rclcpp/src/rclcpp/subscription.cpp b/rclcpp/src/rclcpp/subscription.cpp index 89f6a3c9c5..438f2e2ed8 100644 --- a/rclcpp/src/rclcpp/subscription.cpp +++ b/rclcpp/src/rclcpp/subscription.cpp @@ -121,10 +121,10 @@ SubscriptionBase::get_intra_process_subscription_handle() const return intra_process_subscription_handle_; } -const std::vector> & -SubscriptionBase::get_event_handles() const +const std::vector> & +SubscriptionBase::get_event_handlers() const { - return event_handles_; + return event_handlers_; } const rosidl_message_type_support_t & From cb20529e5e218cde0106c59b12067a9ca9f43022 Mon Sep 17 00:00:00 2001 From: Vinnam Kim Date: Thu, 28 Mar 2019 13:05:42 +0900 Subject: [PATCH 25/27] Add parameter-related templates to LifecycleNode (#645) * Add parameter-related templates to LifecycleNode Signed-off-by: vinnamkim * Update rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp Co-Authored-By: vinnamkim * Update rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp --- .../rclcpp_lifecycle/lifecycle_node.hpp | 63 ++++++++++++++ .../rclcpp_lifecycle/lifecycle_node_impl.hpp | 87 +++++++++++++++++++ 2 files changed, 150 insertions(+) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index c93837ab9e..32459d3593 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -255,6 +255,26 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, rcl_interfaces::msg::SetParametersResult set_parameters_atomically(const std::vector & parameters); + template + void + set_parameter_if_not_set( + const std::string & name, + const ParameterT & value); + + /// Set a map of parameters with the same prefix. + /** + * For each key in the map, a parameter with a name of "name.key" will be set + * to the value in the map. + * + * \param[in] name The prefix of the parameters to set. + * \param[in] values The parameters to set in the given prefix. + */ + template + void + set_parameters_if_not_set( + const std::string & name, + const std::map & values); + RCLCPP_LIFECYCLE_PUBLIC std::vector get_parameters(const std::vector & names) const; @@ -273,6 +293,49 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, bool get_parameter(const std::string & name, ParameterT & parameter) const; + /// Assign the value of the map parameter if set into the values argument. + /** + * Parameter names that are part of a map are of the form "name.member". + * This API gets all parameters that begin with "name", storing them into the + * map with the name of the parameter and their value. + * If there are no members in the named map, then the "values" argument is not changed. + * + * \param[in] name The prefix of the parameters to get. + * \param[out] values The map of output values, with one std::string,MapValueT + * per parameter. + * \returns true if values was changed, false otherwise + */ + template + bool + get_parameters( + const std::string & name, + std::map & values) const; + + template + bool + get_parameter_or( + const std::string & name, + ParameterT & value, + const ParameterT & alternative_value) const; + + /// Get the parameter value; if not set, set the "alternative value" and store it in the node. + /** + * If the parameter is set, then the "value" argument is assigned the value + * in the parameter. + * If the parameter is not set, then the "value" argument is assigned the "alternative_value", + * and the parameter is set to the "alternative_value" on the node. + * + * \param[in] name The name of the parameter to get. + * \param[out] value The output where the value of the parameter should be assigned. + * \param[in] alternative_value Value to be stored in output and parameter if the parameter was not set. + */ + template + void + get_parameter_or_set( + const std::string & name, + ParameterT & value, + const ParameterT & alternative_value); + RCLCPP_LIFECYCLE_PUBLIC std::vector describe_parameters(const std::vector & names) const; diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index 69d8a48e96..71e638de7b 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_ #define RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_ +#include #include #include #include @@ -201,5 +202,91 @@ LifecycleNode::register_param_change_callback(CallbackT && callback) this->node_parameters_->register_param_change_callback(std::forward(callback)); } +template +void +LifecycleNode::set_parameter_if_not_set( + const std::string & name, + const ParameterT & value) +{ + rclcpp::Parameter parameter; + if (!this->get_parameter(name, parameter)) { + this->set_parameters({ + rclcpp::Parameter(name, value), + }); + } +} + +// this is a partially-specialized version of set_parameter_if_not_set above, +// where our concrete type for ParameterT is std::map, but the to-be-determined +// type is the value in the map. +template +void +LifecycleNode::set_parameters_if_not_set( + const std::string & name, + const std::map & values) +{ + std::vector params; + + for (const auto & val : values) { + std::string param_name = name + "." + val.first; + rclcpp::Parameter parameter; + if (!this->get_parameter(param_name, parameter)) { + params.push_back(rclcpp::Parameter(param_name, val.second)); + } + } + + this->set_parameters(params); +} + +// this is a partially-specialized version of get_parameter above, +// where our concrete type for ParameterT is std::map, but the to-be-determined +// type is the value in the map. +template +bool +LifecycleNode::get_parameters( + const std::string & name, + std::map & values) const +{ + std::map params; + bool result = node_parameters_->get_parameters_by_prefix(name, params); + if (result) { + for (const auto & param : params) { + values[param.first] = param.second.get_value(); + } + } + + return result; +} + +template +bool +LifecycleNode::get_parameter_or( + const std::string & name, + ParameterT & value, + const ParameterT & alternative_value) const +{ + bool got_parameter = get_parameter(name, value); + if (!got_parameter) { + value = alternative_value; + } + return got_parameter; +} + +template +void +LifecycleNode::get_parameter_or_set( + const std::string & name, + ParameterT & value, + const ParameterT & alternative_value) +{ + bool got_parameter = get_parameter(name, value); + if (!got_parameter) { + this->set_parameters({ + rclcpp::Parameter(name, alternative_value), + }); + value = alternative_value; + } +} + } // namespace rclcpp_lifecycle #endif // RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_ From 80bcf2ff784b5b387d6d0f32eaf0483145d3cdea Mon Sep 17 00:00:00 2001 From: Miaofei Date: Wed, 27 Mar 2019 18:26:22 -0700 Subject: [PATCH 26/27] update API calls into rcl --- rclcpp/include/rclcpp/publisher.hpp | 14 ++++++-------- rclcpp/include/rclcpp/qos_event.hpp | 13 ++++++------- rclcpp/include/rclcpp/subscription.hpp | 14 ++++++-------- 3 files changed, 18 insertions(+), 23 deletions(-) diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 4a0a9be6af..b02a3b7a03 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -186,15 +186,13 @@ class PublisherBase void add_event_handler( const EventCallbackT & callback, - const rcl_publisher_options_t & publisher_options, const rcl_publisher_event_type_t event_type) { event_handlers_.emplace_back(std::make_shared>( - callback, - rcl_publisher_event_init, - &publisher_handle_, - &publisher_options, - event_type)); + callback, + rcl_publisher_event_init, + &publisher_handle_, + event_type)); } std::shared_ptr rcl_node_handle_; @@ -244,11 +242,11 @@ class Publisher : public PublisherBase if (event_callbacks.deadline_callback_) { this->add_event_handler(event_callbacks.deadline_callback_, - publisher_options, RCL_PUBLISHER_DEADLINE); + RCL_PUBLISHER_OFFERED_DEADLINE_MISSED); } if (event_callbacks.liveliness_callback_) { this->add_event_handler(event_callbacks.liveliness_callback_, - publisher_options, RCL_PUBLISHER_LIVELINESS); + RCL_PUBLISHER_LIVELINESS_LOST); } } diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index 6682cecd35..08833a6a1d 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -30,9 +30,9 @@ namespace rclcpp { using QOSDeadlineRequestedInfo = rmw_requested_deadline_missed_status_t; -using QOSDeadlineOfferedInfo = rmw_offered_deadline_missed_t; +using QOSDeadlineOfferedInfo = rmw_offered_deadline_missed_status_t; using QOSLivelinessChangedInfo = rmw_liveliness_changed_status_t; -using QOSLivelinessLostInfo = rmw_liveliness_lost_t; +using QOSLivelinessLostInfo = rmw_liveliness_lost_status_t; using QOSDeadlineRequestedCallbackType = std::function; using QOSDeadlineOfferedCallbackType = std::function; @@ -65,17 +65,16 @@ template class QOSEventHandler : public QOSEventHandlerBase { public: - template + template QOSEventHandler( const EventCallbackT & callback, InitFuncT init_func, - HandleT handle, - OptionsT options, - EventTypeEnum type) + ParentHandleT parent_handle, + EventTypeEnum event_type) : event_callback_(callback) { event_handle_ = rcl_get_zero_initialized_event(); - rcl_ret_t ret = init_func(&event_handle_, handle, options, type); + rcl_ret_t ret = init_func(&event_handle_, parent_handle, event_type); if (ret != RCL_RET_OK) { rclcpp::exceptions::throw_from_rcl_error(ret, "could not create event"); } diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index df4464326a..6e12d85414 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -157,15 +157,13 @@ class SubscriptionBase void add_event_handler( const EventCallbackT & callback, - const rcl_subscription_options_t & subscription_options, const rcl_subscription_event_type_t event_type) { event_handlers_.emplace_back(std::make_shared>( - callback, - rcl_subscription_event_init, - get_subscription_handle().get(), - &subscription_options, - event_type)); + callback, + rcl_subscription_event_init, + get_subscription_handle().get(), + event_type)); } using IntraProcessManagerWeakPtr = @@ -243,11 +241,11 @@ class Subscription : public SubscriptionBase { if (event_callbacks.deadline_callback_) { this->add_event_handler(event_callbacks.deadline_callback_, - subscription_options, RCL_SUBSCRIPTION_DEADLINE); + RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED); } if (event_callbacks.liveliness_callback_) { this->add_event_handler(event_callbacks.liveliness_callback_, - subscription_options, RCL_SUBSCRIPTION_LIVELINESS); + RCL_SUBSCRIPTION_LIVELINESS_CHANGED); } } From 4f2f8def9819a3e37216254a21e3471655529bc0 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Thu, 28 Mar 2019 11:01:52 -0700 Subject: [PATCH 27/27] fix linter errors in rclcpp_lifecycle (#672) Signed-off-by: Karsten Knese --- .../include/rclcpp_lifecycle/lifecycle_node_impl.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index 71e638de7b..941f31495b 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include "rclcpp/contexts/default_context.hpp" #include "rclcpp/intra_process_manager.hpp"