diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 83f424dd52..77a1fca8e5 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -35,11 +35,17 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/intra_process_manager_impl.cpp src/rclcpp/memory_strategies.cpp src/rclcpp/memory_strategy.cpp + src/rclcpp/node.cpp + src/rclcpp/node_interfaces/node_base.cpp + src/rclcpp/node_interfaces/node_graph.cpp + src/rclcpp/node_interfaces/node_parameters.cpp + src/rclcpp/node_interfaces/node_services.cpp + src/rclcpp/node_interfaces/node_timers.cpp + src/rclcpp/node_interfaces/node_topics.cpp src/rclcpp/parameter.cpp src/rclcpp/parameter_client.cpp src/rclcpp/parameter_service.cpp src/rclcpp/publisher.cpp - src/rclcpp/node.cpp src/rclcpp/service.cpp src/rclcpp/subscription.cpp src/rclcpp/timer.cpp diff --git a/rclcpp/include/rclcpp/any_executable.hpp b/rclcpp/include/rclcpp/any_executable.hpp index 5c1e2ae627..a0ddc75f02 100644 --- a/rclcpp/include/rclcpp/any_executable.hpp +++ b/rclcpp/include/rclcpp/any_executable.hpp @@ -17,8 +17,13 @@ #include +#include "rclcpp/callback_group.hpp" +#include "rclcpp/client.hpp" #include "rclcpp/macros.hpp" -#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/service.hpp" +#include "rclcpp/subscription.hpp" +#include "rclcpp/timer.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -44,7 +49,7 @@ struct AnyExecutable rclcpp::client::ClientBase::SharedPtr client; // These are used to keep the scope on the containing items rclcpp::callback_group::CallbackGroup::SharedPtr callback_group; - rclcpp::node::Node::SharedPtr node; + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base; }; } // namespace executor diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index 15db7741ef..62a395aa2d 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -30,10 +30,12 @@ namespace rclcpp { // Forward declarations for friend statement in class CallbackGroup -namespace node +namespace node_interfaces { -class Node; -} // namespace node +class NodeServices; +class NodeTimers; +class NodeTopics; +} // namespace node_interfaces namespace callback_group { @@ -46,7 +48,9 @@ enum class CallbackGroupType class CallbackGroup { - friend class rclcpp::node::Node; + friend class rclcpp::node_interfaces::NodeServices; + friend class rclcpp::node_interfaces::NodeTimers; + friend class rclcpp::node_interfaces::NodeTopics; public: RCLCPP_SMART_PTR_DEFINITIONS(CallbackGroup) @@ -78,7 +82,7 @@ class CallbackGroup const CallbackGroupType & type() const; -private: +protected: RCLCPP_DISABLE_COPY(CallbackGroup) RCLCPP_PUBLIC diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 1dbfe9fd47..f6ea530c32 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -29,6 +29,7 @@ #include "rclcpp/function_traits.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_graph_interface.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" @@ -39,10 +40,10 @@ namespace rclcpp { -namespace node +namespace node_interfaces { -class Node; -} // namespace node +class NodeBaseInterface; +} // namespace node_interfaces namespace client { @@ -54,7 +55,8 @@ class ClientBase RCLCPP_PUBLIC ClientBase( - std::shared_ptr parent_node, + rclcpp::node_interfaces::NodeBaseInterface * node_base, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, const std::string & service_name); RCLCPP_PUBLIC @@ -98,7 +100,7 @@ class ClientBase rcl_node_t * get_rcl_node_handle() const; - std::weak_ptr node_; + rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_; std::shared_ptr node_handle_; rcl_client_t client_handle_ = rcl_get_zero_initialized_client(); @@ -127,10 +129,11 @@ class Client : public ClientBase RCLCPP_SMART_PTR_DEFINITIONS(Client) Client( - std::shared_ptr parent_node, + 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(parent_node, service_name) + : ClientBase(node_base, node_graph, service_name) { using rosidl_typesupport_cpp::get_service_type_support_handle; auto service_type_support_handle = diff --git a/rclcpp/include/rclcpp/create_publisher.hpp b/rclcpp/include/rclcpp/create_publisher.hpp new file mode 100644 index 0000000000..f136f33653 --- /dev/null +++ b/rclcpp/include/rclcpp/create_publisher.hpp @@ -0,0 +1,51 @@ +// 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__CREATE_PUBLISHER_HPP_ +#define RCLCPP__CREATE_PUBLISHER_HPP_ + +#include +#include + +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/publisher_factory.hpp" +#include "rmw/qos_profiles.h" + +namespace rclcpp +{ + +template +std::shared_ptr +create_publisher( + rclcpp::node_interfaces::NodeTopicsInterface * node_topics, + const std::string & topic_name, + const rmw_qos_profile_t & qos_profile, + bool use_intra_process_comms, + std::shared_ptr allocator) +{ + auto publisher_options = rcl_publisher_get_default_options(); + publisher_options.qos = qos_profile; + + auto pub = node_topics->create_publisher( + topic_name, + rclcpp::create_publisher_factory(allocator), + publisher_options, + use_intra_process_comms); + node_topics->add_publisher(pub); + return std::dynamic_pointer_cast(pub); +} + +} // namespace rclcpp + +#endif // RCLCPP__CREATE_PUBLISHER_HPP_ diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp new file mode 100644 index 0000000000..cab3baa647 --- /dev/null +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -0,0 +1,62 @@ +// 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__CREATE_SUBSCRIPTION_HPP_ +#define RCLCPP__CREATE_SUBSCRIPTION_HPP_ + +#include +#include +#include + +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/subscription_factory.hpp" +#include "rmw/qos_profiles.h" + +namespace rclcpp +{ + +template +typename rclcpp::subscription::Subscription::SharedPtr +create_subscription( + rclcpp::node_interfaces::NodeTopicsInterface * node_topics, + const std::string & topic_name, + CallbackT && callback, + const rmw_qos_profile_t & qos_profile, + 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 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), msg_mem_strat, allocator); + + auto sub = node_topics->create_subscription( + topic_name, + factory, + subscription_options, + use_intra_process_comms); + node_topics->add_subscription(sub, group); + return std::dynamic_pointer_cast(sub); +} + +} // namespace rclcpp + +#endif // RCLCPP__CREATE_SUBSCRIPTION_HPP_ diff --git a/rclcpp/include/rclcpp/exceptions.hpp b/rclcpp/include/rclcpp/exceptions.hpp index 4700029ae0..e775b49a06 100644 --- a/rclcpp/include/rclcpp/exceptions.hpp +++ b/rclcpp/include/rclcpp/exceptions.hpp @@ -95,6 +95,22 @@ 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. +class InvalidEventError : public std::runtime_error +{ +public: + InvalidEventError() + : std::runtime_error("event is invalid") {} +}; + +/// Thrown when an unregistered rclcpp::Event is encountered where a registered one was expected. +class EventNotRegisteredError : public std::runtime_error +{ +public: + EventNotRegisteredError() + : std::runtime_error("event already registered") {} +}; + } // namespace exceptions } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 697c3c16ba..2601086018 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -28,16 +28,21 @@ #include "rcl/guard_condition.h" #include "rcl/wait.h" -#include "rclcpp/any_executable.hpp" -#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/memory_strategies.hpp" #include "rclcpp/memory_strategy.hpp" -#include "rclcpp/node.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp { + +// Forward declaration is used in convenience method signature. +namespace node +{ +class Node; +} // namespace node + namespace executor { @@ -114,7 +119,12 @@ class Executor */ RCLCPP_PUBLIC virtual void - add_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify = true); + add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify = true); + + /// Convenience function which takes Node and forwards NodeBaseInterface. + RCLCPP_PUBLIC + virtual void + add_node(std::shared_ptr node_ptr, bool notify = true); /// Remove a node from the executor. /** @@ -125,7 +135,12 @@ class Executor */ RCLCPP_PUBLIC virtual void - remove_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify = true); + remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify = true); + + /// Convenience function which takes Node and forwards NodeBaseInterface. + RCLCPP_PUBLIC + virtual void + remove_node(std::shared_ptr node_ptr, bool notify = true); /// Add a node to executor, execute the next available unit of work, and remove the node. /** @@ -136,7 +151,7 @@ class Executor */ template void - spin_node_once(rclcpp::node::Node::SharedPtr node, + spin_node_once(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node, std::chrono::duration timeout = std::chrono::duration(-1)) { return spin_node_once_nanoseconds( @@ -145,13 +160,30 @@ class Executor ); } + /// Convenience function which takes Node and forwards NodeBaseInterface. + template + void + spin_node_once(std::shared_ptr node, + std::chrono::duration timeout = std::chrono::duration(-1)) + { + return spin_node_once_nanoseconds( + node->get_node_base_interface(), + std::chrono::duration_cast(timeout) + ); + } + /// Add a node, complete all immediately available work, and remove the node. /** * \param[in] node Shared pointer to the node to add. */ RCLCPP_PUBLIC void - spin_node_some(rclcpp::node::Node::SharedPtr node); + spin_node_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node); + + /// Convenience function which takes Node and forwards NodeBaseInterface. + RCLCPP_PUBLIC + void + spin_node_some(std::shared_ptr node); /// Complete all available queued work without blocking. /** @@ -247,7 +279,9 @@ class Executor protected: RCLCPP_PUBLIC void - spin_node_once_nanoseconds(rclcpp::node::Node::SharedPtr node, std::chrono::nanoseconds timeout); + spin_node_once_nanoseconds( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node, + std::chrono::nanoseconds timeout); /// Find the next available executable and do the work associated with it. /** \param[in] any_exec Union structure that can hold any executable type (timer, subscription, @@ -284,7 +318,7 @@ class Executor wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); RCLCPP_PUBLIC - rclcpp::node::Node::SharedPtr + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group); RCLCPP_PUBLIC @@ -318,7 +352,7 @@ class Executor private: RCLCPP_DISABLE_COPY(Executor) - std::vector> weak_nodes_; + std::vector weak_nodes_; }; } // namespace executor diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index fec281749b..487ae8e26b 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -16,6 +16,7 @@ #define RCLCPP__EXECUTORS_HPP_ #include +#include #include "rclcpp/executors/multi_threaded_executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" @@ -30,13 +31,21 @@ namespace rclcpp // \param[in] node_ptr Shared pointer to the node to spin. RCLCPP_PUBLIC void -spin_some(node::Node::SharedPtr node_ptr); +spin_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr); + +RCLCPP_PUBLIC +void +spin_some(rclcpp::node::Node::SharedPtr node_ptr); /// Create a default single-threaded executor and spin the specified node. // \param[in] node_ptr Shared pointer to the node to spin. RCLCPP_PUBLIC void -spin(node::Node::SharedPtr node_ptr); +spin(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr); + +RCLCPP_PUBLIC +void +spin(rclcpp::node::Node::SharedPtr node_ptr); namespace executors { @@ -57,7 +66,8 @@ using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor; template rclcpp::executor::FutureReturnCode spin_node_until_future_complete( - rclcpp::executor::Executor & executor, rclcpp::node::Node::SharedPtr node_ptr, + rclcpp::executor::Executor & executor, + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, std::shared_future & future, std::chrono::duration timeout = std::chrono::duration(-1)) { @@ -69,18 +79,44 @@ spin_node_until_future_complete( return retcode; } +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)) +{ + return rclcpp::executors::spin_node_until_future_complete( + executor, + node_ptr->get_node_base_interface(), + future, + timeout); +} + } // namespace executors template rclcpp::executor::FutureReturnCode spin_until_future_complete( - node::Node::SharedPtr node_ptr, std::shared_future & future, + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, + std::shared_future & future, 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 +rclcpp::executor::FutureReturnCode +spin_until_future_complete( + std::shared_ptr node_ptr, + std::shared_future & future, + std::chrono::duration timeout = std::chrono::duration(-1)) +{ + return rclcpp::spin_until_future_complete(node_ptr->get_node_base_interface(), future, timeout); +} + } // namespace rclcpp #endif // RCLCPP__EXECUTORS_HPP_ diff --git a/rclcpp/include/rclcpp/graph_listener.hpp b/rclcpp/include/rclcpp/graph_listener.hpp index c40c0fee2e..c95ffd151b 100644 --- a/rclcpp/include/rclcpp/graph_listener.hpp +++ b/rclcpp/include/rclcpp/graph_listener.hpp @@ -24,16 +24,12 @@ #include "rcl/guard_condition.h" #include "rcl/wait.h" #include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_graph_interface.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp { -namespace node -{ -class Node; -} // namespace node - namespace graph_listener { @@ -91,7 +87,7 @@ class GraphListener : public std::enable_shared_from_this RCLCPP_PUBLIC virtual void - add_node(rclcpp::node::Node * node); + add_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph); /// Return true if the given node is in the graph listener's list of nodes. /* Also return false if given nullptr. @@ -101,7 +97,7 @@ class GraphListener : public std::enable_shared_from_this RCLCPP_PUBLIC virtual bool - has_node(rclcpp::node::Node * node); + has_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph); /// Remove a node from the graph listener's list of nodes. /* @@ -112,7 +108,7 @@ class GraphListener : public std::enable_shared_from_this RCLCPP_PUBLIC virtual void - remove_node(rclcpp::node::Node * node); + remove_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph); /// Stop the listening thread. /* The thread cannot be restarted, and the class is defunct after calling. @@ -159,9 +155,9 @@ class GraphListener : public std::enable_shared_from_this std::atomic_bool is_shutdown_; mutable std::mutex shutdown_mutex_; - mutable std::mutex nodes_barrier_mutex_; - mutable std::mutex nodes_mutex_; - std::vector nodes_; + mutable std::mutex node_graph_interfaces_barrier_mutex_; + mutable std::mutex node_graph_interfaces_mutex_; + std::vector node_graph_interfaces_; rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition(); rcl_guard_condition_t * shutdown_guard_condition_; diff --git a/rclcpp/include/rclcpp/intra_process_manager_impl.hpp b/rclcpp/include/rclcpp/intra_process_manager_impl.hpp index f22d79d8b1..ab1aca3e56 100644 --- a/rclcpp/include/rclcpp/intra_process_manager_impl.hpp +++ b/rclcpp/include/rclcpp/intra_process_manager_impl.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -92,6 +93,8 @@ class IntraProcessManagerImpl : public IntraProcessManagerImplBase add_subscription(uint64_t id, subscription::SubscriptionBase::SharedPtr subscription) { subscriptions_[id] = subscription; + // subscription->get_topic_name() -> const char * can be used as the key, + // since subscriptions_ shares the ownership of subscription subscription_ids_by_topic_[subscription->get_topic_name()].insert(id); } @@ -253,8 +256,20 @@ class IntraProcessManagerImpl : public IntraProcessManagerImplBase using SubscriptionMap = std::unordered_map, std::equal_to, RebindAlloc>>; - using IDTopicMap = std::map, RebindAlloc>>; + + struct strcmp_wrapper : public std::binary_function + { + bool + operator()(const char * lhs, const char * rhs) const + { + return std::strcmp(lhs, rhs) < 0; + } + }; + using IDTopicMap = std::map< + const char *, + AllocSet, + strcmp_wrapper, + RebindAlloc>>; SubscriptionMap subscriptions_; diff --git a/rclcpp/include/rclcpp/macros.hpp b/rclcpp/include/rclcpp/macros.hpp index fb961764e4..f8d3d52bdc 100644 --- a/rclcpp/include/rclcpp/macros.hpp +++ b/rclcpp/include/rclcpp/macros.hpp @@ -50,7 +50,23 @@ RCLCPP_WEAK_PTR_DEFINITIONS(__VA_ARGS__) \ __RCLCPP_UNIQUE_PTR_ALIAS(__VA_ARGS__) -#define __RCLCPP_SHARED_PTR_ALIAS(...) using SharedPtr = std::shared_ptr<__VA_ARGS__>; +/* Defines aliases only for using the Class with smart pointers. + * + * Same as RCLCPP_SMART_PTR_DEFINITIONS expect it excludes the static + * method definitions which do not work on pure virtual classes and classes + * which are not CopyConstructable. + * + * Use in the public section of the class. + * Make sure to include in the header when using this. + */ +#define RCLCPP_SMART_PTR_ALIASES_ONLY(...) \ + __RCLCPP_SHARED_PTR_ALIAS(__VA_ARGS__) \ + __RCLCPP_WEAK_PTR_ALIAS(__VA_ARGS__) \ + __RCLCPP_MAKE_SHARED_DEFINITION(__VA_ARGS__) + +#define __RCLCPP_SHARED_PTR_ALIAS(...) \ + using SharedPtr = std::shared_ptr<__VA_ARGS__>; \ + using ConstSharedPtr = std::shared_ptr; #define __RCLCPP_MAKE_SHARED_DEFINITION(...) \ template \ @@ -65,7 +81,9 @@ __RCLCPP_SHARED_PTR_ALIAS(__VA_ARGS__) \ __RCLCPP_MAKE_SHARED_DEFINITION(__VA_ARGS__) -#define __RCLCPP_WEAK_PTR_ALIAS(...) using WeakPtr = std::weak_ptr<__VA_ARGS__>; +#define __RCLCPP_WEAK_PTR_ALIAS(...) \ + using WeakPtr = std::weak_ptr<__VA_ARGS__>; \ + using ConstWeakPtr = std::weak_ptr; /// Defines aliases and static functions for using the Class with weak_ptrs. #define RCLCPP_WEAK_PTR_DEFINITIONS(...) __RCLCPP_WEAK_PTR_ALIAS(__VA_ARGS__) diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp index cfb905bf78..cecc723059 100644 --- a/rclcpp/include/rclcpp/memory_strategy.hpp +++ b/rclcpp/include/rclcpp/memory_strategy.hpp @@ -23,7 +23,7 @@ #include "rclcpp/any_executable.hpp" #include "rclcpp/macros.hpp" -#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -41,7 +41,7 @@ class RCLCPP_PUBLIC MemoryStrategy { public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(MemoryStrategy) - using WeakNodeVector = std::vector>; + using WeakNodeVector = std::vector; virtual bool collect_entities(const WeakNodeVector & weak_nodes) = 0; @@ -88,7 +88,7 @@ class RCLCPP_PUBLIC MemoryStrategy static rclcpp::client::ClientBase::SharedPtr get_client_by_handle(const rcl_client_t * client_handle, const WeakNodeVector & weak_nodes); - static rclcpp::node::Node::SharedPtr + static rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group, const WeakNodeVector & weak_nodes); diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 2d578b24be..42f24eb91e 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -39,6 +39,12 @@ #include "rclcpp/event.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/message_memory_strategy.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_graph_interface.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/node_interfaces/node_timers_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/publisher.hpp" #include "rclcpp/service.hpp" @@ -46,36 +52,12 @@ #include "rclcpp/timer.hpp" #include "rclcpp/visibility_control.hpp" -namespace rcl -{ -struct rcl_node_t; -} // namespace rcl - namespace rclcpp { -namespace graph_listener -{ -class GraphListener; -} // namespace graph_listener - namespace node { -class InvalidEventError : public std::runtime_error -{ -public: - InvalidEventError() - : std::runtime_error("event is invalid") {} -}; - -class EventNotRegisteredError : public std::runtime_error -{ -public: - EventNotRegisteredError() - : std::runtime_error("event already registered") {} -}; - /// Node is the single point of entry for creating publishers and subscribers. class Node : public std::enable_shared_from_this { @@ -109,7 +91,7 @@ class Node : public std::enable_shared_from_this /// Get the name of the node. // \return The name of the node. RCLCPP_PUBLIC - const std::string & + const char * get_name() const; /// Create and return a callback group. @@ -117,6 +99,11 @@ class Node : public std::enable_shared_from_this rclcpp::callback_group::CallbackGroup::SharedPtr create_callback_group(rclcpp::callback_group::CallbackGroupType group_type); + /// Return the list of callback groups in the node. + RCLCPP_PUBLIC + const std::vector & + get_callback_groups() const; + /// Create and return a Publisher. /** * \param[in] topic_name The topic for this publisher to publish on. @@ -160,7 +147,11 @@ class Node : public std::enable_shared_from_this Windows build breaks when static member function passed as default argument to msg_mem_strat, nullptr is a workaround. */ - template> + template< + typename MessageT, + typename CallbackT, + typename Alloc = std::allocator, + typename SubscriptionT = rclcpp::subscription::Subscription> typename rclcpp::subscription::Subscription::SharedPtr create_subscription( const std::string & topic_name, @@ -186,7 +177,11 @@ class Node : public std::enable_shared_from_this Windows build breaks when static member function passed as default argument to msg_mem_strat, nullptr is a workaround. */ - template> + template< + typename MessageT, + typename CallbackT, + typename Alloc = std::allocator, + typename SubscriptionT = rclcpp::subscription::Subscription> typename rclcpp::subscription::Subscription::SharedPtr create_subscription( const std::string & topic_name, @@ -204,30 +199,13 @@ 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 - typename rclcpp::timer::WallTimer::SharedPtr + template + typename rclcpp::timer::WallTimer::SharedPtr create_wall_timer( - std::chrono::nanoseconds period, - CallbackType callback, + std::chrono::duration period, + CallbackT callback, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); - /// Create a timer. - /** - * \param[in] period Time interval between triggers of the callback. - * \param[in] callback User-defined callback function. - * \param[in] group Callback group to execute this timer's callback in. - */ - // TODO(wjwwood): reenable this once I figure out why the demo doesn't build with it. - // rclcpp::timer::WallTimer::SharedPtr - // create_wall_timer( - // std::chrono::duration period, - // rclcpp::timer::CallbackType callback, - // rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); - - using CallbackGroup = rclcpp::callback_group::CallbackGroup; - using CallbackGroupWeakPtr = std::weak_ptr; - using CallbackGroupWeakPtrList = std::list; - /* Create and return a Client. */ template typename rclcpp::client::Client::SharedPtr @@ -258,16 +236,18 @@ class Node : public std::enable_shared_from_this get_parameters(const std::vector & names) const; RCLCPP_PUBLIC - const rclcpp::parameter::ParameterVariant + rclcpp::parameter::ParameterVariant get_parameter(const std::string & name) const; RCLCPP_PUBLIC - bool get_parameter( + bool + get_parameter( const std::string & name, rclcpp::parameter::ParameterVariant & parameter) const; template - bool get_parameter(const std::string & name, ParameterT & parameter) const; + bool + get_parameter(const std::string & name, ParameterT & parameter) const; RCLCPP_PUBLIC std::vector @@ -281,6 +261,15 @@ class Node : public std::enable_shared_from_this rcl_interfaces::msg::ListParametersResult list_parameters(const std::vector & prefixes, uint64_t depth) const; + /// Register the callback for parameter changes + /** + * \param[in] User defined callback function, It is expected to atomically set parameters. + * \note Repeated invocations of this function will overwrite previous callbacks + */ + template + void + register_param_change_callback(CallbackT && callback); + RCLCPP_PUBLIC std::map get_topic_names_and_types() const; @@ -293,53 +282,6 @@ class Node : public std::enable_shared_from_this size_t count_subscribers(const std::string & topic_name) const; - RCLCPP_PUBLIC - const CallbackGroupWeakPtrList & - get_callback_groups() const; - - RCLCPP_PUBLIC - const rcl_guard_condition_t * - get_notify_guard_condition() const; - - RCLCPP_PUBLIC - const rcl_guard_condition_t * - get_graph_guard_condition() const; - - RCLCPP_PUBLIC - const rcl_node_t * - get_rcl_node_handle() const; - - /// Return the rcl_node_t node handle (non-const version). - RCLCPP_PUBLIC - rcl_node_t * - get_rcl_node_handle(); - - /// Return the rcl_node_t node handle in a std::shared_ptr. - /* This handle remains valid after the Node is destroyed. - * The actual rcl node is not finalized until it is out of scope everywhere. - */ - RCLCPP_PUBLIC - std::shared_ptr - get_shared_node_handle(); - - /// Notify threads waiting on graph changes. - /* 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(). - * - * This is typically only used by the rclcpp::graph_listener::GraphListener. - * - * \throws RCLBaseError (a child of that exception) when an rcl error occurs - */ - RCLCPP_PUBLIC - void - notify_graph_change(); - - /// Notify any and all blocking node actions that shutdown has occurred. - RCLCPP_PUBLIC - void - notify_shutdown(); - /// 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 @@ -362,27 +304,35 @@ class Node : public std::enable_shared_from_this rclcpp::event::Event::SharedPtr event, std::chrono::nanoseconds timeout); - /// Return the number of on loan graph events, see get_graph_event(). - /* This is typically only used by the rclcpp::graph_listener::GraphListener. - */ + /// Return the Node's internal NodeBaseInterface implementation. RCLCPP_PUBLIC - size_t - count_graph_users(); + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr + get_node_base_interface(); - /// Register the callback for parameter changes - /** - * \param[in] User defined callback function, It is expected to atomically set parameters. - * \note Repeated invocations of this function will overwrite previous callbacks - */ - template - void register_param_change_callback(CallbackT && callback); + /// Return the Node's internal NodeGraphInterface implementation. + RCLCPP_PUBLIC + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr + get_node_graph_interface(); - std::atomic_bool has_executor; + /// Return the Node's internal NodeTimersInterface implementation. + RCLCPP_PUBLIC + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr + get_node_timers_interface(); + /// Return the Node's internal NodeTopicsInterface implementation. RCLCPP_PUBLIC - void - add_service(rclcpp::service::ServiceBase::SharedPtr service, - rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr + get_node_topics_interface(); + + /// Return the Node's internal NodeServicesInterface implementation. + RCLCPP_PUBLIC + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr + get_node_services_interface(); + + /// Return the Node's internal NodeParametersInterface implementation. + RCLCPP_PUBLIC + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr + get_node_parameters_interface(); private: RCLCPP_DISABLE_COPY(Node) @@ -391,51 +341,14 @@ class Node : public std::enable_shared_from_this bool group_in_node(callback_group::CallbackGroup::SharedPtr group); - std::string name_; - - std::shared_ptr node_handle_; - - rclcpp::context::Context::SharedPtr context_; - - CallbackGroup::SharedPtr default_callback_group_; - CallbackGroupWeakPtrList callback_groups_; - - size_t number_of_subscriptions_; - size_t number_of_timers_; - size_t number_of_services_; - size_t number_of_clients_; + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_; + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_; + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_; + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; bool use_intra_process_comms_; - - mutable std::mutex mutex_; - - /// Guard condition for notifying the Executor of changes to this node. - mutable std::mutex notify_guard_condition_mutex_; - rcl_guard_condition_t notify_guard_condition_ = rcl_get_zero_initialized_guard_condition(); - bool notify_guard_condition_is_valid_; - - /// Graph Listener which waits on graph changes for the node and is shared across nodes. - std::shared_ptr graph_listener_; - /// Whether or not this node needs to be added to the graph listener. - std::atomic_bool should_add_to_graph_listener_; - - /// Mutex to guard the graph event related data structures. - std::mutex graph_mutex_; - /// 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_; - /// 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_; - - std::function & - )> parameters_callback_ = nullptr; - - std::map parameters_; - - publisher::Publisher::SharedPtr events_publisher_; }; } // namespace node diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index f80da7da32..b561b4bad4 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -38,6 +38,8 @@ #include "rclcpp/contexts/default_context.hpp" #include "rclcpp/intra_process_manager.hpp" #include "rclcpp/parameter.hpp" +#include "rclcpp/create_publisher.hpp" +#include "rclcpp/create_subscription.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" @@ -73,66 +75,15 @@ Node::create_publisher( if (!allocator) { allocator = std::make_shared(); } - - auto publisher_options = rcl_publisher_get_default_options(); - publisher_options.qos = qos_profile; - - auto message_alloc = - std::make_shared( - *allocator.get()); - publisher_options.allocator = allocator::get_rcl_allocator( - *message_alloc.get()); - - auto publisher = std::make_shared( - node_handle_, topic_name, publisher_options, message_alloc); - - if (use_intra_process_comms_) { - auto intra_process_manager = - context_->get_sub_context(); - uint64_t intra_process_publisher_id = - intra_process_manager->add_publisher(publisher); - rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = intra_process_manager; - // *INDENT-OFF* - auto shared_publish_callback = - [weak_ipm](uint64_t publisher_id, void * msg, const std::type_info & type_info) -> uint64_t - { - auto ipm = weak_ipm.lock(); - if (!ipm) { - // TODO(wjwwood): should this just return silently? Or maybe return with a logged warning? - throw std::runtime_error( - "intra process publish called after destruction of intra process manager"); - } - if (!msg) { - throw std::runtime_error("cannot publisher msg which is a null pointer"); - } - auto & message_type_info = typeid(MessageT); - if (message_type_info != type_info) { - throw std::runtime_error( - std::string("published type '") + type_info.name() + - "' is incompatible from the publisher type '" + message_type_info.name() + "'"); - } - MessageT * typed_message_ptr = static_cast(msg); - using MessageDeleter = typename publisher::Publisher::MessageDeleter; - std::unique_ptr unique_msg(typed_message_ptr); - uint64_t message_seq = - ipm->store_intra_process_message(publisher_id, unique_msg); - return message_seq; - }; - // *INDENT-ON* - publisher->setup_intra_process( - intra_process_publisher_id, - shared_publish_callback, - publisher_options); - } - if (rcl_trigger_guard_condition(¬ify_guard_condition_) != RCL_RET_OK) { - throw std::runtime_error( - std::string( - "Failed to notify waitset on publisher creation: ") + rmw_get_error_string()); - } - return publisher; + return rclcpp::create_publisher( + this->node_topics_.get(), + topic_name, + qos_profile, + use_intra_process_comms_, + allocator); } -template +template typename rclcpp::subscription::Subscription::SharedPtr Node::create_subscription( const std::string & topic_name, @@ -142,102 +93,30 @@ Node::create_subscription( bool ignore_local_publications, typename rclcpp::message_memory_strategy::MessageMemoryStrategy::SharedPtr msg_mem_strat, - typename std::shared_ptr allocator) + std::shared_ptr allocator) { if (!allocator) { allocator = std::make_shared(); } - rclcpp::subscription::AnySubscriptionCallback any_subscription_callback(allocator); - any_subscription_callback.set(std::forward(callback)); - if (!msg_mem_strat) { - msg_mem_strat = - rclcpp::message_memory_strategy::MessageMemoryStrategy::create_default(); + using rclcpp::message_memory_strategy::MessageMemoryStrategy; + msg_mem_strat = MessageMemoryStrategy::create_default(); } - auto message_alloc = - std::make_shared::MessageAlloc>(); - auto subscription_options = rcl_subscription_get_default_options(); - subscription_options.qos = qos_profile; - subscription_options.allocator = rclcpp::allocator::get_rcl_allocator( - *message_alloc.get()); - subscription_options.ignore_local_publications = ignore_local_publications; - - using rclcpp::subscription::Subscription; - using rclcpp::subscription::SubscriptionBase; - - auto sub = Subscription::make_shared( - node_handle_, + return rclcpp::create_subscription( + this->node_topics_.get(), topic_name, - subscription_options, - any_subscription_callback, - msg_mem_strat); - auto sub_base_ptr = std::dynamic_pointer_cast(sub); - // Setup intra process. - if (use_intra_process_comms_) { - auto intra_process_options = rcl_subscription_get_default_options(); - intra_process_options.allocator = rclcpp::allocator::get_rcl_allocator( - *message_alloc.get()); - intra_process_options.qos = qos_profile; - intra_process_options.ignore_local_publications = false; - - auto intra_process_manager = - context_->get_sub_context(); - rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = intra_process_manager; - uint64_t intra_process_subscription_id = - intra_process_manager->add_subscription(sub_base_ptr); - // *INDENT-OFF* - sub->setup_intra_process( - intra_process_subscription_id, - [weak_ipm]( - uint64_t publisher_id, - uint64_t message_sequence, - uint64_t subscription_id, - typename Subscription::MessageUniquePtr & message) - { - auto ipm = weak_ipm.lock(); - if (!ipm) { - // TODO(wjwwood): should this just return silently? Or maybe return with a logged warning? - throw std::runtime_error( - "intra process take called after destruction of intra process manager"); - } - ipm->take_intra_process_message( - publisher_id, message_sequence, subscription_id, message); - }, - [weak_ipm](const rmw_gid_t * sender_gid) -> bool { - auto ipm = weak_ipm.lock(); - if (!ipm) { - throw std::runtime_error( - "intra process publisher check called after destruction of intra process manager"); - } - return ipm->matches_any_publishers(sender_gid); - }, - intra_process_options - ); - // *INDENT-ON* - } - // Assign to a group. - if (group) { - if (!group_in_node(group)) { - // TODO(jacquelinekay): use custom exception - throw std::runtime_error("Cannot create subscription, group not in node."); - } - group->add_subscription(sub_base_ptr); - } else { - default_callback_group_->add_subscription(sub_base_ptr); - } - number_of_subscriptions_++; - if (rcl_trigger_guard_condition(¬ify_guard_condition_) != RCL_RET_OK) { - throw std::runtime_error( - std::string( - "Failed to notify waitset on subscription creation: ") + rmw_get_error_string()); - } - return sub; + std::forward(callback), + qos_profile, + group, + ignore_local_publications, + use_intra_process_comms_, + msg_mem_strat, + allocator); } -template +template typename rclcpp::subscription::Subscription::SharedPtr Node::create_subscription( const std::string & topic_name, @@ -251,7 +130,7 @@ Node::create_subscription( { rmw_qos_profile_t qos = rmw_qos_profile_default; qos.depth = qos_history_depth; - return this->create_subscription( + return this->create_subscription( topic_name, std::forward(callback), qos, @@ -261,30 +140,17 @@ Node::create_subscription( allocator); } -template -typename rclcpp::timer::WallTimer::SharedPtr +template +typename rclcpp::timer::WallTimer::SharedPtr Node::create_wall_timer( - std::chrono::nanoseconds period, - CallbackType callback, + std::chrono::duration period, + CallbackT callback, rclcpp::callback_group::CallbackGroup::SharedPtr group) { - auto timer = rclcpp::timer::WallTimer::make_shared( - period, std::move(callback)); - if (group) { - if (!group_in_node(group)) { - // TODO(jacquelinekay): use custom exception - throw std::runtime_error("Cannot create timer, group not in node."); - } - group->add_timer(timer); - } else { - default_callback_group_->add_timer(timer); - } - number_of_timers_++; - if (rcl_trigger_guard_condition(¬ify_guard_condition_) != RCL_RET_OK) { - throw std::runtime_error( - std::string( - "Failed to notify waitset on timer creation: ") + rmw_get_error_string()); - } + auto timer = rclcpp::timer::WallTimer::make_shared( + std::chrono::duration_cast(period), + std::move(callback)); + node_timers_->add_timer(timer, group); return timer; } @@ -302,27 +168,13 @@ Node::create_client( using rclcpp::client::ClientBase; auto cli = Client::make_shared( - shared_from_this(), + node_base_.get(), + node_graph_, service_name, options); auto cli_base_ptr = std::dynamic_pointer_cast(cli); - if (group) { - if (!group_in_node(group)) { - // TODO(esteve): use custom exception - throw std::runtime_error("Cannot create client, group not in node."); - } - group->add_client(cli_base_ptr); - } else { - default_callback_group_->add_client(cli_base_ptr); - } - number_of_clients_++; - - if (rcl_trigger_guard_condition(¬ify_guard_condition_) != RCL_RET_OK) { - throw std::runtime_error( - std::string( - "Failed to notify waitset on client creation: ") + rmw_get_error_string()); - } + node_services_->add_client(cli_base_ptr, group); return cli; } @@ -341,21 +193,23 @@ Node::create_service( service_options.qos = qos_profile; auto serv = service::Service::make_shared( - node_handle_, + node_base_->get_shared_rcl_node_handle(), service_name, any_service_callback, service_options); auto serv_base_ptr = std::dynamic_pointer_cast(serv); - add_service(serv_base_ptr, group); + node_services_->add_service(serv_base_ptr, group); return serv; } template -void Node::register_param_change_callback(CallbackT && callback) +void +Node::register_param_change_callback(CallbackT && callback) { - this->parameters_callback_ = callback; + this->node_parameters_->register_param_change_callback(std::forward(callback)); } template -bool Node::get_parameter(const std::string & name, ParameterT & parameter) const +bool +Node::get_parameter(const std::string & name, ParameterT & parameter) const { rclcpp::parameter::ParameterVariant parameter_variant(name, parameter); bool result = get_parameter(name, parameter_variant); diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp new file mode 100644 index 0000000000..d720e05576 --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp @@ -0,0 +1,131 @@ +// 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__NODE_INTERFACES__NODE_BASE_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_ + +#include +#include +#include + +#include "rclcpp/context.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Implementation of the NodeBase part of the Node API. +class NodeBase : public NodeBaseInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBaseInterface) + + RCLCPP_PUBLIC + NodeBase(const std::string & node_name, rclcpp::context::Context::SharedPtr context); + + RCLCPP_PUBLIC + virtual + ~NodeBase(); + + RCLCPP_PUBLIC + virtual + const char * + get_name() const; + + RCLCPP_PUBLIC + virtual + rclcpp::context::Context::SharedPtr + get_context(); + + RCLCPP_PUBLIC + virtual + rcl_node_t * + get_rcl_node_handle(); + + RCLCPP_PUBLIC + virtual + const rcl_node_t * + get_rcl_node_handle() const; + + RCLCPP_PUBLIC + virtual + std::shared_ptr + get_shared_rcl_node_handle(); + + RCLCPP_PUBLIC + virtual + std::shared_ptr + get_shared_rcl_node_handle() const; + + RCLCPP_PUBLIC + virtual + rclcpp::callback_group::CallbackGroup::SharedPtr + create_callback_group(rclcpp::callback_group::CallbackGroupType group_type); + + RCLCPP_PUBLIC + virtual + rclcpp::callback_group::CallbackGroup::SharedPtr + get_default_callback_group(); + + RCLCPP_PUBLIC + virtual + bool + callback_group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group); + + RCLCPP_PUBLIC + virtual + const std::vector & + get_callback_groups() const; + + RCLCPP_PUBLIC + virtual + std::atomic_bool & + get_associated_with_executor_atomic(); + + RCLCPP_PUBLIC + virtual + rcl_guard_condition_t * + get_notify_guard_condition(); + + RCLCPP_PUBLIC + virtual + std::unique_lock + acquire_notify_guard_condition_lock() const; + +private: + RCLCPP_DISABLE_COPY(NodeBase) + + rclcpp::context::Context::SharedPtr context_; + + std::shared_ptr node_handle_; + + rclcpp::callback_group::CallbackGroup::SharedPtr default_callback_group_; + std::vector callback_groups_; + + std::atomic_bool associated_with_executor_; + + /// Guard condition for notifying the Executor of changes to this node. + mutable std::recursive_mutex notify_guard_condition_mutex_; + rcl_guard_condition_t notify_guard_condition_ = rcl_get_zero_initialized_guard_condition(); + bool notify_guard_condition_is_valid_; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp new file mode 100644 index 0000000000..702fe46cb6 --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp @@ -0,0 +1,136 @@ +// 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__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_ + +#include +#include +#include +#include + +#include "rcl/node.h" + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/context.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Pure virtual interface class for the NodeBase part of the Node API. +class NodeBaseInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBaseInterface) + + /// Return the name of the node. + // \return The name of the node. + RCLCPP_PUBLIC + virtual + const char * + get_name() const = 0; + + /// Return the context of the node. + // \return SharedPtr to the node's context. + RCLCPP_PUBLIC + virtual + rclcpp::context::Context::SharedPtr + get_context() = 0; + + /// Return the rcl_node_t node handle (non-const version). + RCLCPP_PUBLIC + virtual + rcl_node_t * + get_rcl_node_handle() = 0; + + /// Return the rcl_node_t node handle (const version). + RCLCPP_PUBLIC + virtual + const rcl_node_t * + get_rcl_node_handle() const = 0; + + /// Return the rcl_node_t node handle in a std::shared_ptr. + /* This handle remains valid after the Node is destroyed. + * The actual rcl node is not finalized until it is out of scope everywhere. + */ + RCLCPP_PUBLIC + virtual + std::shared_ptr + get_shared_rcl_node_handle() = 0; + + /// Return the rcl_node_t node handle in a std::shared_ptr. + /* This handle remains valid after the Node is destroyed. + * The actual rcl node is not finalized until it is out of scope everywhere. + */ + RCLCPP_PUBLIC + virtual + std::shared_ptr + get_shared_rcl_node_handle() const = 0; + + /// Create and return a callback group. + RCLCPP_PUBLIC + virtual + rclcpp::callback_group::CallbackGroup::SharedPtr + create_callback_group(rclcpp::callback_group::CallbackGroupType group_type) = 0; + + /// Return the default callback group. + RCLCPP_PUBLIC + virtual + rclcpp::callback_group::CallbackGroup::SharedPtr + get_default_callback_group() = 0; + + /// Return true if the given callback group is associated with this node. + RCLCPP_PUBLIC + virtual + bool + callback_group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0; + + /// Return list of callback groups associated with this node. + RCLCPP_PUBLIC + virtual + const std::vector & + get_callback_groups() const = 0; + + /// Return the atomic bool which is used to ensure only one executor is used. + RCLCPP_PUBLIC + virtual + std::atomic_bool & + get_associated_with_executor_atomic() = 0; + + /// Return guard condition that should be notified when the internal node state changes. + /* For example, this should be notified when a publisher is added or removed. + * + * \return the rcl_guard_condition_t if it is valid, else nullptr + */ + RCLCPP_PUBLIC + virtual + rcl_guard_condition_t * + get_notify_guard_condition() = 0; + + /// Acquire and return a scoped lock that protects the notify guard condition. + /* This should be used when triggering the notify guard condition. */ + RCLCPP_PUBLIC + virtual + std::unique_lock + acquire_notify_guard_condition_lock() const = 0; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp new file mode 100644 index 0000000000..64370e843b --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp @@ -0,0 +1,129 @@ +// 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__NODE_INTERFACES__NODE_GRAPH_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_GRAPH_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "rcl/guard_condition.h" + +#include "rclcpp/event.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_graph_interface.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +namespace graph_listener +{ +class GraphListener; +} // namespace graph_listener + +namespace node_interfaces +{ + +/// Implementation the NodeGraph part of the Node API. +class NodeGraph : public NodeGraphInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeGraph) + + RCLCPP_PUBLIC + explicit NodeGraph(rclcpp::node_interfaces::NodeBaseInterface * node_base); + + RCLCPP_PUBLIC + virtual + ~NodeGraph(); + + RCLCPP_PUBLIC + virtual + std::map + get_topic_names_and_types() const; + + RCLCPP_PUBLIC + virtual + size_t + count_publishers(const std::string & topic_name) const; + + RCLCPP_PUBLIC + virtual + size_t + count_subscribers(const std::string & topic_name) const; + + RCLCPP_PUBLIC + virtual + const rcl_guard_condition_t * + get_graph_guard_condition() const; + + RCLCPP_PUBLIC + virtual + void + notify_graph_change(); + + RCLCPP_PUBLIC + virtual + void + notify_shutdown(); + + RCLCPP_PUBLIC + virtual + rclcpp::event::Event::SharedPtr + get_graph_event(); + + RCLCPP_PUBLIC + virtual + void + wait_for_graph_change( + rclcpp::event::Event::SharedPtr event, + std::chrono::nanoseconds timeout); + + RCLCPP_PUBLIC + virtual + size_t + count_graph_users(); + +private: + RCLCPP_DISABLE_COPY(NodeGraph) + + /// Handle to the NodeBaseInterface given in the constructor. + rclcpp::node_interfaces::NodeBaseInterface * node_base_; + + /// Graph Listener which waits on graph changes for the node and is shared across nodes. + std::shared_ptr graph_listener_; + /// Whether or not this node needs to be added to the graph listener. + std::atomic_bool should_add_to_graph_listener_; + + /// Mutex to guard the graph event related data structures. + mutable std::mutex graph_mutex_; + /// 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_; + /// 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_; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_GRAPH_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp new file mode 100644 index 0000000000..86054cf95c --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp @@ -0,0 +1,122 @@ +// 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__NODE_INTERFACES__NODE_GRAPH_INTERFACE_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_GRAPH_INTERFACE_HPP_ + +#include +#include +#include + +#include "rcl/guard_condition.h" + +#include "rclcpp/event.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Pure virtual interface class for the NodeGraph part of the Node API. +class NodeGraphInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeGraphInterface) + + /// Return a map of existing topic names (string) to topic types (string). + /* A topic is considered to exist when at least one publisher or subscriber + * exists for it, whether they be local or remote to this process. + */ + RCLCPP_PUBLIC + virtual + std::map + get_topic_names_and_types() const = 0; + + /// Return the number of publishers that are advertised on a given topic. + RCLCPP_PUBLIC + virtual + size_t + count_publishers(const std::string & topic_name) const = 0; + + /// Return the number of subscribers who have created a subscription for a given topic. + RCLCPP_PUBLIC + virtual + size_t + count_subscribers(const std::string & topic_name) const = 0; + + /// Return the rcl guard condition which is triggered when the ROS graph changes. + RCLCPP_PUBLIC + virtual + const rcl_guard_condition_t * + get_graph_guard_condition() const = 0; + + /// Notify threads waiting on graph changes. + /* 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(). + * + * This is typically only used by the rclcpp::graph_listener::GraphListener. + * + * \throws RCLBaseError (a child of that exception) when an rcl error occurs + */ + RCLCPP_PUBLIC + virtual + void + notify_graph_change() = 0; + + /// Notify any and all blocking node actions that shutdown has occurred. + RCLCPP_PUBLIC + virtual + void + notify_shutdown() = 0; + + /// 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 + * out of scope. + */ + RCLCPP_PUBLIC + virtual + rclcpp::event::Event::SharedPtr + get_graph_event() = 0; + + /// 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 + * get_graph_event(). + */ + RCLCPP_PUBLIC + virtual + void + wait_for_graph_change( + rclcpp::event::Event::SharedPtr event, + std::chrono::nanoseconds timeout) = 0; + + /// Return the number of on loan graph events, see get_graph_event(). + /* This is typically only used by the rclcpp::graph_listener::GraphListener. + */ + RCLCPP_PUBLIC + virtual + size_t + count_graph_users() = 0; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_GRAPH_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp new file mode 100644 index 0000000000..883187bfa8 --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp @@ -0,0 +1,122 @@ +// 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__NODE_INTERFACES__NODE_PARAMETERS_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_ + +#include +#include +#include + +#include "rcl_interfaces/msg/list_parameters_result.hpp" +#include "rcl_interfaces/msg/parameter_descriptor.hpp" +#include "rcl_interfaces/msg/parameter_event.hpp" +#include "rcl_interfaces/msg/set_parameters_result.hpp" + +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/parameter.hpp" +#include "rclcpp/publisher.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Implementation of the NodeParameters part of the Node API. +class NodeParameters : public NodeParametersInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeParameters) + + RCLCPP_PUBLIC + NodeParameters( + rclcpp::node_interfaces::NodeTopicsInterface * node_topics, + bool use_intra_process); + + RCLCPP_PUBLIC + virtual + ~NodeParameters(); + + RCLCPP_PUBLIC + virtual + std::vector + set_parameters( + const std::vector & parameters); + + RCLCPP_PUBLIC + virtual + rcl_interfaces::msg::SetParametersResult + set_parameters_atomically( + const std::vector & parameters); + + RCLCPP_PUBLIC + virtual + std::vector + get_parameters(const std::vector & names) const; + + RCLCPP_PUBLIC + virtual + rclcpp::parameter::ParameterVariant + get_parameter(const std::string & name) const; + + RCLCPP_PUBLIC + virtual + bool + get_parameter( + const std::string & name, + rclcpp::parameter::ParameterVariant & parameter) const; + + RCLCPP_PUBLIC + virtual + std::vector + describe_parameters(const std::vector & names) const; + + RCLCPP_PUBLIC + virtual + std::vector + get_parameter_types(const std::vector & names) const; + + RCLCPP_PUBLIC + virtual + rcl_interfaces::msg::ListParametersResult + list_parameters(const std::vector & prefixes, uint64_t depth) const; + + RCLCPP_PUBLIC + virtual + void + register_param_change_callback(ParametersCallbackFunction callback); + +private: + RCLCPP_DISABLE_COPY(NodeParameters) + + rclcpp::node_interfaces::NodeTopicsInterface * node_topics_; + + mutable std::mutex mutex_; + + ParametersCallbackFunction parameters_callback_ = nullptr; + + std::map parameters_; + + publisher::Publisher::SharedPtr events_publisher_; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp new file mode 100644 index 0000000000..b2e9b43d32 --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp @@ -0,0 +1,97 @@ +// 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__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_ + +#include +#include + +#include "rcl_interfaces/msg/list_parameters_result.hpp" +#include "rcl_interfaces/msg/parameter_descriptor.hpp" +#include "rcl_interfaces/msg/set_parameters_result.hpp" + +#include "rclcpp/macros.hpp" +#include "rclcpp/parameter.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Pure virtual interface class for the NodeParameters part of the Node API. +class NodeParametersInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeParametersInterface) + + RCLCPP_PUBLIC + virtual + std::vector + set_parameters( + const std::vector & parameters) = 0; + + RCLCPP_PUBLIC + virtual + rcl_interfaces::msg::SetParametersResult + set_parameters_atomically( + const std::vector & parameters) = 0; + + RCLCPP_PUBLIC + virtual + std::vector + get_parameters(const std::vector & names) const = 0; + + RCLCPP_PUBLIC + virtual + rclcpp::parameter::ParameterVariant + get_parameter(const std::string & name) const = 0; + + RCLCPP_PUBLIC + virtual + bool + get_parameter( + const std::string & name, + rclcpp::parameter::ParameterVariant & parameter) const = 0; + + RCLCPP_PUBLIC + virtual + std::vector + describe_parameters(const std::vector & names) const = 0; + + RCLCPP_PUBLIC + virtual + std::vector + get_parameter_types(const std::vector & names) const = 0; + + RCLCPP_PUBLIC + virtual + rcl_interfaces::msg::ListParametersResult + list_parameters(const std::vector & prefixes, uint64_t depth) const = 0; + + using ParametersCallbackFunction = + std::function &)>; + + RCLCPP_PUBLIC + virtual + void + register_param_change_callback(ParametersCallbackFunction callback) = 0; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_services.hpp b/rclcpp/include/rclcpp/node_interfaces/node_services.hpp new file mode 100644 index 0000000000..e792525400 --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_services.hpp @@ -0,0 +1,67 @@ +// 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__NODE_INTERFACES__NODE_SERVICES_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_SERVICES_HPP_ + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/client.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/service.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Implementation of the NodeServices part of the Node API. +class NodeServices : public NodeServicesInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeServices) + + RCLCPP_PUBLIC + explicit NodeServices(rclcpp::node_interfaces::NodeBaseInterface * node_base); + + RCLCPP_PUBLIC + virtual + ~NodeServices(); + + RCLCPP_PUBLIC + virtual + void + add_client( + rclcpp::client::ClientBase::SharedPtr client_base_ptr, + rclcpp::callback_group::CallbackGroup::SharedPtr group); + + RCLCPP_PUBLIC + virtual + void + add_service( + rclcpp::service::ServiceBase::SharedPtr service_base_ptr, + rclcpp::callback_group::CallbackGroup::SharedPtr group); + +private: + RCLCPP_DISABLE_COPY(NodeServices) + + rclcpp::node_interfaces::NodeBaseInterface * node_base_; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_SERVICES_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp new file mode 100644 index 0000000000..da32bb1727 --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp @@ -0,0 +1,53 @@ +// 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__NODE_INTERFACES__NODE_SERVICES_INTERFACE_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_SERVICES_INTERFACE_HPP_ + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/client.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/service.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Pure virtual interface class for the NodeServices part of the Node API. +class NodeServicesInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeServicesInterface) + + RCLCPP_PUBLIC + virtual + void + add_client( + rclcpp::client::ClientBase::SharedPtr client_base_ptr, + rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0; + + RCLCPP_PUBLIC + virtual + void + add_service( + rclcpp::service::ServiceBase::SharedPtr service_base_ptr, + rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_SERVICES_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_timers.hpp b/rclcpp/include/rclcpp/node_interfaces/node_timers.hpp new file mode 100644 index 0000000000..a93774b24a --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_timers.hpp @@ -0,0 +1,60 @@ +// 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__NODE_INTERFACES__NODE_TIMERS_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_TIMERS_HPP_ + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_timers_interface.hpp" +#include "rclcpp/timer.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Implementation of the NodeTimers part of the Node API. +class NodeTimers : public NodeTimersInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTimers) + + RCLCPP_PUBLIC + explicit NodeTimers(rclcpp::node_interfaces::NodeBaseInterface * node_base); + + RCLCPP_PUBLIC + virtual + ~NodeTimers(); + + /// Add a timer to the node. + RCLCPP_PUBLIC + virtual + void + add_timer( + rclcpp::timer::TimerBase::SharedPtr timer, + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group); + +private: + RCLCPP_DISABLE_COPY(NodeTimers) + + rclcpp::node_interfaces::NodeBaseInterface * node_base_; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_TIMERS_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp new file mode 100644 index 0000000000..b101186ae5 --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp @@ -0,0 +1,46 @@ +// 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__NODE_INTERFACES__NODE_TIMERS_INTERFACE_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_TIMERS_INTERFACE_HPP_ + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/timer.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Pure virtual interface class for the NodeTimers part of the Node API. +class NodeTimersInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTimersInterface) + + /// Add a timer to the node. + RCLCPP_PUBLIC + virtual + void + add_timer( + rclcpp::timer::TimerBase::SharedPtr timer, + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) = 0; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_TIMERS_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp b/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp new file mode 100644 index 0000000000..9341738ef9 --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp @@ -0,0 +1,88 @@ +// 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__NODE_INTERFACES__NODE_TOPICS_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_TOPICS_HPP_ + +#include + +#include "rcl/publisher.h" +#include "rcl/subscription.h" + +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/publisher.hpp" +#include "rclcpp/publisher_factory.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Implementation of the NodeTopics part of the Node API. +class NodeTopics : public NodeTopicsInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTopicsInterface) + + RCLCPP_PUBLIC + explicit NodeTopics(rclcpp::node_interfaces::NodeBaseInterface * node_base); + + RCLCPP_PUBLIC + virtual + ~NodeTopics(); + + RCLCPP_PUBLIC + virtual + rclcpp::publisher::PublisherBase::SharedPtr + create_publisher( + const std::string & topic_name, + const rclcpp::PublisherFactory & publisher_factory, + rcl_publisher_options_t & publisher_options, + bool use_intra_process); + + RCLCPP_PUBLIC + virtual + void + add_publisher( + rclcpp::publisher::PublisherBase::SharedPtr publisher); + + RCLCPP_PUBLIC + virtual + rclcpp::subscription::SubscriptionBase::SharedPtr + create_subscription( + const std::string & topic_name, + const rclcpp::SubscriptionFactory & subscription_factory, + rcl_subscription_options_t & subscription_options, + bool use_intra_process); + + RCLCPP_PUBLIC + virtual + void + add_subscription( + rclcpp::subscription::SubscriptionBase::SharedPtr subscription, + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group); + +private: + RCLCPP_DISABLE_COPY(NodeTopics) + + NodeBaseInterface * node_base_; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_TOPICS_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp new file mode 100644 index 0000000000..b515b2fdc2 --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp @@ -0,0 +1,78 @@ +// 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__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_ + +#include +#include +#include + +#include "rcl/publisher.h" +#include "rcl/subscription.h" + +#include "rclcpp/macros.hpp" +#include "rclcpp/publisher.hpp" +#include "rclcpp/publisher_factory.hpp" +#include "rclcpp/subscription.hpp" +#include "rclcpp/subscription_factory.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Pure virtual interface class for the NodeTopics part of the Node API. +class NodeTopicsInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTopicsInterface) + + RCLCPP_PUBLIC + virtual + rclcpp::publisher::PublisherBase::SharedPtr + create_publisher( + const std::string & topic_name, + const rclcpp::PublisherFactory & publisher_factory, + rcl_publisher_options_t & publisher_options, + bool use_intra_process) = 0; + + RCLCPP_PUBLIC + virtual + void + add_publisher( + rclcpp::publisher::PublisherBase::SharedPtr publisher) = 0; + + RCLCPP_PUBLIC + virtual + rclcpp::subscription::SubscriptionBase::SharedPtr + create_subscription( + const std::string & topic_name, + const rclcpp::SubscriptionFactory & subscription_factory, + rcl_subscription_options_t & subscription_options, + bool use_intra_process) = 0; + + RCLCPP_PUBLIC + virtual + void + add_subscription( + rclcpp::subscription::SubscriptionBase::SharedPtr subscription, + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) = 0; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 78b5f21e09..7b4336414c 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -28,43 +28,48 @@ #include "rcl/publisher.h" #include "rcl_interfaces/msg/intra_process_message.hpp" -#include "rmw/impl/cpp/demangle.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 { -// Forward declaration for friend statement -namespace node +// Forward declaration is used for friend statement. +namespace node_interfaces { -class Node; -} // namespace node +class NodeTopicsInterface; +} namespace publisher { class PublisherBase { + friend rclcpp::node_interfaces::NodeTopicsInterface; + public: RCLCPP_SMART_PTR_DEFINITIONS(PublisherBase) + /// Default constructor. /** * Typically, a publisher is not created through this method, but instead is created through a * call to `Node::create_publisher`. - * \param[in] node_handle The corresponding rcl representation of the owner node. + * \param[in] node_base A pointer to the NodeBaseInterface for the parent node. * \param[in] topic The topic that this publisher publishes on. - * \param[in] queue_size The maximum number of unpublished messages to queue. + * \param[in] type_support The type support structure for the type to be published. + * \param[in] publisher_options QoS settings for this publisher. */ RCLCPP_PUBLIC PublisherBase( - std::shared_ptr node_handle, - std::string topic, - size_t queue_size); + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const std::string & topic, + const rosidl_message_type_support_t & type_support, + const rcl_publisher_options_t & publisher_options); RCLCPP_PUBLIC virtual ~PublisherBase(); @@ -72,7 +77,7 @@ class PublisherBase /// Get the topic that this publisher publishes on. // \return The topic name. RCLCPP_PUBLIC - const std::string & + const char * get_topic_name() const; /// Get the queue size for this publisher. @@ -113,9 +118,9 @@ class PublisherBase bool operator==(const rmw_gid_t * gid) const; - typedef std::function StoreMessageCallbackT; + using StoreMessageCallbackT = std::function; -protected: + /// Implementation utility function used to setup intra process publishing after creation. RCLCPP_PUBLIC void setup_intra_process( @@ -123,14 +128,11 @@ class PublisherBase StoreMessageCallbackT callback, const rcl_publisher_options_t & intra_process_options); - std::shared_ptr node_handle_; +protected: + 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(); - rcl_allocator_t rcl_allocator_ = rcl_get_default_allocator(); - - std::string topic_; - size_t queue_size_; uint64_t intra_process_publisher_id_; StoreMessageCallbackT store_intra_process_message_; @@ -143,8 +145,6 @@ class PublisherBase template> class Publisher : public PublisherBase { - friend rclcpp::node::Node; - public: using MessageAllocTraits = allocator::AllocRebind; using MessageAlloc = typename MessageAllocTraits::allocator_type; @@ -154,55 +154,22 @@ class Publisher : public PublisherBase RCLCPP_SMART_PTR_DEFINITIONS(Publisher) Publisher( - std::shared_ptr node_handle, - std::string topic, + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const std::string & topic, const rcl_publisher_options_t & publisher_options, - std::shared_ptr allocator) - : PublisherBase(node_handle, topic, publisher_options.qos.depth), message_allocator_(allocator) + const std::shared_ptr & allocator) + : PublisherBase( + node_base, + topic, + *rosidl_typesupport_cpp::get_message_type_support_handle(), + publisher_options), + message_allocator_(allocator) { - using rosidl_typesupport_cpp::get_message_type_support_handle; allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get()); - - rcl_allocator_ = publisher_options.allocator; - auto type_support_handle = get_message_type_support_handle(); - if (rcl_publisher_init( - &publisher_handle_, node_handle_.get(), type_support_handle, - topic.c_str(), &publisher_options) != RCL_RET_OK) - { - throw std::runtime_error( - std::string("could not create publisher: ") + - rcl_get_error_string_safe()); - } - // 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) { - throw std::runtime_error( - std::string("failed to get rmw handle: ") + rcl_get_error_string_safe()); - } - if (rmw_get_gid_for_publisher(publisher_rmw_handle, &rmw_gid_) != RMW_RET_OK) { - // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) - throw std::runtime_error( - std::string("failed to get publisher gid: ") + rmw_get_error_string_safe()); - // *INDENT-ON* - } } virtual ~Publisher() - { - if (rcl_publisher_fini(&intra_process_publisher_handle_, node_handle_.get()) != RCL_RET_OK) { - fprintf( - stderr, - "Error in destruction of intra process rcl publisher handle: %s\n", - rcl_get_error_string_safe()); - } - - if (rcl_publisher_fini(&publisher_handle_, node_handle_.get()) != RCL_RET_OK) { - fprintf( - stderr, - "Error in destruction of rcl publisher handle: %s\n", - rcl_get_error_string_safe()); - } - } + {} /// Send a message to the topic for this publisher. /** @@ -318,6 +285,7 @@ class Publisher : public PublisherBase }; } // namespace publisher + } // namespace rclcpp #endif // RCLCPP__PUBLISHER_HPP_ diff --git a/rclcpp/include/rclcpp/publisher_factory.hpp b/rclcpp/include/rclcpp/publisher_factory.hpp new file mode 100644 index 0000000000..bcc0c67491 --- /dev/null +++ b/rclcpp/include/rclcpp/publisher_factory.hpp @@ -0,0 +1,147 @@ +// 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__PUBLISHER_FACTORY_HPP_ +#define RCLCPP__PUBLISHER_FACTORY_HPP_ + +#include +#include +#include + +#include "rcl/publisher.h" + +#include "rosidl_typesupport_cpp/message_type_support.hpp" + +#include "rclcpp/publisher.hpp" +#include "rclcpp/intra_process_manager.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// Factory with functions used to create a MessageT specific PublisherT. +/* This factory class is used to encapsulate the template generated functions + * which are used during the creation of a Message type specific publisher + * within a non-templated class. + * + * It is created using the create_publisher_factory function, which is usually + * called from a templated "create_publisher" method on the Node class, and + * is passed to the non-templated "create_publisher" method on the NodeTopics + * class where it is used to create and setup the Publisher. + */ +struct PublisherFactory +{ + // Creates a PublisherT publisher object and returns it as a PublisherBase. + using PublisherFactoryFunction = std::function< + rclcpp::publisher::PublisherBase::SharedPtr( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const std::string & topic_name, + rcl_publisher_options_t & publisher_options)>; + + PublisherFactoryFunction create_typed_publisher; + + // Adds the PublisherBase to the intraprocess manager with the correctly + // templated call to IntraProcessManager::store_intra_process_message. + using AddPublisherToIntraProcessManagerFunction = std::function< + uint64_t( + rclcpp::intra_process_manager::IntraProcessManager * ipm, + rclcpp::publisher::PublisherBase::SharedPtr publisher)>; + + AddPublisherToIntraProcessManagerFunction add_publisher_to_intra_process_manager; + + // Creates the callback function which is called on each + // PublisherT::publish() and which handles the intra process transmission of + // the message being published. + using SharedPublishCallbackFactoryFunction = std::function< + rclcpp::publisher::PublisherBase::StoreMessageCallbackT( + rclcpp::intra_process_manager::IntraProcessManager::SharedPtr ipm)>; + + SharedPublishCallbackFactoryFunction create_shared_publish_callback; +}; + +/// Return a PublisherFactory with functions setup for creating a PublisherT. +template +PublisherFactory +create_publisher_factory(std::shared_ptr allocator) +{ + PublisherFactory factory; + + // factory function that creates a MessageT specific PublisherT + factory.create_typed_publisher = + [allocator]( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const std::string & topic_name, + rcl_publisher_options_t & publisher_options) -> std::shared_ptr + { + 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); + }; + + // function to add a publisher to the intra process manager + factory.add_publisher_to_intra_process_manager = + []( + rclcpp::intra_process_manager::IntraProcessManager * ipm, + rclcpp::publisher::PublisherBase::SharedPtr publisher) -> uint64_t + { + return ipm->add_publisher(std::dynamic_pointer_cast(publisher)); + }; + + // function to create a shared publish callback std::function + using StoreMessageCallbackT = rclcpp::publisher::PublisherBase::StoreMessageCallbackT; + factory.create_shared_publish_callback = + [](rclcpp::intra_process_manager::IntraProcessManager::SharedPtr ipm) -> StoreMessageCallbackT + { + rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = ipm; + + // this function is called on each call to publish() and handles storing + // of the published message in the intra process manager + auto shared_publish_callback = + [weak_ipm](uint64_t publisher_id, void * msg, const std::type_info & type_info) -> uint64_t + { + auto ipm = weak_ipm.lock(); + if (!ipm) { + // TODO(wjwwood): should this just return silently? Or maybe return with a warning? + throw std::runtime_error( + "intra process publish called after destruction of intra process manager"); + } + if (!msg) { + throw std::runtime_error("cannot publisher msg which is a null pointer"); + } + auto & message_type_info = typeid(MessageT); + if (message_type_info != type_info) { + throw std::runtime_error( + std::string("published type '") + type_info.name() + + "' is incompatible from the publisher type '" + message_type_info.name() + "'"); + } + MessageT * typed_message_ptr = static_cast(msg); + using MessageDeleter = typename publisher::Publisher::MessageDeleter; + std::unique_ptr unique_msg(typed_message_ptr); + uint64_t message_seq = + ipm->store_intra_process_message(publisher_id, unique_msg); + return message_seq; + }; + + return shared_publish_callback; + }; + + // return the factory now that it is populated + return factory; +} + +} // namespace rclcpp + +#endif // RCLCPP__PUBLISHER_FACTORY_HPP_ diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 465fa29b60..1cac9a5905 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -52,8 +52,6 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy using VoidAllocTraits = typename allocator::AllocRebind; using VoidAlloc = typename VoidAllocTraits::allocator_type; - using WeakNodeVector = std::vector>; - explicit AllocatorMemoryStrategy(std::shared_ptr allocator) { executable_allocator_ = std::make_shared(*allocator.get()); @@ -266,7 +264,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy any_exec->subscription = subscription; } any_exec->callback_group = group; - any_exec->node = get_node_by_group(group, weak_nodes); + any_exec->node_base = get_node_by_group(group, weak_nodes); subscription_handles_.erase(it); return; } @@ -300,7 +298,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy // Otherwise it is safe to set and return the any_exec any_exec->service = service; any_exec->callback_group = group; - any_exec->node = get_node_by_group(group, weak_nodes); + any_exec->node_base = get_node_by_group(group, weak_nodes); service_handles_.erase(it); return; } @@ -333,7 +331,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy // Otherwise it is safe to set and return the any_exec any_exec->client = client; any_exec->callback_group = group; - any_exec->node = get_node_by_group(group, weak_nodes); + any_exec->node_base = get_node_by_group(group, weak_nodes); client_handles_.erase(it); return; } diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 10a75f2417..b8969cbede 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -38,10 +38,10 @@ namespace rclcpp { -namespace node +namespace node_interfaces { -class Node; -} // namespace node +class NodeTopicsInterface; +} // namespace node_interfaces namespace subscription { @@ -56,14 +56,16 @@ class SubscriptionBase /// Default constructor. /** * \param[in] node_handle The rcl representation of the node that owns this subscription. + * \param[in] type_support_handle rosidl type support struct, for the Message type of the topic. * \param[in] topic_name Name of the topic to subscribe to. - * \param[in] ignore_local_publications True to ignore local publications (unused). + * \param[in] subscription_options options for the subscription. */ RCLCPP_PUBLIC SubscriptionBase( std::shared_ptr node_handle, + const rosidl_message_type_support_t & type_support_handle, const std::string & topic_name, - bool ignore_local_publications); + const rcl_subscription_options_t & subscription_options); /// Default destructor. RCLCPP_PUBLIC @@ -71,7 +73,7 @@ class SubscriptionBase /// Get the topic that this subscription is subscribed on. RCLCPP_PUBLIC - const std::string & + const char * get_topic_name() const; RCLCPP_PUBLIC @@ -110,8 +112,6 @@ class SubscriptionBase private: RCLCPP_DISABLE_COPY(SubscriptionBase) - std::string topic_name_; - bool ignore_local_publications_; }; using any_subscription_callback::AnySubscriptionCallback; @@ -120,7 +120,7 @@ using any_subscription_callback::AnySubscriptionCallback; template> class Subscription : public SubscriptionBase { - friend class rclcpp::node::Node; + friend class rclcpp::node_interfaces::NodeTopicsInterface; public: using MessageAllocTraits = allocator::AllocRebind; @@ -136,8 +136,8 @@ class Subscription : public SubscriptionBase * should be instantiated through Node::create_subscription. * \param[in] node_handle rcl representation of the node that owns this subscription. * \param[in] topic_name Name of the topic to subscribe to. - * \param[in] ignore_local_publications True to ignore local publications (unused). - * \param[in] callback User-defined callback to call when a message is received. + * \param[in] subscription_options options for the subscription. + * \param[in] callback User defined callback to call when a message is received. * \param[in] memory_strategy The memory strategy to be used for managing message memory. */ Subscription( @@ -149,23 +149,15 @@ class Subscription : public SubscriptionBase memory_strategy = message_memory_strategy::MessageMemoryStrategy::create_default()) : SubscriptionBase( - node_handle, topic_name, subscription_options.ignore_local_publications), + node_handle, + *rosidl_typesupport_cpp::get_message_type_support_handle(), + topic_name, + subscription_options), any_callback_(callback), message_memory_strategy_(memory_strategy), get_intra_process_message_callback_(nullptr), matches_any_intra_process_publishers_(nullptr) - { - using rosidl_typesupport_cpp::get_message_type_support_handle; - - auto type_support_handle = get_message_type_support_handle(); - if (rcl_subscription_init( - &subscription_handle_, node_handle_.get(), type_support_handle, topic_name.c_str(), - &subscription_options) != RCL_RET_OK) - { - throw std::runtime_error( - std::string("could not create subscription: ") + rcl_get_error_string_safe()); - } - } + {} /// Support dynamically setting the message memory strategy. /** @@ -232,13 +224,11 @@ class Subscription : public SubscriptionBase any_callback_.dispatch_intra_process(msg, message_info); } -private: - typedef - std::function< - void (uint64_t, uint64_t, uint64_t, MessageUniquePtr &) - > GetMessageCallbackType; - typedef std::function MatchesAnyPublishersCallbackType; + using GetMessageCallbackType = + std::function; + using MatchesAnyPublishersCallbackType = std::function; + /// Implemenation detail. void setup_intra_process( uint64_t intra_process_subscription_id, GetMessageCallbackType get_message_callback, @@ -248,7 +238,7 @@ class Subscription : public SubscriptionBase if (rcl_subscription_init( &intra_process_subscription_handle_, node_handle_.get(), rclcpp::type_support::get_intra_process_message_msg_type_support(), - (get_topic_name() + "__intra").c_str(), + (std::string(get_topic_name()) + "__intra").c_str(), &intra_process_options) != RCL_RET_OK) { // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) @@ -262,6 +252,7 @@ class Subscription : public SubscriptionBase matches_any_intra_process_publishers_ = matches_any_publisher_callback; } + /// Implemenation detail. const rcl_subscription_t * get_intra_process_subscription_handle() const { @@ -271,6 +262,7 @@ class Subscription : public SubscriptionBase return &intra_process_subscription_handle_; } +private: RCLCPP_DISABLE_COPY(Subscription) AnySubscriptionCallback any_callback_; diff --git a/rclcpp/include/rclcpp/subscription_factory.hpp b/rclcpp/include/rclcpp/subscription_factory.hpp new file mode 100644 index 0000000000..d9f006699e --- /dev/null +++ b/rclcpp/include/rclcpp/subscription_factory.hpp @@ -0,0 +1,170 @@ +// 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__SUBSCRIPTION_FACTORY_HPP_ +#define RCLCPP__SUBSCRIPTION_FACTORY_HPP_ + +#include +#include +#include +#include + +#include "rcl/subscription.h" + +#include "rosidl_typesupport_cpp/message_type_support.hpp" + +#include "rclcpp/subscription.hpp" +#include "rclcpp/intra_process_manager.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// Factory with functions used to create a Subscription. +/* This factory class is used to encapsulate the template generated functions + * which are used during the creation of a Message type specific subscription + * within a non-templated class. + * + * It is created using the create_subscription_factory function, which is + * usually called from a templated "create_subscription" method of the Node + * class, and is passed to the non-templated "create_subscription" method of + * the NodeTopics class where it is used to create and setup the Subscription. + */ +struct SubscriptionFactory +{ + // Creates a Subscription object and returns it as a SubscriptionBase. + using SubscriptionFactoryFunction = std::function< + rclcpp::subscription::SubscriptionBase::SharedPtr( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const std::string & topic_name, + rcl_subscription_options_t & subscription_options)>; + + SubscriptionFactoryFunction create_typed_subscription; + + // Function that takes a MessageT from the intra process manager + using SetupIntraProcessFunction = std::function; + + SetupIntraProcessFunction setup_intra_process; +}; + +/// Return a SubscriptionFactory with functions for creating a SubscriptionT. +template +SubscriptionFactory +create_subscription_factory( + CallbackT && callback, + typename rclcpp::message_memory_strategy::MessageMemoryStrategy::SharedPtr + msg_mem_strat, + std::shared_ptr allocator) +{ + SubscriptionFactory factory; + + using rclcpp::subscription::AnySubscriptionCallback; + AnySubscriptionCallback any_subscription_callback(allocator); + any_subscription_callback.set(std::forward(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]( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const std::string & topic_name, + rcl_subscription_options_t & subscription_options + ) -> rclcpp::subscription::SubscriptionBase::SharedPtr + { + subscription_options.allocator = + rclcpp::allocator::get_rcl_allocator(*message_alloc.get()); + + using rclcpp::subscription::Subscription; + using rclcpp::subscription::SubscriptionBase; + + auto sub = Subscription::make_shared( + node_base->get_shared_rcl_node_handle(), + topic_name, + subscription_options, + any_subscription_callback, + msg_mem_strat); + auto sub_base_ptr = std::dynamic_pointer_cast(sub); + return sub_base_ptr; + }; + + // function that will setup intra process communications for the subscription + factory.setup_intra_process = + [message_alloc]( + rclcpp::intra_process_manager::IntraProcessManager::SharedPtr ipm, + rclcpp::subscription::SubscriptionBase::SharedPtr subscription, + const rcl_subscription_options_t & subscription_options) + { + rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = ipm; + uint64_t intra_process_subscription_id = ipm->add_subscription(subscription); + + auto intra_process_options = rcl_subscription_get_default_options(); + intra_process_options.allocator = rclcpp::allocator::get_rcl_allocator( + *message_alloc.get()); + intra_process_options.qos = subscription_options.qos; + intra_process_options.ignore_local_publications = false; + + // function that will be called to take a MessageT from the intra process manager + auto take_intra_process_message_func = + [weak_ipm]( + uint64_t publisher_id, + uint64_t message_sequence, + uint64_t subscription_id, + typename rclcpp::subscription::Subscription::MessageUniquePtr & message) + { + auto ipm = weak_ipm.lock(); + if (!ipm) { + // TODO(wjwwood): should this just return silently? Or return with a logged warning? + throw std::runtime_error( + "intra process take called after destruction of intra process manager"); + } + ipm->take_intra_process_message( + publisher_id, message_sequence, subscription_id, message); + }; + + // function that is called to see if the publisher id matches any local publishers + auto matches_any_publisher_func = + [weak_ipm](const rmw_gid_t * sender_gid) -> bool + { + auto ipm = weak_ipm.lock(); + if (!ipm) { + throw std::runtime_error( + "intra process publisher check called " + "after destruction of intra process manager"); + } + return ipm->matches_any_publishers(sender_gid); + }; + + auto typed_sub_ptr = std::dynamic_pointer_cast(subscription); + typed_sub_ptr->setup_intra_process( + intra_process_subscription_id, + take_intra_process_message_func, + matches_any_publisher_func, + intra_process_options + ); + }; + // end definition of factory function to setup intra process + + // return the factory now that it is populated + return factory; +} + +} // namespace rclcpp + +#endif // RCLCPP__SUBSCRIPTION_FACTORY_HPP_ diff --git a/rclcpp/src/rclcpp/any_executable.cpp b/rclcpp/src/rclcpp/any_executable.cpp index 0cef731053..256033bbad 100644 --- a/rclcpp/src/rclcpp/any_executable.cpp +++ b/rclcpp/src/rclcpp/any_executable.cpp @@ -23,7 +23,7 @@ AnyExecutable::AnyExecutable() service(nullptr), client(nullptr), callback_group(nullptr), - node(nullptr) + node_base(nullptr) {} AnyExecutable::~AnyExecutable() diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index c9666236ad..c15a0b103b 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -23,7 +23,8 @@ #include "rcl/node.h" #include "rcl/wait.h" #include "rclcpp/exceptions.hpp" -#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_graph_interface.hpp" #include "rclcpp/utilities.hpp" using rclcpp::client::ClientBase; @@ -31,9 +32,11 @@ using rclcpp::exceptions::InvalidNodeError; using rclcpp::exceptions::throw_from_rcl_error; ClientBase::ClientBase( - std::shared_ptr parent_node, + rclcpp::node_interfaces::NodeBaseInterface * node_base, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, const std::string & service_name) -: node_(parent_node), node_handle_(parent_node->get_shared_node_handle()), +: node_graph_(node_graph), + node_handle_(node_base->get_shared_rcl_node_handle()), service_name_(service_name) {} @@ -76,7 +79,7 @@ ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout) return false; } // make an event to reuse, rather than create a new one each time - auto node_ptr = node_.lock(); + auto node_ptr = node_graph_.lock(); if (!node_ptr) { throw InvalidNodeError(); } diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 145b90e863..a22abfa840 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -21,6 +21,7 @@ #include "rcl/error_handling.h" #include "rclcpp/executor.hpp" +#include "rclcpp/node.hpp" #include "rclcpp/scope_exit.hpp" #include "rclcpp/utilities.hpp" @@ -86,10 +87,11 @@ Executor::~Executor() } void -Executor::add_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify) +Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) { // If the node already has an executor - if (node_ptr->has_executor.exchange(true)) { + std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); + if (has_executor.exchange(true)) { throw std::runtime_error("Node has already been added to an executor."); } // Check to ensure node not already added @@ -112,14 +114,20 @@ Executor::add_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify) } void -Executor::remove_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify) +Executor::add_node(std::shared_ptr node_ptr, bool notify) +{ + this->add_node(node_ptr->get_node_base_interface(), notify); +} + +void +Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) { bool node_removed = false; weak_nodes_.erase( std::remove_if( weak_nodes_.begin(), weak_nodes_.end(), // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) - [&](std::weak_ptr & i) + [&](rclcpp::node_interfaces::NodeBaseInterface::WeakPtr & i) { bool matched = (i.lock() == node_ptr); node_removed |= matched; @@ -128,7 +136,8 @@ Executor::remove_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify) // *INDENT-ON* ) ); - node_ptr->has_executor.store(false); + std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); + has_executor.store(false); if (notify) { // If the node was matched and removed, interrupt waiting if (node_removed) { @@ -140,9 +149,15 @@ Executor::remove_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify) memory_strategy_->remove_guard_condition(node_ptr->get_notify_guard_condition()); } +void +Executor::remove_node(std::shared_ptr node_ptr, bool notify) +{ + this->remove_node(node_ptr->get_node_base_interface(), notify); +} + void Executor::spin_node_once_nanoseconds( - rclcpp::node::Node::SharedPtr node, + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node, std::chrono::nanoseconds timeout) { this->add_node(node, false); @@ -152,13 +167,19 @@ Executor::spin_node_once_nanoseconds( } void -Executor::spin_node_some(rclcpp::node::Node::SharedPtr node) +Executor::spin_node_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node) { this->add_node(node, false); spin_some(); this->remove_node(node, false); } +void +Executor::spin_node_some(std::shared_ptr node) +{ + this->spin_node_some(node->get_node_base_interface()); +} + void Executor::spin_some() { @@ -248,7 +269,7 @@ Executor::execute_subscription( } else if (ret != RCL_RET_SUBSCRIPTION_TAKE_FAILED) { fprintf(stderr, "[rclcpp::error] take failed for subscription on topic '%s': %s\n", - subscription->get_topic_name().c_str(), rcl_get_error_string_safe()); + subscription->get_topic_name(), rcl_get_error_string_safe()); } subscription->return_message(message); } @@ -270,7 +291,7 @@ Executor::execute_intra_process_subscription( } else if (status != RCL_RET_SUBSCRIPTION_TAKE_FAILED) { fprintf(stderr, "[rclcpp::error] take failed for intra process subscription on topic '%s': %s\n", - subscription->get_topic_name().c_str(), rcl_get_error_string_safe()); + subscription->get_topic_name(), rcl_get_error_string_safe()); } } @@ -332,7 +353,7 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) remove_if( weak_nodes_.begin(), weak_nodes_.end(), // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) - [](std::weak_ptr i) + [](rclcpp::node_interfaces::NodeBaseInterface::WeakPtr i) { return i.expired(); } @@ -412,11 +433,11 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) } } -rclcpp::node::Node::SharedPtr +rclcpp::node_interfaces::NodeBaseInterface::SharedPtr Executor::get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group) { if (!group) { - return rclcpp::node::Node::SharedPtr(); + return nullptr; } for (auto & weak_node : weak_nodes_) { auto node = weak_node.lock(); @@ -430,7 +451,7 @@ Executor::get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr gro } } } - return rclcpp::node::Node::SharedPtr(); + return nullptr; } rclcpp::callback_group::CallbackGroup::SharedPtr diff --git a/rclcpp/src/rclcpp/executors.cpp b/rclcpp/src/rclcpp/executors.cpp index 1dd2f181dd..d526808238 100644 --- a/rclcpp/src/rclcpp/executors.cpp +++ b/rclcpp/src/rclcpp/executors.cpp @@ -15,17 +15,29 @@ #include "rclcpp/executors.hpp" void -rclcpp::spin_some(node::Node::SharedPtr node_ptr) +rclcpp::spin_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) { rclcpp::executors::SingleThreadedExecutor exec; exec.spin_node_some(node_ptr); } void -rclcpp::spin(node::Node::SharedPtr node_ptr) +rclcpp::spin_some(rclcpp::node::Node::SharedPtr node_ptr) +{ + rclcpp::spin_some(node_ptr->get_node_base_interface()); +} + +void +rclcpp::spin(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) { rclcpp::executors::SingleThreadedExecutor exec; exec.add_node(node_ptr); exec.spin(); exec.remove_node(node_ptr); } + +void +rclcpp::spin(rclcpp::node::Node::SharedPtr node_ptr) +{ + rclcpp::spin(node_ptr->get_node_base_interface()); +} diff --git a/rclcpp/src/rclcpp/graph_listener.cpp b/rclcpp/src/rclcpp/graph_listener.cpp index a68ded4b7a..6d24158015 100644 --- a/rclcpp/src/rclcpp/graph_listener.cpp +++ b/rclcpp/src/rclcpp/graph_listener.cpp @@ -115,17 +115,17 @@ GraphListener::run_loop() rcl_ret_t ret; { // This "barrier" lock ensures that other functions can acquire the - // nodes_mutex_ after waking up rcl_wait. - std::lock_guard nodes_barrier_lock(nodes_barrier_mutex_); + // node_graph_interfaces_mutex_ after waking up rcl_wait. + std::lock_guard nodes_barrier_lock(node_graph_interfaces_barrier_mutex_); // This is ownership is passed to nodes_lock in the next line. - nodes_mutex_.lock(); + node_graph_interfaces_mutex_.lock(); } // This lock is released when the loop continues or exits. - std::lock_guard nodes_lock(nodes_mutex_, std::adopt_lock); + std::lock_guard nodes_lock(node_graph_interfaces_mutex_, std::adopt_lock); // Resize the wait set if necessary. - if (wait_set_.size_of_guard_conditions < (nodes_.size() + 2)) { - ret = rcl_wait_set_resize_guard_conditions(&wait_set_, nodes_.size() + 2); + if (wait_set_.size_of_guard_conditions < (node_graph_interfaces_.size() + 2)) { + ret = rcl_wait_set_resize_guard_conditions(&wait_set_, node_graph_interfaces_.size() + 2); if (RCL_RET_OK != ret) { throw_from_rcl_error(ret, "failed to resize wait set"); } @@ -146,13 +146,13 @@ GraphListener::run_loop() throw_from_rcl_error(ret, "failed to add shutdown guard condition to wait set"); } // Put graph guard conditions for each node into the wait set. - for (const auto node_ptr : nodes_) { + for (const auto node_ptr : node_graph_interfaces_) { // Only wait on graph changes if some user of the node is watching. if (node_ptr->count_graph_users() == 0) { continue; } // Add the graph guard condition for the node to the wait set. - auto graph_gc = rcl_node_get_graph_guard_condition(node_ptr->get_rcl_node_handle()); + auto graph_gc = node_ptr->get_graph_guard_condition(); if (!graph_gc) { throw_from_rcl_error(RCL_RET_ERROR, "failed to get graph guard condition"); } @@ -179,8 +179,8 @@ GraphListener::run_loop() } } // Notify nodes who's guard conditions are set (triggered). - for (const auto node_ptr : nodes_) { - auto graph_gc = rcl_node_get_graph_guard_condition(node_ptr->get_rcl_node_handle()); + for (const auto node_ptr : node_graph_interfaces_) { + auto graph_gc = node_ptr->get_graph_guard_condition(); if (!graph_gc) { throw_from_rcl_error(RCL_RET_ERROR, "failed to get graph guard condition"); } @@ -208,25 +208,27 @@ interrupt_(rcl_guard_condition_t * interrupt_guard_condition) static void acquire_nodes_lock_( - std::mutex * nodes_barrier_mutex, - std::mutex * nodes_mutex, + std::mutex * node_graph_interfaces_barrier_mutex, + std::mutex * node_graph_interfaces_mutex, rcl_guard_condition_t * interrupt_guard_condition) { { // Acquire this lock to prevent the run loop from re-locking the // nodes_mutext_ after being woken up. - std::lock_guard nodes_barrier_lock(*nodes_barrier_mutex); + std::lock_guard nodes_barrier_lock(*node_graph_interfaces_barrier_mutex); // Trigger the interrupt guard condition to wake up rcl_wait. interrupt_(interrupt_guard_condition); - nodes_mutex->lock(); + node_graph_interfaces_mutex->lock(); } } static bool -has_node_(std::vector * nodes, rclcpp::node::Node * node) +has_node_( + std::vector * node_graph_interfaces, + rclcpp::node_interfaces::NodeGraphInterface * node_graph) { - for (const auto node_ptr : (*nodes)) { - if (node == node_ptr) { + for (const auto node_ptr : (*node_graph_interfaces)) { + if (node_graph == node_ptr) { return true; } } @@ -234,23 +236,26 @@ has_node_(std::vector * nodes, rclcpp::node::Node * node) } bool -GraphListener::has_node(rclcpp::node::Node * node) +GraphListener::has_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph) { - if (!node) { + if (!node_graph) { return false; } // Acquire the nodes mutex using the barrier to prevent the run loop from // re-locking the nodes mutex after being interrupted. - acquire_nodes_lock_(&nodes_barrier_mutex_, &nodes_mutex_, &interrupt_guard_condition_); - // Store the now acquired nodes_mutex_ in the scoped lock using adopt_lock. - std::lock_guard nodes_lock(nodes_mutex_, std::adopt_lock); - return has_node_(&nodes_, node); + acquire_nodes_lock_( + &node_graph_interfaces_barrier_mutex_, + &node_graph_interfaces_mutex_, + &interrupt_guard_condition_); + // Store the now acquired node_graph_interfaces_mutex_ in the scoped lock using adopt_lock. + std::lock_guard nodes_lock(node_graph_interfaces_mutex_, std::adopt_lock); + return has_node_(&node_graph_interfaces_, node_graph); } void -GraphListener::add_node(rclcpp::node::Node * node) +GraphListener::add_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph) { - if (!node) { + if (!node_graph) { throw std::invalid_argument("node is nullptr"); } std::lock_guard shutdown_lock(shutdown_mutex_); @@ -260,25 +265,30 @@ GraphListener::add_node(rclcpp::node::Node * node) // Acquire the nodes mutex using the barrier to prevent the run loop from // re-locking the nodes mutex after being interrupted. - acquire_nodes_lock_(&nodes_barrier_mutex_, &nodes_mutex_, &interrupt_guard_condition_); - // Store the now acquired nodes_mutex_ in the scoped lock using adopt_lock. - std::lock_guard nodes_lock(nodes_mutex_, std::adopt_lock); - if (has_node_(&nodes_, node)) { + acquire_nodes_lock_( + &node_graph_interfaces_barrier_mutex_, + &node_graph_interfaces_mutex_, + &interrupt_guard_condition_); + // Store the now acquired node_graph_interfaces_mutex_ in the scoped lock using adopt_lock. + std::lock_guard nodes_lock(node_graph_interfaces_mutex_, std::adopt_lock); + if (has_node_(&node_graph_interfaces_, node_graph)) { throw NodeAlreadyAddedError(); } - nodes_.push_back(node); + node_graph_interfaces_.push_back(node_graph); // The run loop has already been interrupted by acquire_nodes_lock_() and - // will evaluate the new node when nodes_lock releases the nodes_mutex_. + // will evaluate the new node when nodes_lock releases the node_graph_interfaces_mutex_. } static void -remove_node_(std::vector * nodes, rclcpp::node::Node * node) +remove_node_( + std::vector * node_graph_interfaces, + rclcpp::node_interfaces::NodeGraphInterface * node_graph) { // Remove the node if it is found. - for (auto it = nodes->begin(); it != nodes->end(); ++it) { - if (node == *it) { + for (auto it = node_graph_interfaces->begin(); it != node_graph_interfaces->end(); ++it) { + if (node_graph == *it) { // Found the node, remove it. - nodes->erase(it); + node_graph_interfaces->erase(it); // Now trigger the interrupt guard condition to make sure return; } @@ -288,23 +298,26 @@ remove_node_(std::vector * nodes, rclcpp::node::Node * nod } void -GraphListener::remove_node(rclcpp::node::Node * node) +GraphListener::remove_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph) { - if (!node) { + if (!node_graph) { throw std::invalid_argument("node is nullptr"); } std::lock_guard shutdown_lock(shutdown_mutex_); if (is_shutdown()) { // If shutdown, then the run loop has been joined, so we can remove them directly. - return remove_node_(&nodes_, node); + return remove_node_(&node_graph_interfaces_, node_graph); } // Otherwise, first interrupt and lock against the run loop to safely remove the node. // Acquire the nodes mutex using the barrier to prevent the run loop from // re-locking the nodes mutex after being interrupted. - acquire_nodes_lock_(&nodes_barrier_mutex_, &nodes_mutex_, &interrupt_guard_condition_); - // Store the now acquired nodes_mutex_ in the scoped lock using adopt_lock. - std::lock_guard nodes_lock(nodes_mutex_, std::adopt_lock); - remove_node_(&nodes_, node); + acquire_nodes_lock_( + &node_graph_interfaces_barrier_mutex_, + &node_graph_interfaces_mutex_, + &interrupt_guard_condition_); + // Store the now acquired node_graph_interfaces_mutex_ in the scoped lock using adopt_lock. + std::lock_guard nodes_lock(node_graph_interfaces_mutex_, std::adopt_lock); + remove_node_(&node_graph_interfaces_, node_graph); } void diff --git a/rclcpp/src/rclcpp/memory_strategy.cpp b/rclcpp/src/rclcpp/memory_strategy.cpp index ae449f66ce..61b8432784 100644 --- a/rclcpp/src/rclcpp/memory_strategy.cpp +++ b/rclcpp/src/rclcpp/memory_strategy.cpp @@ -96,7 +96,7 @@ MemoryStrategy::get_client_by_handle(const rcl_client_t * client_handle, return nullptr; } -rclcpp::node::Node::SharedPtr +rclcpp::node_interfaces::NodeBaseInterface::SharedPtr MemoryStrategy::get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group, const WeakNodeVector & weak_nodes) { diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 604aae6f30..e1a4aba59a 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -20,11 +20,15 @@ #include #include -#include "rcl/graph.h" -#include "rcl_interfaces/srv/list_parameters.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/graph_listener.hpp" #include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_base.hpp" +#include "rclcpp/node_interfaces/node_graph.hpp" +#include "rclcpp/node_interfaces/node_parameters.hpp" +#include "rclcpp/node_interfaces/node_services.hpp" +#include "rclcpp/node_interfaces/node_timers.hpp" +#include "rclcpp/node_interfaces/node_topics.hpp" using rclcpp::node::Node; using rclcpp::exceptions::throw_from_rcl_error; @@ -40,532 +44,165 @@ Node::Node( const std::string & node_name, rclcpp::context::Context::SharedPtr context, bool use_intra_process_comms) -: name_(node_name), context_(context), - number_of_subscriptions_(0), number_of_timers_(0), number_of_services_(0), number_of_clients_(0), - use_intra_process_comms_(use_intra_process_comms), notify_guard_condition_is_valid_(false), - graph_listener_(context->get_sub_context()), - should_add_to_graph_listener_(true), graph_users_count_(0) +: node_base_(new rclcpp::node_interfaces::NodeBase(node_name, context)), + node_graph_(new rclcpp::node_interfaces::NodeGraph(node_base_.get())), + node_timers_(new rclcpp::node_interfaces::NodeTimers(node_base_.get())), + node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get())), + node_services_(new rclcpp::node_interfaces::NodeServices(node_base_.get())), + node_parameters_(new rclcpp::node_interfaces::NodeParameters( + node_topics_.get(), + use_intra_process_comms + )), + use_intra_process_comms_(use_intra_process_comms) { - rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options(); - rcl_ret_t ret = rcl_guard_condition_init(¬ify_guard_condition_, guard_condition_options); - if (ret != RCL_RET_OK) { - throw_from_rcl_error(ret, "failed to create interrupt guard condition"); - } - - has_executor.store(false); - size_t domain_id = 0; - char * ros_domain_id = nullptr; - const char * env_var = "ROS_DOMAIN_ID"; -#ifndef _WIN32 - ros_domain_id = getenv(env_var); -#else - size_t ros_domain_id_size; - _dupenv_s(&ros_domain_id, &ros_domain_id_size, env_var); -#endif - if (ros_domain_id) { - uint32_t number = strtoul(ros_domain_id, NULL, 0); - if (number == (std::numeric_limits::max)()) { - // Finalize the interrupt guard condition. - if (rcl_guard_condition_fini(¬ify_guard_condition_) != RCL_RET_OK) { - fprintf(stderr, - "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe()); - } - throw std::runtime_error("failed to interpret ROS_DOMAIN_ID as integral number"); - } - domain_id = static_cast(number); -#ifdef _WIN32 - free(ros_domain_id); -#endif - } - - rcl_node_t * rcl_node = new rcl_node_t(rcl_get_zero_initialized_node()); - node_handle_.reset(rcl_node, [](rcl_node_t * node) { - if (rcl_node_fini(node) != RCL_RET_OK) { - fprintf( - stderr, "Error in destruction of rmw node handle: %s\n", rcl_get_error_string_safe()); - } - delete node; - }); - rcl_node_options_t options = rcl_node_get_default_options(); - // TODO(jacquelinekay): Allocator options - options.domain_id = domain_id; - ret = rcl_node_init(node_handle_.get(), name_.c_str(), &options); - if (ret != RCL_RET_OK) { - // Finalize the interrupt guard condition. - if (rcl_guard_condition_fini(¬ify_guard_condition_) != RCL_RET_OK) { - fprintf(stderr, - "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe()); - } - - throw_from_rcl_error(ret, "failed to initialize rcl node"); - } - - using rclcpp::callback_group::CallbackGroupType; - default_callback_group_ = create_callback_group( - CallbackGroupType::MutuallyExclusive); - events_publisher_ = create_publisher( - "parameter_events", rmw_qos_profile_parameter_events); - notify_guard_condition_is_valid_ = true; } Node::~Node() -{ - // Remove self from graph listener. - // Exchange with false to prevent others from trying to add this node to the - // graph listener after checking that it was not here. - if (!should_add_to_graph_listener_.exchange(false)) { - // If it was already false, then it needs to now be removed. - graph_listener_->remove_node(this); - } - // Finalize the interrupt guard condition after removing self from graph listener. - { - std::lock_guard notify_guard_condition_lock(notify_guard_condition_mutex_); - notify_guard_condition_is_valid_ = false; - if (rcl_guard_condition_fini(¬ify_guard_condition_) != RCL_RET_OK) { - fprintf(stderr, - "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe()); - } - } -} +{} -const std::string & +const char * Node::get_name() const { - return name_; + return node_base_->get_name(); } rclcpp::callback_group::CallbackGroup::SharedPtr Node::create_callback_group( rclcpp::callback_group::CallbackGroupType group_type) { - using rclcpp::callback_group::CallbackGroup; - using rclcpp::callback_group::CallbackGroupType; - auto group = CallbackGroup::SharedPtr(new CallbackGroup(group_type)); - callback_groups_.push_back(group); - return group; + return node_base_->create_callback_group(group_type); } bool Node::group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group) { - bool group_belongs_to_this_node = false; - for (auto & weak_group : this->callback_groups_) { - auto cur_group = weak_group.lock(); - if (cur_group && (cur_group == group)) { - group_belongs_to_this_node = true; - } - } - return group_belongs_to_this_node; + return node_base_->callback_group_in_node(group); } -// TODO(wjwwood): reenable this once I figure out why the demo doesn't build with it. -// rclcpp::timer::WallTimer::SharedPtr -// Node::create_wall_timer( -// std::chrono::duration period, -// rclcpp::timer::CallbackType callback, -// rclcpp::callback_group::CallbackGroup::SharedPtr group) -// { -// return create_wall_timer( -// std::chrono::duration_cast(period), -// callback, -// group); -// } - std::vector Node::set_parameters( const std::vector & parameters) { - std::vector results; - for (auto p : parameters) { - auto result = set_parameters_atomically({{p}}); - results.push_back(result); - } - return results; + return node_parameters_->set_parameters(parameters); } rcl_interfaces::msg::SetParametersResult Node::set_parameters_atomically( const std::vector & parameters) { - std::lock_guard lock(mutex_); - std::map tmp_map; - auto parameter_event = std::make_shared(); - - // TODO(jacquelinekay): handle parameter constraints - rcl_interfaces::msg::SetParametersResult result; - if (parameters_callback_) { - result = parameters_callback_(parameters); - } else { - result.successful = true; - } - - if (!result.successful) { - return result; - } - - for (auto p : parameters) { - if (parameters_.find(p.get_name()) == parameters_.end()) { - if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) { - parameter_event->new_parameters.push_back(p.to_parameter()); - } - } else if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) { - parameter_event->changed_parameters.push_back(p.to_parameter()); - } else { - parameter_event->deleted_parameters.push_back(p.to_parameter()); - } - tmp_map[p.get_name()] = p; - } - // std::map::insert will not overwrite elements, so we'll keep the new - // ones and add only those that already exist in the Node's internal map - tmp_map.insert(parameters_.begin(), parameters_.end()); - std::swap(tmp_map, parameters_); - - events_publisher_->publish(parameter_event); - - return result; + return node_parameters_->set_parameters_atomically(parameters); } std::vector Node::get_parameters( const std::vector & names) const { - std::lock_guard lock(mutex_); - std::vector results; - - for (auto & name : names) { - if (std::any_of(parameters_.cbegin(), parameters_.cend(), - [&name](const std::pair & kv) { - return name == kv.first; - })) - { - results.push_back(parameters_.at(name)); - } - } - return results; + return node_parameters_->get_parameters(names); } -const rclcpp::parameter::ParameterVariant +rclcpp::parameter::ParameterVariant Node::get_parameter(const std::string & name) const { - rclcpp::parameter::ParameterVariant parameter; - - if (get_parameter(name, parameter)) { - return parameter; - } else { - throw std::out_of_range("Parameter '" + name + "' not set"); - } + return node_parameters_->get_parameter(name); } bool Node::get_parameter(const std::string & name, rclcpp::parameter::ParameterVariant & parameter) const { - std::lock_guard lock(mutex_); - - if (parameters_.count(name)) { - parameter = parameters_.at(name); - return true; - } else { - return false; - } + return node_parameters_->get_parameter(name, parameter); } std::vector Node::describe_parameters( const std::vector & names) const { - std::lock_guard lock(mutex_); - std::vector results; - for (auto & kv : parameters_) { - if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) { - return name == kv.first; - })) - { - rcl_interfaces::msg::ParameterDescriptor parameter_descriptor; - parameter_descriptor.name = kv.first; - parameter_descriptor.type = kv.second.get_type(); - results.push_back(parameter_descriptor); - } - } - return results; + return node_parameters_->describe_parameters(names); } std::vector Node::get_parameter_types( const std::vector & names) const { - std::lock_guard lock(mutex_); - std::vector results; - for (auto & kv : parameters_) { - if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) { - return name == kv.first; - })) - { - results.push_back(kv.second.get_type()); - } else { - results.push_back(rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET); - } - } - return results; + return node_parameters_->get_parameter_types(names); } rcl_interfaces::msg::ListParametersResult Node::list_parameters( const std::vector & prefixes, uint64_t depth) const { - std::lock_guard lock(mutex_); - rcl_interfaces::msg::ListParametersResult result; - - // TODO(esteve): define parameter separator, use "." for now - for (auto & kv : parameters_) { - if (((prefixes.size() == 0) && - ((depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) || - (static_cast(std::count(kv.first.begin(), kv.first.end(), '.')) < depth))) || - (std::any_of(prefixes.cbegin(), prefixes.cend(), [&kv, &depth](const std::string & prefix) { - if (kv.first == prefix) { - return true; - } else if (kv.first.find(prefix + ".") == 0) { - size_t length = prefix.length(); - std::string substr = kv.first.substr(length); - // Cast as unsigned integer to avoid warning - return (depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) || - (static_cast(std::count(substr.begin(), substr.end(), '.')) < depth); - } - return false; - }))) - { - result.names.push_back(kv.first); - size_t last_separator = kv.first.find_last_of('.'); - if (std::string::npos != last_separator) { - std::string prefix = kv.first.substr(0, last_separator); - if (std::find(result.prefixes.cbegin(), result.prefixes.cend(), prefix) == - result.prefixes.cend()) - { - result.prefixes.push_back(prefix); - } - } - } - } - return result; + return node_parameters_->list_parameters(prefixes, depth); } std::map Node::get_topic_names_and_types() const { - rcl_topic_names_and_types_t topic_names_and_types = - rcl_get_zero_initialized_topic_names_and_types(); - - auto ret = rcl_get_topic_names_and_types(node_handle_.get(), - &topic_names_and_types); - if (ret != RMW_RET_OK) { - // *INDENT-OFF* - throw std::runtime_error( - std::string("could not get topic names and types: ") + rmw_get_error_string_safe()); - // *INDENT-ON* - } - - std::map topics; - for (size_t i = 0; i < topic_names_and_types.topic_count; ++i) { - topics[topic_names_and_types.topic_names[i]] = topic_names_and_types.type_names[i]; - } - - ret = rmw_destroy_topic_names_and_types(&topic_names_and_types); - if (ret != RMW_RET_OK) { - // *INDENT-OFF* - throw std::runtime_error( - std::string("could not destroy topic names and types: ") + rmw_get_error_string_safe()); - // *INDENT-ON* - } - - return topics; + return node_graph_->get_topic_names_and_types(); } size_t Node::count_publishers(const std::string & topic_name) const { - size_t count; - auto ret = rmw_count_publishers(rcl_node_get_rmw_handle(node_handle_.get()), - topic_name.c_str(), &count); - if (ret != RMW_RET_OK) { - // *INDENT-OFF* - throw std::runtime_error( - std::string("could not count publishers: ") + rmw_get_error_string_safe()); - // *INDENT-ON* - } - return count; + return node_graph_->count_publishers(topic_name); } size_t Node::count_subscribers(const std::string & topic_name) const { - size_t count; - auto ret = rmw_count_subscribers(rcl_node_get_rmw_handle(node_handle_.get()), - topic_name.c_str(), &count); - if (ret != RMW_RET_OK) { - // *INDENT-OFF* - throw std::runtime_error( - std::string("could not count subscribers: ") + rmw_get_error_string_safe()); - // *INDENT-ON* - } - return count; + return node_graph_->count_subscribers(topic_name); } - -const Node::CallbackGroupWeakPtrList & +const std::vector & Node::get_callback_groups() const { - return callback_groups_; -} - -const rcl_guard_condition_t * -Node::get_notify_guard_condition() const -{ - std::lock_guard notify_guard_condition_lock(notify_guard_condition_mutex_); - if (!notify_guard_condition_is_valid_) { - return nullptr; - } - return ¬ify_guard_condition_; + return node_base_->get_callback_groups(); } -const rcl_guard_condition_t * -Node::get_graph_guard_condition() const -{ - return rcl_node_get_graph_guard_condition(node_handle_.get()); -} - -const rcl_node_t * -Node::get_rcl_node_handle() const -{ - return node_handle_.get(); -} - -rcl_node_t * -Node::get_rcl_node_handle() +rclcpp::event::Event::SharedPtr +Node::get_graph_event() { - return node_handle_.get(); + return node_graph_->get_graph_event(); } -std::shared_ptr -Node::get_shared_node_handle() +void +Node::wait_for_graph_change( + rclcpp::event::Event::SharedPtr event, + std::chrono::nanoseconds timeout) { - return node_handle_; + node_graph_->wait_for_graph_change(event, timeout); } -void -Node::notify_graph_change() +rclcpp::node_interfaces::NodeBaseInterface::SharedPtr +Node::get_node_base_interface() { - { - std::lock_guard graph_changed_lock(graph_mutex_); - bool bad_ptr_encountered = false; - for (auto & event_wptr : graph_events_) { - auto event_ptr = event_wptr.lock(); - if (event_ptr) { - event_ptr->set(); - } else { - bad_ptr_encountered = true; - } - } - if (bad_ptr_encountered) { - // remove invalid pointers with the erase-remove idiom - graph_events_.erase( - std::remove_if( - graph_events_.begin(), - graph_events_.end(), - [](const rclcpp::event::Event::WeakPtr & wptr) { - return wptr.expired(); - }), - graph_events_.end()); - // update graph_users_count_ - graph_users_count_.store(graph_events_.size()); - } - } - graph_cv_.notify_all(); - { - std::lock_guard notify_guard_condition_lock(notify_guard_condition_mutex_); - rcl_ret_t ret = rcl_trigger_guard_condition(¬ify_guard_condition_); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "failed to trigger notify guard condition"); - } - } + return node_base_; } -void -Node::notify_shutdown() +rclcpp::node_interfaces::NodeGraphInterface::SharedPtr +Node::get_node_graph_interface() { - // notify here anything that will not be woken up by ctrl-c or rclcpp::shutdown(). - graph_cv_.notify_all(); + return node_graph_; } -rclcpp::event::Event::SharedPtr -Node::get_graph_event() +rclcpp::node_interfaces::NodeTimersInterface::SharedPtr +Node::get_node_timers_interface() { - auto event = rclcpp::event::Event::make_shared(); - std::lock_guard graph_changed_lock(graph_mutex_); - // on first call, add node to graph_listener_ - if (should_add_to_graph_listener_.exchange(false)) { - graph_listener_->add_node(this); - graph_listener_->start_if_not_started(); - } - graph_events_.push_back(event); - graph_users_count_++; - return event; + return node_timers_; } -void -Node::wait_for_graph_change( - rclcpp::event::Event::SharedPtr event, - std::chrono::nanoseconds timeout) +rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr +Node::get_node_topics_interface() { - if (!event) { - throw InvalidEventError(); - } - { - std::lock_guard graph_changed_lock(graph_mutex_); - bool event_in_graph_events = false; - for (const auto & event_wptr : graph_events_) { - if (event == event_wptr.lock()) { - event_in_graph_events = true; - break; - } - } - if (!event_in_graph_events) { - throw EventNotRegisteredError(); - } - } - std::unique_lock graph_lock(graph_mutex_); - graph_cv_.wait_for(graph_lock, timeout, [&event]() { - return event->check() || !rclcpp::utilities::ok(); - }); + return node_topics_; } -size_t -Node::count_graph_users() +rclcpp::node_interfaces::NodeServicesInterface::SharedPtr +Node::get_node_services_interface() { - return graph_users_count_.load(); + return node_services_; } - -// PROTECTED IMPLEMENTATION -void -Node::add_service(rclcpp::service::ServiceBase::SharedPtr serv_base_ptr, - rclcpp::callback_group::CallbackGroup::SharedPtr group) +rclcpp::node_interfaces::NodeParametersInterface::SharedPtr +Node::get_node_parameters_interface() { - if (!serv_base_ptr) { - throw std::runtime_error("Cannot add empty service to group"); - } - - if (group) { - if (!group_in_node(group)) { - // TODO(jacquelinekay): use custom exception - throw std::runtime_error("Cannot create service, group not in node."); - } - group->add_service(serv_base_ptr); - } else { - default_callback_group_->add_service(serv_base_ptr); - } - number_of_services_++; - if (rcl_trigger_guard_condition(¬ify_guard_condition_) != RCL_RET_OK) { - throw std::runtime_error( - std::string( - "Failed to notify waitset on service creation: ") + rmw_get_error_string()); - } + return node_parameters_; } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp new file mode 100644 index 0000000000..50f880142b --- /dev/null +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -0,0 +1,215 @@ +// 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 +#include +#include +#include + +#include "rclcpp/node_interfaces/node_base.hpp" + +#include "rclcpp/exceptions.hpp" + +using rclcpp::exceptions::throw_from_rcl_error; + +using rclcpp::node_interfaces::NodeBase; + +NodeBase::NodeBase( + const std::string & node_name, + rclcpp::context::Context::SharedPtr context) +: context_(context), + node_handle_(nullptr), + default_callback_group_(nullptr), + associated_with_executor_(false), + notify_guard_condition_is_valid_(false) +{ + // Setup the guard condition that is notified when changes occur in the graph. + rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options(); + rcl_ret_t ret = rcl_guard_condition_init(¬ify_guard_condition_, guard_condition_options); + if (ret != RCL_RET_OK) { + throw_from_rcl_error(ret, "failed to create interrupt guard condition"); + } + + // Setup a safe exit lamda to clean up the guard condition in case of an error here. + auto finalize_notify_guard_condition = [this]() { + // Finalize the interrupt guard condition. + if (rcl_guard_condition_fini(¬ify_guard_condition_) != RCL_RET_OK) { + fprintf(stderr, + "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe()); + } + }; + + // Determine the domain id based on the options and the ROS_DOMAIN_ID env variable. + size_t domain_id = 0; + char * ros_domain_id = nullptr; + const char * env_var = "ROS_DOMAIN_ID"; +#ifndef _WIN32 + ros_domain_id = getenv(env_var); +#else + size_t ros_domain_id_size; + _dupenv_s(&ros_domain_id, &ros_domain_id_size, env_var); +#endif + if (ros_domain_id) { + uint32_t number = strtoul(ros_domain_id, NULL, 0); + if (number == (std::numeric_limits::max)()) { + // Finalize the interrupt guard condition. + finalize_notify_guard_condition(); +#ifdef _WIN32 + // free the ros_domain_id before throwing, if getenv was used on Windows + free(ros_domain_id); +#endif + + throw std::runtime_error("failed to interpret ROS_DOMAIN_ID as integral number"); + } + domain_id = static_cast(number); +#ifdef _WIN32 + free(ros_domain_id); +#endif + } + + // Create the rcl node and store it in a shared_ptr with a custom destructor. + rcl_node_t * rcl_node = new rcl_node_t(rcl_get_zero_initialized_node()); + + rcl_node_options_t options = rcl_node_get_default_options(); + // TODO(wjwwood): pass the Allocator to the options + options.domain_id = domain_id; + ret = rcl_node_init(rcl_node, node_name.c_str(), &options); + if (ret != RCL_RET_OK) { + // Finalize the interrupt guard condition. + finalize_notify_guard_condition(); + + throw_from_rcl_error(ret, "failed to initialize rcl node"); + } + + node_handle_.reset(rcl_node, [](rcl_node_t * node) -> void { + if (rcl_node_fini(node) != RCL_RET_OK) { + fprintf( + stderr, "Error in destruction of rcl node handle: %s\n", rcl_get_error_string_safe()); + } + delete node; + }); + + // Create the default callback group. + using rclcpp::callback_group::CallbackGroupType; + default_callback_group_ = create_callback_group(CallbackGroupType::MutuallyExclusive); + + // Indicate the notify_guard_condition is now valid. + notify_guard_condition_is_valid_ = true; +} + +NodeBase::~NodeBase() +{ + // Finalize the interrupt guard condition after removing self from graph listener. + { + std::lock_guard notify_condition_lock(notify_guard_condition_mutex_); + notify_guard_condition_is_valid_ = false; + if (rcl_guard_condition_fini(¬ify_guard_condition_) != RCL_RET_OK) { + fprintf(stderr, + "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe()); + } + } +} + +const char * +NodeBase::get_name() const +{ + return rcl_node_get_name(node_handle_.get()); +} + +rclcpp::context::Context::SharedPtr +NodeBase::get_context() +{ + return context_; +} + +rcl_node_t * +NodeBase::get_rcl_node_handle() +{ + return node_handle_.get(); +} + +const rcl_node_t * +NodeBase::get_rcl_node_handle() const +{ + return node_handle_.get(); +} + +std::shared_ptr +NodeBase::get_shared_rcl_node_handle() +{ + return node_handle_; +} + +std::shared_ptr +NodeBase::get_shared_rcl_node_handle() const +{ + return node_handle_; +} + +rclcpp::callback_group::CallbackGroup::SharedPtr +NodeBase::create_callback_group(rclcpp::callback_group::CallbackGroupType group_type) +{ + using rclcpp::callback_group::CallbackGroup; + using rclcpp::callback_group::CallbackGroupType; + auto group = CallbackGroup::SharedPtr(new CallbackGroup(group_type)); + callback_groups_.push_back(group); + return group; +} + +rclcpp::callback_group::CallbackGroup::SharedPtr +NodeBase::get_default_callback_group() +{ + return default_callback_group_; +} + +bool +NodeBase::callback_group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group) +{ + bool group_belongs_to_this_node = false; + for (auto & weak_group : this->callback_groups_) { + auto cur_group = weak_group.lock(); + if (cur_group && (cur_group == group)) { + group_belongs_to_this_node = true; + } + } + return group_belongs_to_this_node; +} + +const std::vector & +NodeBase::get_callback_groups() const +{ + return callback_groups_; +} + +std::atomic_bool & +NodeBase::get_associated_with_executor_atomic() +{ + return associated_with_executor_; +} + +rcl_guard_condition_t * +NodeBase::get_notify_guard_condition() +{ + std::lock_guard notify_condition_lock(notify_guard_condition_mutex_); + if (!notify_guard_condition_is_valid_) { + return nullptr; + } + return ¬ify_guard_condition_; +} + +std::unique_lock +NodeBase::acquire_notify_guard_condition_lock() const +{ + return std::unique_lock(notify_guard_condition_mutex_); +} diff --git a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp new file mode 100644 index 0000000000..693ab49b13 --- /dev/null +++ b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp @@ -0,0 +1,209 @@ +// 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/node_interfaces/node_graph.hpp" + +#include +#include + +#include "rcl/graph.h" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/event.hpp" +#include "rclcpp/graph_listener.hpp" + +using rclcpp::node_interfaces::NodeGraph; +using rclcpp::exceptions::throw_from_rcl_error; +using rclcpp::graph_listener::GraphListener; + +NodeGraph::NodeGraph(rclcpp::node_interfaces::NodeBaseInterface * node_base) +: node_base_(node_base), + graph_listener_(node_base->get_context()->get_sub_context()), + should_add_to_graph_listener_(true), + graph_users_count_(0) +{} + +NodeGraph::~NodeGraph() +{ + // Remove self from graph listener. + // Exchange with false to prevent others from trying to add this node to the + // graph listener after checking that it was not here. + if (!should_add_to_graph_listener_.exchange(false)) { + // If it was already false, then it needs to now be removed. + graph_listener_->remove_node(this); + } +} + +std::map +NodeGraph::get_topic_names_and_types() const +{ + rcl_topic_names_and_types_t topic_names_and_types = + rcl_get_zero_initialized_topic_names_and_types(); + + auto ret = rcl_get_topic_names_and_types(node_base_->get_rcl_node_handle(), + &topic_names_and_types); + if (ret != RMW_RET_OK) { + // *INDENT-OFF* + throw std::runtime_error( + std::string("could not get topic names and types: ") + rmw_get_error_string_safe()); + // *INDENT-ON* + } + + std::map topics; + for (size_t i = 0; i < topic_names_and_types.topic_count; ++i) { + topics[topic_names_and_types.topic_names[i]] = topic_names_and_types.type_names[i]; + } + + ret = rmw_destroy_topic_names_and_types(&topic_names_and_types); + if (ret != RMW_RET_OK) { + // *INDENT-OFF* + throw std::runtime_error( + std::string("could not destroy topic names and types: ") + rmw_get_error_string_safe()); + // *INDENT-ON* + } + + return topics; +} + +size_t +NodeGraph::count_publishers(const std::string & topic_name) const +{ + size_t count; + // TODO(wjwwood): use the rcl equivalent methods + auto ret = rmw_count_publishers(rcl_node_get_rmw_handle(node_base_->get_rcl_node_handle()), + topic_name.c_str(), &count); + if (ret != RMW_RET_OK) { + // *INDENT-OFF* + throw std::runtime_error( + std::string("could not count publishers: ") + rmw_get_error_string_safe()); + // *INDENT-ON* + } + return count; +} + +size_t +NodeGraph::count_subscribers(const std::string & topic_name) const +{ + size_t count; + // TODO(wjwwood): use the rcl equivalent methods + auto ret = rmw_count_subscribers(rcl_node_get_rmw_handle(node_base_->get_rcl_node_handle()), + topic_name.c_str(), &count); + if (ret != RMW_RET_OK) { + // *INDENT-OFF* + throw std::runtime_error( + std::string("could not count subscribers: ") + rmw_get_error_string_safe()); + // *INDENT-ON* + } + return count; +} + +const rcl_guard_condition_t * +NodeGraph::get_graph_guard_condition() const +{ + return rcl_node_get_graph_guard_condition(node_base_->get_rcl_node_handle()); +} + +void +NodeGraph::notify_graph_change() +{ + { + std::lock_guard graph_changed_lock(graph_mutex_); + bool bad_ptr_encountered = false; + for (auto & event_wptr : graph_events_) { + auto event_ptr = event_wptr.lock(); + if (event_ptr) { + event_ptr->set(); + } else { + bad_ptr_encountered = true; + } + } + if (bad_ptr_encountered) { + // remove invalid pointers with the erase-remove idiom + graph_events_.erase( + std::remove_if( + graph_events_.begin(), + graph_events_.end(), + [](const rclcpp::event::Event::WeakPtr & wptr) { + return wptr.expired(); + }), + graph_events_.end()); + // update graph_users_count_ + graph_users_count_.store(graph_events_.size()); + } + } + graph_cv_.notify_all(); + { + auto notify_condition_lock = node_base_->acquire_notify_guard_condition_lock(); + rcl_ret_t ret = rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()); + if (RCL_RET_OK != ret) { + throw_from_rcl_error(ret, "failed to trigger notify guard condition"); + } + } +} + +void +NodeGraph::notify_shutdown() +{ + // notify here anything that will not be woken up by ctrl-c or rclcpp::shutdown(). + graph_cv_.notify_all(); +} + +rclcpp::event::Event::SharedPtr +NodeGraph::get_graph_event() +{ + auto event = rclcpp::event::Event::make_shared(); + std::lock_guard graph_changed_lock(graph_mutex_); + // on first call, add node to graph_listener_ + if (should_add_to_graph_listener_.exchange(false)) { + graph_listener_->add_node(this); + graph_listener_->start_if_not_started(); + } + graph_events_.push_back(event); + graph_users_count_++; + return event; +} + +void +NodeGraph::wait_for_graph_change( + rclcpp::event::Event::SharedPtr event, + std::chrono::nanoseconds timeout) +{ + using rclcpp::exceptions::InvalidEventError; + using rclcpp::exceptions::EventNotRegisteredError; + if (!event) { + throw InvalidEventError(); + } + { + std::lock_guard graph_changed_lock(graph_mutex_); + bool event_in_graph_events = false; + for (const auto & event_wptr : graph_events_) { + if (event == event_wptr.lock()) { + event_in_graph_events = true; + break; + } + } + if (!event_in_graph_events) { + throw EventNotRegisteredError(); + } + } + std::unique_lock graph_lock(graph_mutex_); + graph_cv_.wait_for(graph_lock, timeout, [&event]() { + return event->check() || !rclcpp::utilities::ok(); + }); +} + +size_t +NodeGraph::count_graph_users() +{ + return graph_users_count_.load(); +} diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp new file mode 100644 index 0000000000..2d0016f316 --- /dev/null +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -0,0 +1,230 @@ +// 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/node_interfaces/node_parameters.hpp" + +#include +#include +#include +#include +#include + +#include "rcl_interfaces/srv/list_parameters.hpp" +#include "rclcpp/create_publisher.hpp" +#include "rmw/qos_profiles.h" + +using rclcpp::node_interfaces::NodeParameters; + +NodeParameters::NodeParameters( + rclcpp::node_interfaces::NodeTopicsInterface * node_topics, + bool use_intra_process) +: node_topics_(node_topics) +{ + using MessageT = rcl_interfaces::msg::ParameterEvent; + using PublisherT = rclcpp::publisher::Publisher; + using AllocatorT = std::allocator; + // TODO(wjwwood): expose this allocator through the Parameter interface. + auto allocator = std::make_shared(); + + events_publisher_ = rclcpp::create_publisher( + node_topics_, + "parameter_events", + rmw_qos_profile_parameter_events, + use_intra_process, + allocator); +} + +NodeParameters::~NodeParameters() +{} + +std::vector +NodeParameters::set_parameters( + const std::vector & parameters) +{ + std::vector results; + for (auto p : parameters) { + auto result = set_parameters_atomically({{p}}); + results.push_back(result); + } + return results; +} + +rcl_interfaces::msg::SetParametersResult +NodeParameters::set_parameters_atomically( + const std::vector & parameters) +{ + std::lock_guard lock(mutex_); + std::map tmp_map; + auto parameter_event = std::make_shared(); + + // TODO(jacquelinekay): handle parameter constraints + rcl_interfaces::msg::SetParametersResult result; + if (parameters_callback_) { + result = parameters_callback_(parameters); + } else { + result.successful = true; + } + + if (!result.successful) { + return result; + } + + for (auto p : parameters) { + if (parameters_.find(p.get_name()) == parameters_.end()) { + if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) { + parameter_event->new_parameters.push_back(p.to_parameter()); + } + } else if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) { + parameter_event->changed_parameters.push_back(p.to_parameter()); + } else { + parameter_event->deleted_parameters.push_back(p.to_parameter()); + } + tmp_map[p.get_name()] = p; + } + // std::map::insert will not overwrite elements, so we'll keep the new + // ones and add only those that already exist in the Node's internal map + tmp_map.insert(parameters_.begin(), parameters_.end()); + std::swap(tmp_map, parameters_); + + events_publisher_->publish(parameter_event); + + return result; +} + +std::vector +NodeParameters::get_parameters(const std::vector & names) const +{ + std::lock_guard lock(mutex_); + std::vector results; + + for (auto & name : names) { + if (std::any_of(parameters_.cbegin(), parameters_.cend(), + [&name](const std::pair & kv) { + return name == kv.first; + })) + { + results.push_back(parameters_.at(name)); + } + } + return results; +} + +rclcpp::parameter::ParameterVariant +NodeParameters::get_parameter(const std::string & name) const +{ + rclcpp::parameter::ParameterVariant parameter; + + if (get_parameter(name, parameter)) { + return parameter; + } else { + throw std::out_of_range("Parameter '" + name + "' not set"); + } +} + +bool +NodeParameters::get_parameter( + const std::string & name, + rclcpp::parameter::ParameterVariant & parameter) const +{ + std::lock_guard lock(mutex_); + + if (parameters_.count(name)) { + parameter = parameters_.at(name); + return true; + } else { + return false; + } +} + +std::vector +NodeParameters::describe_parameters(const std::vector & names) const +{ + std::lock_guard lock(mutex_); + std::vector results; + for (auto & kv : parameters_) { + if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) { + return name == kv.first; + })) + { + rcl_interfaces::msg::ParameterDescriptor parameter_descriptor; + parameter_descriptor.name = kv.first; + parameter_descriptor.type = kv.second.get_type(); + results.push_back(parameter_descriptor); + } + } + return results; +} + +std::vector +NodeParameters::get_parameter_types(const std::vector & names) const +{ + std::lock_guard lock(mutex_); + std::vector results; + for (auto & kv : parameters_) { + if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) { + return name == kv.first; + })) + { + results.push_back(kv.second.get_type()); + } else { + results.push_back(rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET); + } + } + return results; +} + +rcl_interfaces::msg::ListParametersResult +NodeParameters::list_parameters(const std::vector & prefixes, uint64_t depth) const +{ + std::lock_guard lock(mutex_); + rcl_interfaces::msg::ListParametersResult result; + + // TODO(esteve): define parameter separator, use "." for now + for (auto & kv : parameters_) { + if (((prefixes.size() == 0) && + ((depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) || + (static_cast(std::count(kv.first.begin(), kv.first.end(), '.')) < depth))) || + (std::any_of(prefixes.cbegin(), prefixes.cend(), [&kv, &depth](const std::string & prefix) { + if (kv.first == prefix) { + return true; + } else if (kv.first.find(prefix + ".") == 0) { + size_t length = prefix.length(); + std::string substr = kv.first.substr(length); + // Cast as unsigned integer to avoid warning + return (depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) || + (static_cast(std::count(substr.begin(), substr.end(), '.')) < depth); + } + return false; + }))) + { + result.names.push_back(kv.first); + size_t last_separator = kv.first.find_last_of('.'); + if (std::string::npos != last_separator) { + std::string prefix = kv.first.substr(0, last_separator); + if (std::find(result.prefixes.cbegin(), result.prefixes.cend(), prefix) == + result.prefixes.cend()) + { + result.prefixes.push_back(prefix); + } + } + } + } + return result; +} + +void +NodeParameters::register_param_change_callback(ParametersCallbackFunction callback) +{ + parameters_callback_ = callback; +} diff --git a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp new file mode 100644 index 0000000000..196404fd75 --- /dev/null +++ b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp @@ -0,0 +1,78 @@ +// 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/node_interfaces/node_services.hpp" + +#include + +using rclcpp::node_interfaces::NodeServices; + +NodeServices::NodeServices(rclcpp::node_interfaces::NodeBaseInterface * node_base) +: node_base_(node_base) +{} + +NodeServices::~NodeServices() +{} + +void +NodeServices::add_service( + rclcpp::service::ServiceBase::SharedPtr service_base_ptr, + rclcpp::callback_group::CallbackGroup::SharedPtr group) +{ + if (group) { + if (!node_base_->callback_group_in_node(group)) { + // TODO(jacquelinekay): use custom exception + throw std::runtime_error("Cannot create service, group not in node."); + } + group->add_service(service_base_ptr); + } else { + node_base_->get_default_callback_group()->add_service(service_base_ptr); + } + + // Notify the executor that a new service was created using the parent Node. + { + auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); + if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { + throw std::runtime_error( + std::string("Failed to notify waitset on service creation: ") + rmw_get_error_string() + ); + } + } +} + +void +NodeServices::add_client( + rclcpp::client::ClientBase::SharedPtr client_base_ptr, + rclcpp::callback_group::CallbackGroup::SharedPtr group) +{ + if (group) { + if (!node_base_->callback_group_in_node(group)) { + // TODO(jacquelinekay): use custom exception + throw std::runtime_error("Cannot create client, group not in node."); + } + group->add_client(client_base_ptr); + } else { + node_base_->get_default_callback_group()->add_client(client_base_ptr); + } + + // Notify the executor that a new client was created using the parent Node. + { + auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); + if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { + throw std::runtime_error( + std::string("Failed to notify waitset on client creation: ") + rmw_get_error_string() + ); + } + } +} diff --git a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp new file mode 100644 index 0000000000..b7176dc3f6 --- /dev/null +++ b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp @@ -0,0 +1,47 @@ +// 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/node_interfaces/node_timers.hpp" + +#include + +using rclcpp::node_interfaces::NodeTimers; + +NodeTimers::NodeTimers(rclcpp::node_interfaces::NodeBaseInterface * node_base) +: node_base_(node_base) +{} + +NodeTimers::~NodeTimers() +{} + +void +NodeTimers::add_timer( + rclcpp::timer::TimerBase::SharedPtr timer, + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) +{ + if (callback_group) { + if (!node_base_->callback_group_in_node(callback_group)) { + // TODO(jacquelinekay): use custom exception + throw std::runtime_error("Cannot create timer, group not in node."); + } + callback_group->add_timer(timer); + } else { + node_base_->get_default_callback_group()->add_timer(timer); + } + if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { + throw std::runtime_error( + std::string( + "Failed to notify waitset on timer creation: ") + rmw_get_error_string()); + } +} diff --git a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp new file mode 100644 index 0000000000..857a71cbe6 --- /dev/null +++ b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp @@ -0,0 +1,131 @@ +// 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/node_interfaces/node_topics.hpp" + +#include + +#include "rclcpp/intra_process_manager.hpp" +#include "rclcpp/exceptions.hpp" + +using rclcpp::exceptions::throw_from_rcl_error; + +using rclcpp::node_interfaces::NodeTopics; + +NodeTopics::NodeTopics(rclcpp::node_interfaces::NodeBaseInterface * node_base) +: node_base_(node_base) +{} + +NodeTopics::~NodeTopics() +{} + +rclcpp::publisher::PublisherBase::SharedPtr +NodeTopics::create_publisher( + const std::string & topic_name, + const rclcpp::PublisherFactory & publisher_factory, + rcl_publisher_options_t & publisher_options, + bool use_intra_process) +{ + // Create the MessageT specific Publisher using the factory, but store it as PublisherBase. + auto publisher = publisher_factory.create_typed_publisher( + node_base_, topic_name, publisher_options); + + // Setup intra process publishing if requested. + if (use_intra_process) { + auto context = node_base_->get_context(); + // Get the intra process manager instance for this context. + auto ipm = context->get_sub_context(); + // Register the publisher with the intra process manager. + uint64_t intra_process_publisher_id = + publisher_factory.add_publisher_to_intra_process_manager(ipm.get(), publisher); + // Create a function to be called when publisher to do the intra process publish. + auto shared_publish_callback = publisher_factory.create_shared_publish_callback(ipm); + publisher->setup_intra_process( + intra_process_publisher_id, + shared_publish_callback, + publisher_options); + } + + // Return the completed publisher. + return publisher; +} + +void +NodeTopics::add_publisher( + rclcpp::publisher::PublisherBase::SharedPtr publisher) +{ + // 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; + // 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(); + if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { + throw std::runtime_error( + std::string( + "Failed to notify waitset on publisher creation: ") + rmw_get_error_string()); + } + } +} + +rclcpp::subscription::SubscriptionBase::SharedPtr +NodeTopics::create_subscription( + const std::string & topic_name, + const rclcpp::SubscriptionFactory & subscription_factory, + rcl_subscription_options_t & subscription_options, + bool use_intra_process) +{ + auto subscription = subscription_factory.create_typed_subscription( + node_base_, topic_name, subscription_options); + + // Setup intra process publishing if requested. + if (use_intra_process) { + auto context = node_base_->get_context(); + auto intra_process_manager = + context->get_sub_context(); + subscription_factory.setup_intra_process( + intra_process_manager, subscription, subscription_options); + } + + // Return the completed subscription. + return subscription; +} + +void +NodeTopics::add_subscription( + rclcpp::subscription::SubscriptionBase::SharedPtr subscription, + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) +{ + // Assign to a group. + if (callback_group) { + if (!node_base_->callback_group_in_node(callback_group)) { + // TODO(jacquelinekay): use custom exception + throw std::runtime_error("Cannot create subscription, callback group not in node."); + } + callback_group->add_subscription(subscription); + } else { + node_base_->get_default_callback_group()->add_subscription(subscription); + } + + // Notify the executor that a new subscription was created using the parent Node. + { + auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); + if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { + throw std::runtime_error( + std::string( + "Failed to notify waitset on subscription creation: ") + rmw_get_error_string() + ); + } + } +} diff --git a/rclcpp/src/rclcpp/parameter_client.cpp b/rclcpp/src/rclcpp/parameter_client.cpp index 42d04a681c..03523eaebc 100644 --- a/rclcpp/src/rclcpp/parameter_client.cpp +++ b/rclcpp/src/rclcpp/parameter_client.cpp @@ -251,7 +251,8 @@ std::vector SyncParametersClient::get_parameters(const std::vector & parameter_names) { auto f = async_parameters_client_->get_parameters(parameter_names); - if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) == + using rclcpp::executors::spin_node_until_future_complete; + if (spin_node_until_future_complete(*executor_, node_->get_node_base_interface(), f) == rclcpp::executor::FutureReturnCode::SUCCESS) { return f.get(); @@ -274,7 +275,8 @@ SyncParametersClient::get_parameter_types(const std::vector & param { auto f = async_parameters_client_->get_parameter_types(parameter_names); - if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) == + using rclcpp::executors::spin_node_until_future_complete; + if (spin_node_until_future_complete(*executor_, node_->get_node_base_interface(), f) == rclcpp::executor::FutureReturnCode::SUCCESS) { return f.get(); @@ -288,7 +290,8 @@ SyncParametersClient::set_parameters( { auto f = async_parameters_client_->set_parameters(parameters); - if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) == + using rclcpp::executors::spin_node_until_future_complete; + if (spin_node_until_future_complete(*executor_, node_->get_node_base_interface(), f) == rclcpp::executor::FutureReturnCode::SUCCESS) { return f.get(); @@ -302,7 +305,8 @@ SyncParametersClient::set_parameters_atomically( { auto f = async_parameters_client_->set_parameters_atomically(parameters); - if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) == + using rclcpp::executors::spin_node_until_future_complete; + if (spin_node_until_future_complete(*executor_, node_->get_node_base_interface(), f) == rclcpp::executor::FutureReturnCode::SUCCESS) { return f.get(); @@ -318,7 +322,8 @@ SyncParametersClient::list_parameters( { auto f = async_parameters_client_->list_parameters(parameter_prefixes, depth); - if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) == + using rclcpp::executors::spin_node_until_future_complete; + if (spin_node_until_future_complete(*executor_, node_->get_node_base_interface(), f) == rclcpp::executor::FutureReturnCode::SUCCESS) { return f.get(); diff --git a/rclcpp/src/rclcpp/parameter_service.cpp b/rclcpp/src/rclcpp/parameter_service.cpp index 55fc354d8b..659ea2624f 100644 --- a/rclcpp/src/rclcpp/parameter_service.cpp +++ b/rclcpp/src/rclcpp/parameter_service.cpp @@ -16,6 +16,7 @@ #include #include +#include #include using rclcpp::parameter_service::ParameterService; @@ -28,7 +29,7 @@ ParameterService::ParameterService( std::weak_ptr captured_node = node_; // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) get_parameters_service_ = node_->create_service( - node_->get_name() + "__get_parameters", + std::string(node_->get_name()) + "__get_parameters", [captured_node]( const std::shared_ptr, const std::shared_ptr request, @@ -47,7 +48,7 @@ ParameterService::ParameterService( ); get_parameter_types_service_ = node_->create_service( - node_->get_name() + "__get_parameter_types", + std::string(node_->get_name()) + "__get_parameter_types", [captured_node]( const std::shared_ptr, const std::shared_ptr request, @@ -67,7 +68,7 @@ ParameterService::ParameterService( ); set_parameters_service_ = node_->create_service( - node_->get_name() + "__set_parameters", + std::string(node_->get_name()) + "__set_parameters", [captured_node]( const std::shared_ptr, const std::shared_ptr request, @@ -89,7 +90,7 @@ ParameterService::ParameterService( set_parameters_atomically_service_ = node_->create_service( - node_->get_name() + "__set_parameters_atomically", + std::string(node_->get_name()) + "__set_parameters_atomically", [captured_node]( const std::shared_ptr, const std::shared_ptr request, @@ -113,7 +114,7 @@ ParameterService::ParameterService( ); describe_parameters_service_ = node_->create_service( - node_->get_name() + "__describe_parameters", + std::string(node_->get_name()) + "__describe_parameters", [captured_node]( const std::shared_ptr, const std::shared_ptr request, @@ -130,7 +131,7 @@ ParameterService::ParameterService( ); list_parameters_service_ = node_->create_service( - node_->get_name() + "__list_parameters", + std::string(node_->get_name()) + "__list_parameters", [captured_node]( const std::shared_ptr, const std::shared_ptr request, diff --git a/rclcpp/src/rclcpp/publisher.cpp b/rclcpp/src/rclcpp/publisher.cpp index 53e948cbb4..98ca8e8e22 100644 --- a/rclcpp/src/rclcpp/publisher.cpp +++ b/rclcpp/src/rclcpp/publisher.cpp @@ -29,33 +29,72 @@ #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/allocator/allocator_deleter.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/node.hpp" using rclcpp::publisher::PublisherBase; PublisherBase::PublisherBase( - std::shared_ptr node_handle, - std::string topic, - size_t queue_size) -: node_handle_(node_handle), - topic_(topic), queue_size_(queue_size), + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const std::string & topic, + const rosidl_message_type_support_t & type_support, + const rcl_publisher_options_t & publisher_options) +: rcl_node_handle_(node_base->get_shared_rcl_node_handle()), intra_process_publisher_id_(0), store_intra_process_message_(nullptr) { + if (rcl_publisher_init( + &publisher_handle_, rcl_node_handle_.get(), &type_support, + topic.c_str(), &publisher_options) != RCL_RET_OK) + { + throw std::runtime_error( + std::string("could not create publisher: ") + + rcl_get_error_string_safe()); + } + // 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) { + throw std::runtime_error( + std::string("failed to get rmw handle: ") + rcl_get_error_string_safe()); + } + if (rmw_get_gid_for_publisher(publisher_rmw_handle, &rmw_gid_) != RMW_RET_OK) { + // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) + throw std::runtime_error( + std::string("failed to get publisher gid: ") + rmw_get_error_string_safe()); + // *INDENT-ON* + } } PublisherBase::~PublisherBase() { + if (rcl_publisher_fini(&intra_process_publisher_handle_, rcl_node_handle_.get()) != RCL_RET_OK) { + fprintf( + stderr, + "Error in destruction of intra process rcl publisher handle: %s\n", + rcl_get_error_string_safe()); + } + + if (rcl_publisher_fini(&publisher_handle_, rcl_node_handle_.get()) != RCL_RET_OK) { + fprintf( + stderr, + "Error in destruction of rcl publisher handle: %s\n", + rcl_get_error_string_safe()); + } } -const std::string & +const char * PublisherBase::get_topic_name() const { - return topic_; + return rcl_publisher_get_topic_name(&publisher_handle_); } size_t PublisherBase::get_queue_size() const { - return queue_size_; + const rcl_publisher_options_t * publisher_options = rcl_publisher_get_options(&publisher_handle_); + if (!publisher_options) { + throw std::runtime_error( + std::string("failed to get publisher options: ") + rcl_get_error_string_safe()); + } + return publisher_options->qos.depth; } const rmw_gid_t & @@ -101,10 +140,11 @@ PublisherBase::setup_intra_process( StoreMessageCallbackT callback, const rcl_publisher_options_t & intra_process_options) { + auto intra_process_topic_name = std::string(this->get_topic_name()) + "__intra"; if (rcl_publisher_init( - &intra_process_publisher_handle_, node_handle_.get(), + &intra_process_publisher_handle_, rcl_node_handle_.get(), rclcpp::type_support::get_intra_process_message_msg_type_support(), - (topic_ + "__intra").c_str(), &intra_process_options) != RCL_RET_OK) + intra_process_topic_name.c_str(), &intra_process_options) != RCL_RET_OK) { throw std::runtime_error( std::string("could not create intra process publisher: ") + diff --git a/rclcpp/src/rclcpp/subscription.cpp b/rclcpp/src/rclcpp/subscription.cpp index 5deb85a1ff..5d5ba174c9 100644 --- a/rclcpp/src/rclcpp/subscription.cpp +++ b/rclcpp/src/rclcpp/subscription.cpp @@ -26,14 +26,21 @@ using rclcpp::subscription::SubscriptionBase; SubscriptionBase::SubscriptionBase( std::shared_ptr node_handle, + const rosidl_message_type_support_t & type_support_handle, const std::string & topic_name, - bool ignore_local_publications) -: node_handle_(node_handle), - topic_name_(topic_name), - ignore_local_publications_(ignore_local_publications) + const rcl_subscription_options_t & subscription_options) +: node_handle_(node_handle) { - // To avoid unused private member warnings. - (void)ignore_local_publications_; + rcl_ret_t ret = rcl_subscription_init( + &subscription_handle_, + node_handle_.get(), + &type_support_handle, + topic_name.c_str(), + &subscription_options); + if (ret != RCL_RET_OK) { + throw std::runtime_error( + std::string("could not create subscription: ") + rcl_get_error_string_safe()); + } } SubscriptionBase::~SubscriptionBase() @@ -54,10 +61,10 @@ SubscriptionBase::~SubscriptionBase() } } -const std::string & +const char * SubscriptionBase::get_topic_name() const { - return this->topic_name_; + return rcl_subscription_get_topic_name(&subscription_handle_); } const rcl_subscription_t * diff --git a/rclcpp/src/rclcpp/utilities.cpp b/rclcpp/src/rclcpp/utilities.cpp index 9a96538f13..3322ef43b0 100644 --- a/rclcpp/src/rclcpp/utilities.cpp +++ b/rclcpp/src/rclcpp/utilities.cpp @@ -86,7 +86,7 @@ signal_handler(int signal_value) g_signal_status = signal_value; { std::lock_guard lock(g_sigint_guard_cond_handles_mutex); - for (auto const & kv : g_sigint_guard_cond_handles) { + for (auto & kv : g_sigint_guard_cond_handles) { rcl_ret_t status = rcl_trigger_guard_condition(&(kv.second)); if (status != RCL_RET_OK) { fprintf(stderr, @@ -164,7 +164,7 @@ rclcpp::utilities::shutdown() g_signal_status = SIGINT; { std::lock_guard lock(g_sigint_guard_cond_handles_mutex); - for (auto const & kv : g_sigint_guard_cond_handles) { + for (auto & kv : g_sigint_guard_cond_handles) { if (rcl_trigger_guard_condition(&(kv.second)) != RCL_RET_OK) { fprintf(stderr, "[rclcpp::error] failed to trigger sigint guard condition: %s\n", diff --git a/rclcpp/test/test_externally_defined_services.cpp b/rclcpp/test/test_externally_defined_services.cpp index 03161a2223..efd05db1f6 100644 --- a/rclcpp/test/test_externally_defined_services.cpp +++ b/rclcpp/test/test_externally_defined_services.cpp @@ -66,7 +66,8 @@ TEST_F(TestExternallyDefinedServices, extern_defined_uninitialized) { // don't initialize the service // expect fail try { - rclcpp::service::Service(node_handle->get_shared_node_handle(), + rclcpp::service::Service( + node_handle->get_node_base_interface()->get_shared_rcl_node_handle(), &service_handle, cb); } catch (const std::runtime_error &) { SUCCEED(); @@ -84,9 +85,11 @@ TEST_F(TestExternallyDefinedServices, extern_defined_initialized) { rcl_service_options_t service_options = rcl_service_get_default_options(); const rosidl_service_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT( rclcpp, srv, Mock); - if (rcl_service_init(&service_handle, node_handle->get_rcl_node_handle(), - ts, "base_node_service", &service_options) != RCL_RET_OK) - { + rcl_ret_t ret = rcl_service_init( + &service_handle, + node_handle->get_node_base_interface()->get_rcl_node_handle(), + ts, "base_node_service", &service_options); + if (ret != RCL_RET_OK) { FAIL(); return; } @@ -94,7 +97,8 @@ TEST_F(TestExternallyDefinedServices, extern_defined_initialized) { rclcpp::any_service_callback::AnyServiceCallback cb; try { - rclcpp::service::Service(node_handle->get_shared_node_handle(), + rclcpp::service::Service( + node_handle->get_node_base_interface()->get_shared_rcl_node_handle(), &service_handle, cb); } catch (const std::runtime_error &) { FAIL(); @@ -112,9 +116,11 @@ TEST_F(TestExternallyDefinedServices, extern_defined_destructor) { rcl_service_options_t service_options = rcl_service_get_default_options(); const rosidl_service_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT( rclcpp, srv, Mock); - if (rcl_service_init(&service_handle, node_handle->get_rcl_node_handle(), - ts, "base_node_service", &service_options) != RCL_RET_OK) - { + rcl_ret_t ret = rcl_service_init( + &service_handle, + node_handle->get_node_base_interface()->get_rcl_node_handle(), + ts, "base_node_service", &service_options); + if (ret != RCL_RET_OK) { FAIL(); return; } @@ -122,7 +128,8 @@ TEST_F(TestExternallyDefinedServices, extern_defined_destructor) { { // Call constructor - rclcpp::service::Service srv_cpp(node_handle->get_shared_node_handle(), + rclcpp::service::Service srv_cpp( + node_handle->get_node_base_interface()->get_shared_rcl_node_handle(), &service_handle, cb); // Call destructor } diff --git a/rclcpp/test/test_find_weak_nodes.cpp b/rclcpp/test/test_find_weak_nodes.cpp index e0850761ef..13126718cc 100644 --- a/rclcpp/test/test_find_weak_nodes.cpp +++ b/rclcpp/test/test_find_weak_nodes.cpp @@ -37,8 +37,8 @@ TEST_F(TestFindWeakNodes, allocator_strategy_with_weak_nodes) { auto existing_node = rclcpp::node::Node::make_shared("existing_node"); auto dead_node = rclcpp::node::Node::make_shared("dead_node"); rclcpp::memory_strategy::MemoryStrategy::WeakNodeVector weak_nodes; - weak_nodes.push_back(existing_node); - weak_nodes.push_back(dead_node); + weak_nodes.push_back(existing_node->get_node_base_interface()); + weak_nodes.push_back(dead_node->get_node_base_interface()); // AND // Delete dead_node, creating a dangling pointer in weak_nodes @@ -62,8 +62,8 @@ TEST_F(TestFindWeakNodes, allocator_strategy_no_weak_nodes) { auto existing_node1 = rclcpp::node::Node::make_shared("existing_node1"); auto existing_node2 = rclcpp::node::Node::make_shared("existing_node2"); rclcpp::memory_strategy::MemoryStrategy::WeakNodeVector weak_nodes; - weak_nodes.push_back(existing_node1); - weak_nodes.push_back(existing_node2); + weak_nodes.push_back(existing_node1->get_node_base_interface()); + weak_nodes.push_back(existing_node2->get_node_base_interface()); ASSERT_FALSE(weak_nodes[0].expired()); ASSERT_FALSE(weak_nodes[1].expired()); diff --git a/rclcpp/test/test_intra_process_manager.cpp b/rclcpp/test/test_intra_process_manager.cpp index bc26543842..df9b7e1e00 100644 --- a/rclcpp/test/test_intra_process_manager.cpp +++ b/rclcpp/test/test_intra_process_manager.cpp @@ -36,9 +36,9 @@ class PublisherBase PublisherBase() : mock_topic_name(""), mock_queue_size(0) {} - const std::string & get_topic_name() const + const char * get_topic_name() const { - return mock_topic_name; + return mock_topic_name.c_str(); } size_t get_queue_size() const { @@ -98,9 +98,9 @@ class SubscriptionBase SubscriptionBase() : mock_topic_name(""), mock_queue_size(0) {} - const std::string & get_topic_name() const + const char * get_topic_name() const { - return mock_topic_name; + return mock_topic_name.c_str(); } size_t get_queue_size() const {