diff --git a/rclpy/CMakeLists.txt b/rclpy/CMakeLists.txt index b1abfd719..1481e6a05 100644 --- a/rclpy/CMakeLists.txt +++ b/rclpy/CMakeLists.txt @@ -148,7 +148,6 @@ install(TARGETS rclpy_common # Split from main extension and converted to pybind11 pybind11_add_module(_rclpy_pybind11 SHARED - src/rclpy/_rclpy_action.cpp src/rclpy/_rclpy_handle.cpp src/rclpy/_rclpy_logging.cpp src/rclpy/_rclpy_pybind11.cpp @@ -164,7 +163,6 @@ pybind11_add_module(_rclpy_pybind11 SHARED src/rclpy/graph.cpp src/rclpy/guard_condition.cpp src/rclpy/handle.cpp - src/rclpy/init.cpp src/rclpy/logging.cpp src/rclpy/names.cpp src/rclpy/node.cpp diff --git a/rclpy/rclpy/action/client.py b/rclpy/rclpy/action/client.py index 22ca2d79f..d476dc131 100644 --- a/rclpy/rclpy/action/client.py +++ b/rclpy/rclpy/action/client.py @@ -149,9 +149,9 @@ def __init__( self._node = node self._action_type = action_type self._action_name = action_name - with node.handle as node_capsule: + with node.handle: self._client_handle = _rclpy.ActionClient( - node_capsule, + node.handle, action_type, action_name, goal_service_qos_profile.get_c_qos_profile(), diff --git a/rclpy/rclpy/action/graph.py b/rclpy/rclpy/action/graph.py index 828fd9a8c..362e48b97 100644 --- a/rclpy/rclpy/action/graph.py +++ b/rclpy/rclpy/action/graph.py @@ -15,7 +15,6 @@ from typing import List from typing import Tuple -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.node import Node @@ -34,9 +33,9 @@ def get_action_client_names_and_types_by_node( The first element of each tuple is the action name and the second element is a list of action types. """ - with node.handle as node_capsule: - return _rclpy.rclpy_action_get_client_names_and_types_by_node( - node_capsule, remote_node_name, remote_node_namespace) + with node.handle: + return node.handle.get_action_client_names_and_types_by_node( + remote_node_name, remote_node_namespace) def get_action_server_names_and_types_by_node( @@ -54,9 +53,9 @@ def get_action_server_names_and_types_by_node( The first element of each tuple is the action name and the second element is a list of action types. """ - with node.handle as node_capsule: - return _rclpy.rclpy_action_get_server_names_and_types_by_node( - node_capsule, remote_node_name, remote_node_namespace) + with node.handle: + return node.handle.get_action_server_names_and_types_by_node( + remote_node_name, remote_node_namespace) def get_action_names_and_types(node: Node) -> List[Tuple[str, List[str]]]: @@ -68,5 +67,5 @@ def get_action_names_and_types(node: Node) -> List[Tuple[str, List[str]]]: The first element of each tuple is the action name and the second element is a list of action types. """ - with node.handle as node_capsule: - return _rclpy.rclpy_action_get_names_and_types(node_capsule) + with node.handle: + return node.handle.get_action_names_and_types() diff --git a/rclpy/rclpy/action/server.py b/rclpy/rclpy/action/server.py index 72632dd5c..64e7aab72 100644 --- a/rclpy/rclpy/action/server.py +++ b/rclpy/rclpy/action/server.py @@ -240,9 +240,9 @@ def __init__( check_for_type_support(action_type) self._node = node self._action_type = action_type - with node.handle as node_capsule, node.get_clock().handle: + with node.handle, node.get_clock().handle: self._handle = _rclpy.ActionServer( - node_capsule, + node.handle, node.get_clock().handle, action_type, action_name, diff --git a/rclpy/rclpy/context.py b/rclpy/rclpy/context.py index d133ddb1d..f7a45ff8b 100644 --- a/rclpy/rclpy/context.py +++ b/rclpy/rclpy/context.py @@ -34,9 +34,6 @@ class Context: """ def __init__(self): - from rclpy.impl.implementation_singleton import rclpy_implementation - from .handle import Handle - self._handle = Handle(rclpy_implementation.rclpy_create_context()) self._lock = threading.Lock() self._callbacks = [] self._callbacks_lock = threading.Lock() @@ -44,7 +41,10 @@ def __init__(self): @property def handle(self): - return self._handle + return self.__context + + def destroy(self): + self.__context.destroy_when_not_in_use() def init(self, args: Optional[List[str]] = None, @@ -57,31 +57,36 @@ def init(self, :param args: List of command line arguments. """ # imported locally to avoid loading extensions on module import - from rclpy.impl.implementation_singleton import rclpy_implementation + from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy global g_logging_ref_count - with self._handle as capsule, self._lock: + with self._lock: if domain_id is not None and domain_id < 0: raise RuntimeError( 'Domain id ({}) should not be lower than zero.' .format(domain_id)) - rclpy_implementation.rclpy_init( - args if args is not None else sys.argv, - capsule, - domain_id if domain_id is not None else rclpy_implementation.RCL_DEFAULT_DOMAIN_ID) - if initialize_logging and not self._logging_initialized: - with g_logging_configure_lock: - g_logging_ref_count += 1 - if g_logging_ref_count == 1: - rclpy_implementation.rclpy_logging_configure(capsule) - self._logging_initialized = True + try: + if self.__context is not None: + raise RuntimeError + except AttributeError: + self.__context = _rclpy.Context( + args if args is not None else sys.argv, + domain_id if domain_id is not None else _rclpy.RCL_DEFAULT_DOMAIN_ID) + if initialize_logging and not self._logging_initialized: + with g_logging_configure_lock: + g_logging_ref_count += 1 + if g_logging_ref_count == 1: + _rclpy.rclpy_logging_configure(self.__context) + self._logging_initialized = True def ok(self): """Check if context hasn't been shut down.""" # imported locally to avoid loading extensions on module import - from rclpy.impl.implementation_singleton import rclpy_implementation - with self._handle as capsule, self._lock: - return rclpy_implementation.rclpy_ok(capsule) + try: + with self.__context, self._lock: + return self.__context.ok() + except AttributeError: + return False def _call_on_shutdown_callbacks(self): with self._callbacks_lock: @@ -94,19 +99,17 @@ def _call_on_shutdown_callbacks(self): def shutdown(self): """Shutdown this context.""" # imported locally to avoid loading extensions on module import - from rclpy.impl.implementation_singleton import rclpy_implementation - with self._handle as capsule, self._lock: - rclpy_implementation.rclpy_shutdown(capsule) + with self.__context, self._lock: + self.__context.shutdown() self._call_on_shutdown_callbacks() self._logging_fini() def try_shutdown(self): """Shutdown this context, if not already shutdown.""" # imported locally to avoid loading extensions on module import - from rclpy.impl.implementation_singleton import rclpy_implementation - with self._handle as capsule, self._lock: - if rclpy_implementation.rclpy_ok(capsule): - rclpy_implementation.rclpy_shutdown(capsule) + with self.__context, self._lock: + if self.__context.ok(): + self.__context.shutdown() self._call_on_shutdown_callbacks() def _remove_callback(self, weak_method): @@ -138,6 +141,5 @@ def _logging_fini(self): def get_domain_id(self): """Get domain id of context.""" - from rclpy.impl.implementation_singleton import rclpy_implementation - with self._handle as capsule, self._lock: - return rclpy_implementation.rclpy_context_get_domain_id(capsule) + with self.__context, self._lock: + return self.__context.get_domain_id() diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py index bfd0b2fe0..09a34d661 100644 --- a/rclpy/rclpy/executors.py +++ b/rclpy/rclpy/executors.py @@ -556,7 +556,8 @@ def _wait_for_ready_callbacks( except InvalidHandle: pass - context_capsule = context_stack.enter_context(self._context.handle) + context_stack.enter_context(self._context.handle) + wait_set = _rclpy.WaitSet( entity_count.num_subscriptions, entity_count.num_guard_conditions, @@ -564,7 +565,7 @@ def _wait_for_ready_callbacks( entity_count.num_clients, entity_count.num_services, entity_count.num_events, - context_capsule) + self._context.handle) wait_set.clear_entities() for sub_handle in sub_handles: diff --git a/rclpy/rclpy/guard_condition.py b/rclpy/rclpy/guard_condition.py index b8ba32664..8b83df621 100644 --- a/rclpy/rclpy/guard_condition.py +++ b/rclpy/rclpy/guard_condition.py @@ -20,8 +20,8 @@ class GuardCondition: def __init__(self, callback, callback_group, context=None): self._context = get_default_context() if context is None else context - with self._context.handle as capsule: - self.__gc = _rclpy.GuardCondition(capsule) + with self._context.handle: + self.__gc = _rclpy.GuardCondition(self._context.handle) self.callback = callback self.callback_group = callback_group # True when the callback is ready to fire but has not been "taken" by an executor diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index b8f36137b..4a090704b 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -56,7 +56,6 @@ from rclpy.executors import Executor from rclpy.expand_topic_name import expand_topic_name from rclpy.guard_condition import GuardCondition -from rclpy.handle import Handle from rclpy.handle import InvalidHandle from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.logging import get_logger @@ -170,16 +169,16 @@ def __init__( namespace = namespace or '' if not self._context.ok(): raise NotInitializedException('cannot create node') - with self._context.handle as capsule: + with self._context.handle: try: - self.__handle = Handle(_rclpy.rclpy_create_node( + self.__node = _rclpy.Node( node_name, namespace, - capsule, + self._context.handle, cli_args, use_global_arguments, enable_rosout - )) + ) except ValueError: # these will raise more specific errors if the name or namespace is bad validate_node_name(node_name) @@ -191,16 +190,16 @@ def __init__( validate_namespace(namespace) # Should not get to this point raise RuntimeError('rclpy_create_node failed for unknown reason') - with self.handle as capsule: - self._logger = get_logger(_rclpy.rclpy_get_node_logger_name(capsule)) + with self.handle: + self._logger = get_logger(self.__node.logger_name()) self.__executor_weakref = None self._parameter_event_publisher = self.create_publisher( ParameterEvent, '/parameter_events', qos_profile_parameter_events) - with self.handle as capsule: - self._parameter_overrides = _rclpy.rclpy_get_node_parameters(Parameter, capsule) + with self.handle: + self._parameter_overrides = self.__node.get_parameters(Parameter) # Combine parameters from params files with those from the node constructor and # use the set_parameters_atomically API so a parameter event is published. if parameter_overrides is not None: @@ -305,7 +304,7 @@ def handle(self): :raises: AttributeError if modified after creation. """ - return self.__handle + return self.__node @handle.setter def handle(self, value): @@ -313,13 +312,13 @@ def handle(self, value): def get_name(self) -> str: """Get the name of the node.""" - with self.handle as capsule: - return _rclpy.rclpy_get_node_name(capsule) + with self.handle: + return self.handle.get_node_name() def get_namespace(self) -> str: """Get the namespace of the node.""" - with self.handle as capsule: - return _rclpy.rclpy_get_node_namespace(capsule) + with self.handle: + return self.handle.get_namespace() def get_clock(self) -> Clock: """Get the clock used by the node.""" @@ -1190,8 +1189,8 @@ def resolve_topic_name(self, topic: str, *, only_expand: bool = False) -> str: :return: a fully qualified topic name, result of applying expansion and remapping to the given `topic`. """ - with self.handle as capsule: - return _rclpy.rclpy_resolve_name(capsule, topic, only_expand, False) + with self.handle: + return _rclpy.rclpy_resolve_name(self.handle, topic, only_expand, False) def resolve_service_name( self, service: str, *, only_expand: bool = False @@ -1204,8 +1203,8 @@ def resolve_service_name( :return: a fully qualified service name, result of applying expansion and remapping to the given `service`. """ - with self.handle as capsule: - return _rclpy.rclpy_resolve_name(capsule, service, only_expand, True) + with self.handle: + return _rclpy.rclpy_resolve_name(self.handle, service, only_expand, True) def create_publisher( self, @@ -1256,9 +1255,9 @@ def create_publisher( failed = False check_is_valid_msg_type(msg_type) try: - with self.handle as node_capsule: + with self.handle: publisher_object = _rclpy.Publisher( - node_capsule, msg_type, topic, qos_profile.get_c_qos_profile()) + self.handle, msg_type, topic, qos_profile.get_c_qos_profile()) except ValueError: failed = True if failed: @@ -1333,9 +1332,9 @@ def create_subscription( failed = False check_is_valid_msg_type(msg_type) try: - with self.handle as capsule: + with self.handle: subscription_object = _rclpy.Subscription( - capsule, msg_type, topic, qos_profile.get_c_qos_profile()) + self.handle, msg_type, topic, qos_profile.get_c_qos_profile()) except ValueError: failed = True if failed: @@ -1380,9 +1379,9 @@ def create_client( check_is_valid_srv_type(srv_type) failed = False try: - with self.handle as node_capsule: + with self.handle: client_impl = _rclpy.Client( - node_capsule, + self.handle, srv_type, srv_name, qos_profile.get_c_qos_profile()) @@ -1425,9 +1424,9 @@ def create_service( check_is_valid_srv_type(srv_type) failed = False try: - with self.handle as node_capsule: + with self.handle: service_impl = _rclpy.Service( - node_capsule, + self.handle, srv_type, srv_name, qos_profile.get_c_qos_profile()) @@ -1653,7 +1652,7 @@ def destroy_node(self) -> bool: self.destroy_timer(self.__timers[0]) while self.__guards: self.destroy_guard_condition(self.__guards[0]) - self.handle.destroy() + self.__node.destroy_when_not_in_use() self._wake_executor() def get_publisher_names_and_types_by_node( @@ -1674,9 +1673,9 @@ def get_publisher_names_and_types_by_node( :raise NodeNameNonExistentError: If the node wasn't found. :raise RuntimeError: Unexpected failure. """ - with self.handle as capsule: + with self.handle: return _rclpy.rclpy_get_publisher_names_and_types_by_node( - capsule, no_demangle, node_name, node_namespace) + self.handle, no_demangle, node_name, node_namespace) def get_subscriber_names_and_types_by_node( self, @@ -1696,9 +1695,9 @@ def get_subscriber_names_and_types_by_node( :raise NodeNameNonExistentError: If the node wasn't found. :raise RuntimeError: Unexpected failure. """ - with self.handle as capsule: + with self.handle: return _rclpy.rclpy_get_subscriber_names_and_types_by_node( - capsule, no_demangle, node_name, node_namespace) + self.handle, no_demangle, node_name, node_namespace) def get_service_names_and_types_by_node( self, @@ -1716,9 +1715,9 @@ def get_service_names_and_types_by_node( :raise NodeNameNonExistentError: If the node wasn't found. :raise RuntimeError: Unexpected failure. """ - with self.handle as capsule: + with self.handle: return _rclpy.rclpy_get_service_names_and_types_by_node( - capsule, node_name, node_namespace) + self.handle, node_name, node_namespace) def get_client_names_and_types_by_node( self, @@ -1736,9 +1735,9 @@ def get_client_names_and_types_by_node( :raise NodeNameNonExistentError: If the node wasn't found. :raise RuntimeError: Unexpected failure. """ - with self.handle as capsule: + with self.handle: return _rclpy.rclpy_get_client_names_and_types_by_node( - capsule, node_name, node_namespace) + self.handle, node_name, node_namespace) def get_topic_names_and_types(self, no_demangle: bool = False) -> List[Tuple[str, List[str]]]: """ @@ -1749,8 +1748,8 @@ def get_topic_names_and_types(self, no_demangle: bool = False) -> List[Tuple[str The first element of each tuple is the topic name and the second element is a list of topic types. """ - with self.handle as capsule: - return _rclpy.rclpy_get_topic_names_and_types(capsule, no_demangle) + with self.handle: + return _rclpy.rclpy_get_topic_names_and_types(self.handle, no_demangle) def get_service_names_and_types(self) -> List[Tuple[str, List[str]]]: """ @@ -1760,8 +1759,8 @@ def get_service_names_and_types(self) -> List[Tuple[str, List[str]]]: The first element of each tuple is the service name and the second element is a list of service types. """ - with self.handle as capsule: - return _rclpy.rclpy_get_service_names_and_types(capsule) + with self.handle: + return _rclpy.rclpy_get_service_names_and_types(self.handle) def get_node_names(self) -> List[str]: """ @@ -1769,8 +1768,8 @@ def get_node_names(self) -> List[str]: :return: List of node names. """ - with self.handle as capsule: - names_ns = _rclpy.rclpy_get_node_names_and_namespaces(capsule) + with self.handle: + names_ns = self.handle.get_node_names_and_namespaces() return [n[0] for n in names_ns] def get_node_names_and_namespaces(self) -> List[Tuple[str, str]]: @@ -1779,8 +1778,8 @@ def get_node_names_and_namespaces(self) -> List[Tuple[str, str]]: :return: List of tuples containing two strings: the node name and node namespace. """ - with self.handle as capsule: - return _rclpy.rclpy_get_node_names_and_namespaces(capsule) + with self.handle: + return self.handle.get_node_names_and_namespaces() def get_node_names_and_namespaces_with_enclaves(self) -> List[Tuple[str, str, str]]: """ @@ -1789,8 +1788,8 @@ def get_node_names_and_namespaces_with_enclaves(self) -> List[Tuple[str, str, st :return: List of tuples containing three strings: the node name, node namespace and enclave. """ - with self.handle as capsule: - return _rclpy.rclpy_get_node_names_and_namespaces_with_enclaves(capsule) + with self.handle: + return self.handle.get_node_names_and_namespaces_with_enclaves() def get_fully_qualified_name(self) -> str: """ @@ -1798,14 +1797,14 @@ def get_fully_qualified_name(self) -> str: :return: Fully qualified node name. """ - with self.handle as capsule: - return _rclpy.rclpy_node_get_fully_qualified_name(capsule) + with self.handle: + return self.handle.get_fully_qualified_name() def _count_publishers_or_subscribers(self, topic_name, func): fq_topic_name = expand_topic_name(topic_name, self.get_name(), self.get_namespace()) validate_full_topic_name(fq_topic_name) - with self.handle as node_capsule: - return func(node_capsule, fq_topic_name) + with self.handle: + return func(fq_topic_name) def count_publishers(self, topic_name: str) -> int: """ @@ -1818,7 +1817,9 @@ def count_publishers(self, topic_name: str) -> int: :param topic_name: the topic_name on which to count the number of publishers. :return: the number of publishers on the topic. """ - return self._count_publishers_or_subscribers(topic_name, _rclpy.rclpy_count_publishers) + with self.handle: + return self._count_publishers_or_subscribers( + topic_name, self.handle.get_count_publishers) def count_subscribers(self, topic_name: str) -> int: """ @@ -1831,7 +1832,9 @@ def count_subscribers(self, topic_name: str) -> int: :param topic_name: the topic_name on which to count the number of subscribers. :return: the number of subscribers on the topic. """ - return self._count_publishers_or_subscribers(topic_name, _rclpy.rclpy_count_subscribers) + with self.handle: + return self._count_publishers_or_subscribers( + topic_name, self.handle.get_count_subscribers) def _get_info_by_topic( self, @@ -1839,16 +1842,16 @@ def _get_info_by_topic( no_mangle: bool, func: Callable[[object, str, bool], List[Dict]] ) -> List[TopicEndpointInfo]: - with self.handle as node_capsule: + with self.handle: if no_mangle: fq_topic_name = topic_name else: fq_topic_name = expand_topic_name( topic_name, self.get_name(), self.get_namespace()) validate_full_topic_name(fq_topic_name) - fq_topic_name = _rclpy.rclpy_remap_topic_name(node_capsule, fq_topic_name) + fq_topic_name = _rclpy.rclpy_remap_topic_name(self.handle, fq_topic_name) - info_dicts = func(node_capsule, fq_topic_name, no_mangle) + info_dicts = func(self.handle, fq_topic_name, no_mangle) infos = [TopicEndpointInfo(**x) for x in info_dicts] return infos diff --git a/rclpy/rclpy/timer.py b/rclpy/rclpy/timer.py index 83d4fc4f8..c570524d9 100644 --- a/rclpy/rclpy/timer.py +++ b/rclpy/rclpy/timer.py @@ -25,9 +25,9 @@ class Timer: def __init__(self, callback, callback_group, timer_period_ns, clock, *, context=None): self._context = get_default_context() if context is None else context self._clock = clock - with self._clock.handle, self._context.handle as context_capsule: + with self._clock.handle, self._context.handle: self.__timer = _rclpy.Timer( - self._clock.handle, context_capsule, timer_period_ns) + self._clock.handle, self._context.handle, timer_period_ns) self.timer_period_ns = timer_period_ns self.callback = callback self.callback_group = callback_group diff --git a/rclpy/src/rclpy/_rclpy_action.cpp b/rclpy/src/rclpy/_rclpy_action.cpp deleted file mode 100644 index ae140bcef..000000000 --- a/rclpy/src/rclpy/_rclpy_action.cpp +++ /dev/null @@ -1,177 +0,0 @@ -// Copyright 2019 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include -#include - -#include -#include -#include - -#include "rclpy_common/exceptions.hpp" - -#include "rclpy_common/common.h" -#include "rclpy_common/handle.h" - -namespace py = pybind11; - - -template -T -get_pointer(py::capsule & capsule, const char * name) -{ - if (strcmp(name, capsule.name())) { - std::string error_text{"Expected capusle with name '"}; - error_text += name; - error_text += "' but got '"; - error_text += capsule.name(); - error_text += "'"; - throw py::value_error(error_text); - } - // TODO(sloretz) use get_pointer() in pybind11 2.6+ - return static_cast(capsule); -} - -/// Fetch a predefined qos_profile from rcl_action and convert it to a Python QoSProfile object. -/** - * Raises RuntimeError if the QoS profile is unknown. - * - * This function takes a string defining a rmw_qos_profile_t and returns the - * corresponding Python QoSProfile object. - * \param[in] rmw_profile String with the name of the profile to load. - * \return QoSProfile object. - */ -py::dict -rclpy_action_get_rmw_qos_profile(const char * rmw_profile) -{ - PyObject * pyqos_profile = NULL; - if (0 == strcmp(rmw_profile, "rcl_action_qos_profile_status_default")) { - pyqos_profile = rclpy_common_convert_to_qos_dict(&rcl_action_qos_profile_status_default); - } else { - std::string error_text = "Requested unknown rmw_qos_profile: "; - error_text += rmw_profile; - throw std::runtime_error(error_text); - } - return py::reinterpret_steal(pyqos_profile); -} - -py::object -rclpy_action_get_client_names_and_types_by_node( - py::capsule pynode, const char * remote_node_name, const char * remote_node_namespace) -{ - rcl_node_t * node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - rcl_names_and_types_t names_and_types = rcl_get_zero_initialized_names_and_types(); - rcl_allocator_t allocator = rcl_get_default_allocator(); - rcl_ret_t ret = rcl_action_get_client_names_and_types_by_node( - node, - &allocator, - remote_node_name, - remote_node_namespace, - &names_and_types); - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed to get action client names and type"); - } - - py::object pynames_and_types = py::reinterpret_steal( - rclpy_convert_to_py_names_and_types(&names_and_types)); - if (!rclpy_names_and_types_fini(&names_and_types)) { - throw py::error_already_set(); - } - return pynames_and_types; -} - -py::object -rclpy_action_get_server_names_and_types_by_node( - py::capsule pynode, const char * remote_node_name, const char * remote_node_namespace) -{ - rcl_node_t * node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - rcl_names_and_types_t names_and_types = rcl_get_zero_initialized_names_and_types(); - rcl_allocator_t allocator = rcl_get_default_allocator(); - rcl_ret_t ret = rcl_action_get_server_names_and_types_by_node( - node, - &allocator, - remote_node_name, - remote_node_namespace, - &names_and_types); - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed to get action server names and type"); - } - - py::object pynames_and_types = py::reinterpret_steal( - rclpy_convert_to_py_names_and_types(&names_and_types)); - if (!rclpy_names_and_types_fini(&names_and_types)) { - throw py::error_already_set(); - } - return pynames_and_types; -} - -py::object -rclpy_action_get_names_and_types(py::capsule pynode) -{ - rcl_node_t * node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - rcl_names_and_types_t names_and_types = rcl_get_zero_initialized_names_and_types(); - rcl_allocator_t allocator = rcl_get_default_allocator(); - rcl_ret_t ret = rcl_action_get_names_and_types(node, &allocator, &names_and_types); - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed to get action names and type"); - } - - py::object pynames_and_types = py::reinterpret_steal( - rclpy_convert_to_py_names_and_types(&names_and_types)); - if (!rclpy_names_and_types_fini(&names_and_types)) { - throw py::error_already_set(); - } - return pynames_and_types; -} - - -namespace rclpy -{ -void -define_action_api(py::module m) -{ - m.def( - "rclpy_action_get_rmw_qos_profile", &rclpy_action_get_rmw_qos_profile, - "Get an action RMW QoS profile."); - - m.def( - "rclpy_action_get_client_names_and_types_by_node", - &rclpy_action_get_client_names_and_types_by_node, - "Get action client names and types by node."); - m.def( - "rclpy_action_get_server_names_and_types_by_node", - &rclpy_action_get_server_names_and_types_by_node, - "Get action server names and types by node."); - m.def( - "rclpy_action_get_names_and_types", &rclpy_action_get_names_and_types, - "Get action names and types."); -} -} // namespace rclpy diff --git a/rclpy/src/rclpy/_rclpy_pybind11.cpp b/rclpy/src/rclpy/_rclpy_pybind11.cpp index 82c9e04aa..e191984b3 100644 --- a/rclpy/src/rclpy/_rclpy_pybind11.cpp +++ b/rclpy/src/rclpy/_rclpy_pybind11.cpp @@ -17,7 +17,6 @@ #include #include -#include "action_api.hpp" #include "action_client.hpp" #include "action_goal_handle.hpp" #include "action_server.hpp" @@ -29,7 +28,6 @@ #include "graph.hpp" #include "guard_condition.hpp" #include "handle_api.hpp" -#include "init.hpp" #include "logging.hpp" #include "logging_api.hpp" #include "names.hpp" @@ -111,24 +109,9 @@ PYBIND11_MODULE(_rclpy_pybind11, m) { py::register_exception( m, "InvalidHandle", PyExc_RuntimeError); - m.def( - "rclpy_init", &rclpy::init, - "Initialize RCL."); - m.def( - "rclpy_shutdown", &rclpy::shutdown, - "rclpy_shutdown."); - rclpy::define_client(m); - m.def( - "rclpy_context_get_domain_id", &rclpy::context_get_domain_id, - "Retrieves domain ID from init_options of context"); - m.def( - "rclpy_create_context", &rclpy::create_context, - "Create a capsule with an rcl_context_t instance"); - m.def( - "rclpy_ok", &rclpy::context_is_valid, - "Return true if the context is valid"); + rclpy::define_context(m); rclpy::define_duration(m); @@ -145,6 +128,9 @@ PYBIND11_MODULE(_rclpy_pybind11, m) { rclpy::define_action_client(m); rclpy::define_action_goal_handle(m); rclpy::define_action_server(m); + m.def( + "rclpy_action_get_rmw_qos_profile", &rclpy::rclpy_action_get_rmw_qos_profile, + "Get an action RMW QoS profile."); rclpy::define_guard_condition(m); rclpy::define_timer(m); rclpy::define_subscription(m); @@ -215,43 +201,9 @@ PYBIND11_MODULE(_rclpy_pybind11, m) { "rclpy_deserialize", &rclpy::deserialize, "Deserialize a ROS message."); - m.def( - "rclpy_create_node", &rclpy::create_node, - "Create a Node."); - m.def( - "rclpy_node_get_fully_qualified_name", &rclpy::get_node_fully_qualified_name, - "Get the fully qualified name of node."); - m.def( - "rclpy_get_node_logger_name", &rclpy::get_node_logger_name, - "Get the logger name associated with a node."); - m.def( - "rclpy_get_node_name", &rclpy::get_node_name, - "Get the name of a node."); - m.def( - "rclpy_get_node_namespace", &rclpy::get_node_namespace, - "Get the namespace of a node."); - m.def( - "rclpy_get_node_names_and_namespaces", &rclpy::get_node_names_and_namespaces, - "Get node names and namespaces list from graph API."); - m.def( - "rclpy_get_node_names_and_namespaces_with_enclaves", - &rclpy::get_node_names_and_namespaces_with_enclaves, - "Get node names, namespaces, and enclaves list from graph API."); - - m.def( - "rclpy_count_publishers", &rclpy::get_count_publishers, - "Count publishers for a topic."); - - m.def( - "rclpy_count_subscribers", &rclpy::get_count_subscribers, - "Count subscribers for a topic."); - + rclpy::define_node(m); rclpy::define_qos_event(m); - m.def( - "rclpy_get_node_parameters", &rclpy::get_node_parameters, - "Get the initial parameters for a node from the command line."); - m.def( "rclpy_get_rmw_implementation_identifier", &rclpy::get_rmw_implementation_identifier, @@ -277,5 +229,4 @@ PYBIND11_MODULE(_rclpy_pybind11, m) { rclpy::define_pycapsule_api(m); rclpy::define_handle_api(m); rclpy::define_logging_api(m); - rclpy::define_action_api(m); } diff --git a/rclpy/src/rclpy/action_api.hpp b/rclpy/src/rclpy/action_api.hpp deleted file mode 100644 index 6da5c7d3c..000000000 --- a/rclpy/src/rclpy/action_api.hpp +++ /dev/null @@ -1,31 +0,0 @@ -// Copyright 2021 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 RCLPY__ACTION_API_HPP_ -#define RCLPY__ACTION_API_HPP_ - -#include - -namespace py = pybind11; - -namespace rclpy -{ -/// Define methods on a module for the action API -/** - * \param[in] module a pybind11 module to add the definition to - */ -void -define_action_api(py::module module); -} // namespace rclpy -#endif // RCLPY__ACTION_API_HPP_ diff --git a/rclpy/src/rclpy/action_client.cpp b/rclpy/src/rclpy/action_client.cpp index 3fa78cc76..6fac5bf81 100644 --- a/rclpy/src/rclpy/action_client.cpp +++ b/rclpy/src/rclpy/action_client.cpp @@ -34,11 +34,11 @@ void ActionClient::destroy() { rcl_action_client_.reset(); - node_handle_.reset(); + node_.destroy(); } ActionClient::ActionClient( - py::capsule pynode, + Node & node, py::object pyaction_type, const char * action_name, const rmw_qos_profile_t & goal_service_qos, @@ -46,10 +46,8 @@ ActionClient::ActionClient( const rmw_qos_profile_t & cancel_service_qos, const rmw_qos_profile_t & feedback_topic_qos, const rmw_qos_profile_t & status_topic_qos) -: node_handle_(std::make_shared(pynode)) +: node_(node) { - auto node = node_handle_->cast("rcl_node_t"); - rosidl_action_type_support_t * ts = static_cast(rclpy_common_get_type_support( pyaction_type.ptr())); @@ -67,11 +65,10 @@ ActionClient::ActionClient( rcl_action_client_ = std::shared_ptr( new rcl_action_client_t, - [this](rcl_action_client_t * action_client) + [node](rcl_action_client_t * action_client) { - auto node = node_handle_->cast_or_warn("rcl_node_t"); - - rcl_ret_t ret = rcl_action_client_fini(action_client, node); + // Intentionally capture node by value so shared_ptr can be transferred to copies + rcl_ret_t ret = rcl_action_client_fini(action_client, node.rcl_ptr()); if (RCL_RET_OK != ret) { // Warning should use line number of the current stack frame int stack_level = 1; @@ -87,7 +84,7 @@ ActionClient::ActionClient( rcl_ret_t ret = rcl_action_client_init( rcl_action_client_.get(), - node, + node_.rcl_ptr(), ts, action_name, &action_client_ops); @@ -222,11 +219,9 @@ ActionClient::get_num_entities() bool ActionClient::is_action_server_available() { - auto node = node_handle_->cast("rcl_node_t"); - bool is_available = false; rcl_ret_t ret = rcl_action_server_is_available( - node, rcl_action_client_.get(), &is_available); + node_.rcl_ptr(), rcl_action_client_.get(), &is_available); if (RCL_RET_OK != ret) { throw rclpy::RCLError("Failed to check if action server is available"); } @@ -278,7 +273,7 @@ define_action_client(py::object module) { py::class_>(module, "ActionClient") .def( - py::init()) .def_property_readonly( diff --git a/rclpy/src/rclpy/action_client.hpp b/rclpy/src/rclpy/action_client.hpp index cfb0aa898..e8a24229c 100644 --- a/rclpy/src/rclpy/action_client.hpp +++ b/rclpy/src/rclpy/action_client.hpp @@ -22,7 +22,7 @@ #include #include "destroyable.hpp" -#include "handle.hpp" +#include "node.hpp" #include "wait_set.hpp" namespace py = pybind11; @@ -42,7 +42,7 @@ class ActionClient : public Destroyable, public std::enable_shared_from_this node_handle_; + Node node_; std::shared_ptr rcl_action_client_; }; /// Define a pybind11 wrapper for an rcl_time_point_t diff --git a/rclpy/src/rclpy/action_goal_handle.cpp b/rclpy/src/rclpy/action_goal_handle.cpp index 45904043c..771cde2a1 100644 --- a/rclpy/src/rclpy/action_goal_handle.cpp +++ b/rclpy/src/rclpy/action_goal_handle.cpp @@ -31,6 +31,7 @@ namespace rclpy { ActionGoalHandle::ActionGoalHandle( rclpy::ActionServer & action_server, py::object pygoal_info_msg) +: action_server_(action_server) { destroy_ros_message_signature * destroy_ros_message = NULL; auto goal_info_msg = static_cast( @@ -72,6 +73,7 @@ void ActionGoalHandle::destroy() { rcl_action_goal_handle_.reset(); + action_server_.destroy(); } rcl_action_goal_state_t diff --git a/rclpy/src/rclpy/action_goal_handle.hpp b/rclpy/src/rclpy/action_goal_handle.hpp index b1e1bfc94..bb9da8819 100644 --- a/rclpy/src/rclpy/action_goal_handle.hpp +++ b/rclpy/src/rclpy/action_goal_handle.hpp @@ -75,6 +75,7 @@ class ActionGoalHandle : public Destroyable, public std::enable_shared_from_this destroy() override; private: + ActionServer action_server_; std::shared_ptr rcl_action_goal_handle_; }; diff --git a/rclpy/src/rclpy/action_server.cpp b/rclpy/src/rclpy/action_server.cpp index f8639a0c8..a0c1ee302 100644 --- a/rclpy/src/rclpy/action_server.cpp +++ b/rclpy/src/rclpy/action_server.cpp @@ -34,11 +34,11 @@ void ActionServer::destroy() { rcl_action_server_.reset(); - node_handle_.reset(); + node_.destroy(); } ActionServer::ActionServer( - py::capsule pynode, + Node & node, const rclpy::Clock & rclpy_clock, py::object pyaction_type, const char * action_name, @@ -48,10 +48,8 @@ ActionServer::ActionServer( const rmw_qos_profile_t & feedback_topic_qos, const rmw_qos_profile_t & status_topic_qos, double result_timeout) -: node_handle_(std::make_shared(pynode)) +: node_(node) { - auto node = node_handle_->cast("rcl_node_t"); - rcl_clock_t * clock = rclpy_clock.rcl_ptr(); rosidl_action_type_support_t * ts = static_cast( @@ -71,11 +69,10 @@ ActionServer::ActionServer( rcl_action_server_ = std::shared_ptr( new rcl_action_server_t, - [this](rcl_action_server_t * action_server) + [node](rcl_action_server_t * action_server) { - auto node = node_handle_->cast_or_warn("rcl_node_t"); - - rcl_ret_t ret = rcl_action_server_fini(action_server, node); + // Intentionally capture node by value so shared_ptr can be transferred to copies + rcl_ret_t ret = rcl_action_server_fini(action_server, node.rcl_ptr()); if (RCL_RET_OK != ret) { // Warning should use line number of the current stack frame int stack_level = 1; @@ -91,7 +88,7 @@ ActionServer::ActionServer( rcl_ret_t ret = rcl_action_server_init( rcl_action_server_.get(), - node, + node_.rcl_ptr(), clock, ts, action_name, @@ -348,7 +345,7 @@ define_action_server(py::object module) { py::class_>(module, "ActionServer") .def( - py::init()) .def_property_readonly( diff --git a/rclpy/src/rclpy/action_server.hpp b/rclpy/src/rclpy/action_server.hpp index ae21b56b0..6482bbd06 100644 --- a/rclpy/src/rclpy/action_server.hpp +++ b/rclpy/src/rclpy/action_server.hpp @@ -23,7 +23,7 @@ #include "clock.hpp" #include "destroyable.hpp" -#include "handle.hpp" +#include "node.hpp" #include "wait_set.hpp" namespace py = pybind11; @@ -44,7 +44,7 @@ class ActionServer : public Destroyable, public std::enable_shared_from_this node_handle_; + Node node_; std::shared_ptr rcl_action_server_; }; /// Define a pybind11 wrapper for an rcl_time_point_t diff --git a/rclpy/src/rclpy/client.cpp b/rclpy/src/rclpy/client.cpp index d277dee71..4cba6cd88 100644 --- a/rclpy/src/rclpy/client.cpp +++ b/rclpy/src/rclpy/client.cpp @@ -34,15 +34,13 @@ void Client::destroy() { rcl_client_.reset(); - node_handle_.reset(); + node_.destroy(); } Client::Client( - py::capsule pynode, py::object pysrv_type, const char * service_name, py::object pyqos_profile) -: node_handle_(std::make_shared(pynode)) + Node & node, py::object pysrv_type, const char * service_name, py::object pyqos_profile) +: node_(node) { - auto node = node_handle_->cast("rcl_node_t"); - auto srv_type = static_cast( rclpy_common_get_type_support(pysrv_type.ptr())); if (!srv_type) { @@ -59,11 +57,10 @@ Client::Client( // Create a client rcl_client_ = std::shared_ptr( PythonAllocator().allocate(1), - [this](rcl_client_t * client) + [node](rcl_client_t * client) { - auto node = node_handle_->cast_or_warn("rcl_node_t"); - - rcl_ret_t ret = rcl_client_fini(client, node); + // Intentionally capture node by value so shared_ptr can be transferred to copies + rcl_ret_t ret = rcl_client_fini(client, node.rcl_ptr()); if (RCL_RET_OK != ret) { // Warning should use line number of the current stack frame int stack_level = 1; @@ -78,7 +75,7 @@ Client::Client( *rcl_client_ = rcl_get_zero_initialized_client(); rcl_ret_t ret = rcl_client_init( - rcl_client_.get(), node, srv_type, service_name, &client_ops); + rcl_client_.get(), node_.rcl_ptr(), srv_type, service_name, &client_ops); if (RCL_RET_OK != ret) { if (RCL_RET_SERVICE_NAME_INVALID == ret) { std::string error_text{"failed to create client due to invalid service name '"}; @@ -114,9 +111,8 @@ Client::send_request(py::object pyrequest) bool Client::service_server_is_available() { - auto node = node_handle_->cast("rcl_node_t"); bool is_ready; - rcl_ret_t ret = rcl_service_server_is_available(node, rcl_client_.get(), &is_ready); + rcl_ret_t ret = rcl_service_server_is_available(node_.rcl_ptr(), rcl_client_.get(), &is_ready); if (RCL_RET_OK != ret) { throw RCLError("failed to check service availability"); } @@ -156,7 +152,7 @@ void define_client(py::object module) { py::class_>(module, "Client") - .def(py::init()) + .def(py::init()) .def_property_readonly( "pointer", [](const Client & client) { return reinterpret_cast(client.rcl_ptr()); diff --git a/rclpy/src/rclpy/client.hpp b/rclpy/src/rclpy/client.hpp index 8c7621a8d..1280bd099 100644 --- a/rclpy/src/rclpy/client.hpp +++ b/rclpy/src/rclpy/client.hpp @@ -22,7 +22,7 @@ #include #include "destroyable.hpp" -#include "handle.hpp" +#include "node.hpp" namespace py = pybind11; @@ -40,12 +40,12 @@ class Client : public Destroyable, public std::enable_shared_from_this * Raises ValueError if the capsules are not the correct types * Raises RuntimeError if the client could not be created * - * \param[in] pynode Capsule pointing to the node to add the client to + * \param[in] node Node to add the client to * \param[in] pysrv_type Service module associated with the client * \param[in] service_name Python object containing the service name * \param[in] pyqos rmw_qos_profile_t object for this client */ - Client(py::capsule pynode, py::object pysrv_type, const char * service_name, py::object pyqos); + Client(Node & node, py::object pysrv_type, const char * service_name, py::object pyqos); ~Client() = default; @@ -91,8 +91,7 @@ class Client : public Destroyable, public std::enable_shared_from_this destroy() override; private: - // TODO(sloretz) replace with std::shared_ptr when rclpy::Node exists - std::shared_ptr node_handle_; + Node node_; std::shared_ptr rcl_client_; }; diff --git a/rclpy/src/rclpy/context.cpp b/rclpy/src/rclpy/context.cpp index 794c29a78..e1c155c45 100644 --- a/rclpy/src/rclpy/context.cpp +++ b/rclpy/src/rclpy/context.cpp @@ -20,103 +20,137 @@ #include #include -#include +#include +#include +#include #include "rclpy_common/handle.h" #include "rclpy_common/exceptions.hpp" #include "context.hpp" +#include "utils.hpp" namespace rclpy { -size_t -context_get_domain_id(py::capsule pycontext) +Context::Context(py::list pyargs, size_t domain_id) { - auto context = static_cast( - rclpy_handle_get_pointer_from_capsule(pycontext.ptr(), "rcl_context_t")); - if (!context) { - throw py::error_already_set(); + rcl_context_ = std::shared_ptr( + new rcl_context_t, + [](rcl_context_t * context) + { + if (NULL != context->impl) { + rcl_ret_t ret; + if (rcl_context_is_valid(context)) { + // shutdown first, if still valid + ret = rcl_shutdown(context); + if (RCL_RET_OK != ret) { + // Warning should use line number of the current stack frame + int stack_level = 1; + PyErr_WarnFormat( + PyExc_RuntimeWarning, stack_level, + "[rclpy| %s : %s ]: failed to shutdown rcl_context_t: %s", + RCUTILS_STRINGIFY(__FILE__), RCUTILS_STRINGIFY(__LINE__), rcl_get_error_string().str); + rcl_reset_error(); + } + } + ret = rcl_context_fini(context); + if (RCL_RET_OK != ret) { + // Warning should use line number of the current stack frame + int stack_level = 1; + PyErr_WarnFormat( + PyExc_RuntimeWarning, stack_level, + "[rclpy| %s : %s ]: failed to fini rcl_context_t: %s", + RCUTILS_STRINGIFY(__FILE__), RCUTILS_STRINGIFY(__LINE__), rcl_get_error_string().str); + rcl_reset_error(); + } + } + delete context; + }); + *rcl_context_ = rcl_get_zero_initialized_context(); + + // turn the arguments into an array of C-style strings + std::vector arg_c_values(pyargs.size()); + for (size_t i = 0; i < pyargs.size(); ++i) { + // CPython owns const char * memory - no need to free it + arg_c_values[i] = PyUnicode_AsUTF8(pyargs[i].ptr()); + if (!arg_c_values[i]) { + throw py::error_already_set(); + } } - size_t domain_id; - rcl_ret_t ret = rcl_context_get_domain_id(context, &domain_id); + InitOptions init_options(rcl_get_default_allocator()); + + // Set domain id + rcl_ret_t ret = rcl_init_options_set_domain_id(&init_options.rcl_options, domain_id); if (RCL_RET_OK != ret) { - throw RCLError("Failed to get domain id"); + throw RCLError("failed to set domain id to init options"); } - return domain_id; + if (arg_c_values.size() > static_cast(std::numeric_limits::max())) { + throw std::range_error("Too many cli arguments"); + } + int argc = static_cast(arg_c_values.size()); + const char ** argv = argc > 0 ? &(arg_c_values[0]) : nullptr; + ret = rcl_init(argc, argv, &init_options.rcl_options, rcl_context_.get()); + if (RCL_RET_OK != ret) { + throw RCLError("failed to initialize rcl"); + } + + throw_if_unparsed_ros_args(pyargs, rcl_context_.get()->global_arguments); } void -_rclpy_context_handle_destructor(void * p) +Context::destroy() { - auto context = static_cast(p); - if (!context) { - // Warning should use line number of the current stack frame - int stack_level = 1; - PyErr_WarnFormat( - PyExc_RuntimeWarning, stack_level, "_rclpy_context_handle_destructor failed to get pointer"); - return; - } - if (NULL != context->impl) { - rcl_ret_t ret; - if (rcl_context_is_valid(context)) { - // shutdown first, if still valid - ret = rcl_shutdown(context); - if (RCL_RET_OK != ret) { - fprintf( - stderr, - "[rclpy|" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: " - "failed to shutdown rcl_context_t (%d) during PyCapsule destructor: %s\n", - ret, - rcl_get_error_string().str); - rcl_reset_error(); - } - } - ret = rcl_context_fini(context); - if (RCL_RET_OK != ret) { - fprintf( - stderr, - "[rclpy|" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: " - "failed to fini rcl_context_t (%d) during PyCapsule destructor: %s\n", - ret, - rcl_get_error_string().str); - rcl_reset_error(); - } - } - PyMem_FREE(context); + rcl_context_.reset(); } -py::capsule -create_context() +size_t +Context::get_domain_id() { - auto context = static_cast(PyMem_Malloc(sizeof(rcl_context_t))); - if (!context) { - throw std::bad_alloc(); - } - *context = rcl_get_zero_initialized_context(); - PyObject * capsule = rclpy_create_handle_capsule( - context, "rcl_context_t", _rclpy_context_handle_destructor); - if (!capsule) { - throw py::error_already_set(); + size_t domain_id; + rcl_ret_t ret = rcl_context_get_domain_id(rcl_context_.get(), &domain_id); + if (RCL_RET_OK != ret) { + throw RCLError("Failed to get domain id"); } - return py::reinterpret_steal(capsule); + + return domain_id; } -/// Status of the the client library -/** - * \return True if rcl is running properly, False otherwise - */ bool -context_is_valid(py::capsule pycontext) +Context::ok() { - auto context = static_cast(rclpy_handle_get_pointer_from_capsule( - pycontext.ptr(), "rcl_context_t")); - if (!context) { - throw py::error_already_set(); + return rcl_context_is_valid(rcl_context_.get()); +} + +void +Context::shutdown() +{ + rcl_ret_t ret = rcl_shutdown(rcl_context_.get()); + if (RCL_RET_OK != ret) { + throw RCLError("failed to shutdown"); } +} - return rcl_context_is_valid(context); +void define_context(py::object module) +{ + py::class_>(module, "Context") + .def(py::init()) + .def_property_readonly( + "pointer", [](const Context & context) { + return reinterpret_cast(context.rcl_ptr()); + }, + "Get the address of the entity as an integer") + .def( + "get_domain_id", &Context::get_domain_id, + "Retrieves domain id from init_options of context.") + .def( + "ok", &Context::ok, + "Status of the the client library") + .def( + "shutdown", &Context::shutdown, + "Shutdown context"); } + } // namespace rclpy diff --git a/rclpy/src/rclpy/context.hpp b/rclpy/src/rclpy/context.hpp index f6f4181a1..ed00992ef 100644 --- a/rclpy/src/rclpy/context.hpp +++ b/rclpy/src/rclpy/context.hpp @@ -17,36 +17,91 @@ #include +#include + +#include +#include + +#include "destroyable.hpp" +#include "rclpy_common/exceptions.hpp" + namespace py = pybind11; namespace rclpy { -/// Retrieves domain id from init_options of context -/** - * \param[in] pycontext Capsule containing rcl_context_t - * \return domain id - */ -size_t -context_get_domain_id(py::capsule pycontext); - -/// Create a capsule with an rcl_context_t instance. -/** - * The returned context is zero-initialized for use with rclpy_init(). - * - * Raises MemoryError if allocating memory fails. - * Raises RuntimeError if creating the context fails. - * - * \return capsule with the rcl_context_t instance - */ -py::capsule -create_context(); - -/// Status of the the client library -/** - * \return True if rcl is running properly, False otherwise - */ -bool -context_is_valid(py::capsule context); +struct InitOptions +{ + explicit InitOptions(rcl_allocator_t allocator) + { + rcl_options = rcl_get_zero_initialized_init_options(); + rcl_ret_t ret = rcl_init_options_init(&rcl_options, allocator); + if (RCL_RET_OK != ret) { + throw RCLError("Failed to initialize init options"); + } + } + + ~InitOptions() + { + rcl_ret_t ret = rcl_init_options_fini(&rcl_options); + if (RCL_RET_OK != ret) { + int stack_level = 1; + PyErr_WarnFormat( + PyExc_RuntimeWarning, stack_level, + "[rclpy| %s : %s ]: failed to fini rcl_init_options_t in destructor: %s", + RCUTILS_STRINGIFY(__FILE__), RCUTILS_STRINGIFY(__LINE__), rcl_get_error_string().str); + rcl_reset_error(); + } + } + + rcl_init_options_t rcl_options; +}; + +class Context : public Destroyable, public std::enable_shared_from_this +{ +public: + /// Create a Context instance. + /** + * Raises MemoryError if allocating memory fails. + * Raises RuntimeError if creating the context fails. + * + * \param[in] pyargs List of command line arguments + * \param[in] domain_id domain id to be set in this context + */ + Context(py::list pyargs, size_t domain_id); + + /// Retrieves domain id from init_options of context + /** + * \param[in] pycontext Capsule containing rcl_context_t + * \return domain id + */ + size_t + get_domain_id(); + + /// Status of the the client library + /** + * \return True if rcl is running properly, False otherwise + */ + bool + ok(); + + void + shutdown(); + + /// Get rcl_context_t pointer + rcl_context_t * rcl_ptr() const + { + return rcl_context_.get(); + } + + /// Force an early destruction of this object + void destroy() override; + +private: + std::shared_ptr rcl_context_; +}; + +/// Define a pybind11 wrapper for an rclpy::Service +void define_context(py::object module); } // namespace rclpy #endif // RCLPY__CONTEXT_HPP_ diff --git a/rclpy/src/rclpy/destroyable.cpp b/rclpy/src/rclpy/destroyable.cpp index fc8fda849..2865983e8 100644 --- a/rclpy/src/rclpy/destroyable.cpp +++ b/rclpy/src/rclpy/destroyable.cpp @@ -19,8 +19,17 @@ #include "destroyable.hpp" #include "rclpy_common/exceptions.hpp" + namespace rclpy { +Destroyable::Destroyable(const Destroyable &) +{ + // When a destroyable is copied, it does not matter if someone asked + // to destroy the original. The copy has its own lifetime. + use_count = 0; + please_destroy_ = false; +} + void Destroyable::enter() { @@ -53,6 +62,10 @@ Destroyable::destroy() void Destroyable::destroy_when_not_in_use() { + if (please_destroy_) { + // already asked to destroy + return; + } please_destroy_ = true; if (0u == use_count) { destroy(); diff --git a/rclpy/src/rclpy/destroyable.hpp b/rclpy/src/rclpy/destroyable.hpp index 333db0916..92d6b0ad3 100644 --- a/rclpy/src/rclpy/destroyable.hpp +++ b/rclpy/src/rclpy/destroyable.hpp @@ -25,6 +25,12 @@ namespace rclpy class Destroyable { public: + /// Default constructor + Destroyable() = default; + + /// Copy constructor + Destroyable(const Destroyable & other); + /// Context manager __enter__ - block destruction void enter(); diff --git a/rclpy/src/rclpy/graph.cpp b/rclpy/src/rclpy/graph.cpp index b63c8f366..b9af8fa28 100644 --- a/rclpy/src/rclpy/graph.cpp +++ b/rclpy/src/rclpy/graph.cpp @@ -36,19 +36,13 @@ namespace rclpy py::list graph_get_publisher_names_and_types_by_node( - py::capsule pynode, bool no_demangle, + Node & node, bool no_demangle, std::string node_name, std::string node_namespace) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - rcl_names_and_types_t publisher_names_and_types = rcl_get_zero_initialized_names_and_types(); rcl_allocator_t allocator = rcl_get_default_allocator(); rcl_ret_t ret = rcl_get_publisher_names_and_types_by_node( - node, &allocator, no_demangle, node_name.c_str(), + node.rcl_ptr(), &allocator, no_demangle, node_name.c_str(), node_namespace.c_str(), &publisher_names_and_types); if (RCL_RET_OK != ret) { if (RCL_RET_NODE_NAME_NON_EXISTENT == ret) { @@ -75,19 +69,13 @@ graph_get_publisher_names_and_types_by_node( py::list graph_get_subscriber_names_and_types_by_node( - py::capsule pynode, bool no_demangle, + Node & node, bool no_demangle, std::string node_name, std::string node_namespace) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - rcl_names_and_types_t subscriber_names_and_types = rcl_get_zero_initialized_names_and_types(); rcl_allocator_t allocator = rcl_get_default_allocator(); rcl_ret_t ret = rcl_get_subscriber_names_and_types_by_node( - node, &allocator, no_demangle, node_name.c_str(), + node.rcl_ptr(), &allocator, no_demangle, node_name.c_str(), node_namespace.c_str(), &subscriber_names_and_types); if (RCL_RET_OK != ret) { if (RCL_RET_NODE_NAME_NON_EXISTENT == ret) { @@ -114,18 +102,13 @@ graph_get_subscriber_names_and_types_by_node( py::list graph_get_service_names_and_types_by_node( - py::capsule pynode, std::string node_name, std::string node_namespace) + Node & node, std::string node_name, std::string node_namespace) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - rcl_names_and_types_t service_names_and_types = rcl_get_zero_initialized_names_and_types(); rcl_allocator_t allocator = rcl_get_default_allocator(); rcl_ret_t ret = rcl_get_service_names_and_types_by_node( - node, &allocator, node_name.c_str(), node_namespace.c_str(), &service_names_and_types); + node.rcl_ptr(), &allocator, node_name.c_str(), node_namespace.c_str(), + &service_names_and_types); if (RCL_RET_OK != ret) { if (RCL_RET_NODE_NAME_NON_EXISTENT == ret) { throw NodeNameNonExistentError( @@ -151,18 +134,12 @@ graph_get_service_names_and_types_by_node( py::list graph_get_client_names_and_types_by_node( - py::capsule pynode, std::string node_name, std::string node_namespace) + Node & node, std::string node_name, std::string node_namespace) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - rcl_names_and_types_t client_names_and_types = rcl_get_zero_initialized_names_and_types(); rcl_allocator_t allocator = rcl_get_default_allocator(); rcl_ret_t ret = rcl_get_client_names_and_types_by_node( - node, &allocator, node_name.c_str(), node_namespace.c_str(), &client_names_and_types); + node.rcl_ptr(), &allocator, node_name.c_str(), node_namespace.c_str(), &client_names_and_types); if (RCL_RET_OK != ret) { if (RCL_RET_NODE_NAME_NON_EXISTENT == ret) { throw NodeNameNonExistentError( @@ -187,18 +164,12 @@ graph_get_client_names_and_types_by_node( } py::list -graph_get_topic_names_and_types(py::capsule pynode, bool no_demangle) +graph_get_topic_names_and_types(Node & node, bool no_demangle) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - rcl_names_and_types_t topic_names_and_types = rcl_get_zero_initialized_names_and_types(); rcl_allocator_t allocator = rcl_get_default_allocator(); rcl_ret_t ret = - rcl_get_topic_names_and_types(node, &allocator, no_demangle, &topic_names_and_types); + rcl_get_topic_names_and_types(node.rcl_ptr(), &allocator, no_demangle, &topic_names_and_types); if (RCL_RET_OK != ret) { throw RCLError("failed to get topic names and types"); } @@ -220,17 +191,12 @@ graph_get_topic_names_and_types(py::capsule pynode, bool no_demangle) } py::list -graph_get_service_names_and_types(py::capsule pynode) +graph_get_service_names_and_types(Node & node) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - rcl_names_and_types_t service_names_and_types = rcl_get_zero_initialized_names_and_types(); rcl_allocator_t allocator = rcl_get_default_allocator(); - rcl_ret_t ret = rcl_get_service_names_and_types(node, &allocator, &service_names_and_types); + rcl_ret_t ret = rcl_get_service_names_and_types( + node.rcl_ptr(), &allocator, &service_names_and_types); if (RCL_RET_OK != ret) { throw RCLError("failed to get service names and types"); } @@ -259,18 +225,12 @@ typedef rcl_ret_t (* rcl_get_info_by_topic_func_t)( py::list _get_info_by_topic( - py::capsule pynode, + Node & node, const char * topic_name, bool no_mangle, const char * type, rcl_get_info_by_topic_func_t rcl_get_info_by_topic) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - rcutils_allocator_t allocator = rcutils_get_default_allocator(); rcl_topic_endpoint_info_array_t info_array = rcl_get_zero_initialized_topic_endpoint_info_array(); @@ -287,7 +247,8 @@ _get_info_by_topic( } }); - rcl_ret_t ret = rcl_get_info_by_topic(node, &allocator, topic_name, no_mangle, &info_array); + rcl_ret_t ret = rcl_get_info_by_topic( + node.rcl_ptr(), &allocator, topic_name, no_mangle, &info_array); if (RCL_RET_OK != ret) { if (RCL_RET_UNSUPPORTED == ret) { throw NotImplementedError( @@ -304,19 +265,19 @@ _get_info_by_topic( py::list graph_get_publishers_info_by_topic( - py::capsule pynode, const char * topic_name, bool no_mangle) + Node & node, const char * topic_name, bool no_mangle) { return _get_info_by_topic( - pynode, topic_name, no_mangle, "publishers", + node, topic_name, no_mangle, "publishers", rcl_get_publishers_info_by_topic); } py::list graph_get_subscriptions_info_by_topic( - py::capsule pynode, const char * topic_name, bool no_mangle) + Node & node, const char * topic_name, bool no_mangle) { return _get_info_by_topic( - pynode, topic_name, no_mangle, "subscriptions", + node, topic_name, no_mangle, "subscriptions", rcl_get_subscriptions_info_by_topic); } diff --git a/rclpy/src/rclpy/graph.hpp b/rclpy/src/rclpy/graph.hpp index 05ca47604..22967626f 100644 --- a/rclpy/src/rclpy/graph.hpp +++ b/rclpy/src/rclpy/graph.hpp @@ -22,6 +22,8 @@ #include #include +#include "node.hpp" + namespace py = pybind11; namespace rclpy @@ -29,11 +31,10 @@ namespace rclpy /// Get topic names and types for which a remote node has publishers. /** - * Raises ValueError if pynode is not a node capsule * Raises NodeNameNonExistentError if remote node was not found * Raises RCLError if there is an rcl error * - * \param[in] pynode Capsule pointing to the node. + * \param[in] node Node to get publisher topic names and types. * \param[in] no_demangle Whether to demangle topic names following ROS * conventions or not. * \param[in] node_name Name of the remote node to query. @@ -45,16 +46,15 @@ namespace rclpy */ py::list graph_get_publisher_names_and_types_by_node( - py::capsule pynode, bool no_demangle, + Node & node, bool no_demangle, std::string node_name, std::string node_namespace); /// Get topic names and types for which a remote node has subscribers. /** - * Raises ValueError if pynode is not a node capsule * Raises NodeNameNonExistentError if the remote node was not found * Raises RCLError if there is an rcl error * - * \param[in] pynode Capsule pointing to the node. + * \param[in] node node to get subscriber topic names and types * \param[in] no_demangle Whether to demangle topic names following ROS * conventions or not. * \param[in] node_name Name of the remote node to query. @@ -66,16 +66,15 @@ graph_get_publisher_names_and_types_by_node( */ py::list graph_get_subscriber_names_and_types_by_node( - py::capsule pynode, bool no_demangle, + Node & node, bool no_demangle, std::string node_name, std::string node_namespace); /// Get service names and types for which a remote node has servers. /** - * Raises ValueError if pynode is not a node capsule * Raises NodeNameNonExistentError if the remote node was not found * Raises RCLError if there is an rcl error * - * \param[in] pynode Capsule pointing to the node. + * \param[in] node Node to get service topic names and types * \param[in] node_name Name of the remote node to query. * \param[in] node_namespace Namespace of the remote node to query. * \return List of tuples, where the first element of each tuple is the service @@ -85,15 +84,14 @@ graph_get_subscriber_names_and_types_by_node( */ py::list graph_get_service_names_and_types_by_node( - py::capsule pynode, std::string node_name, std::string node_namespace); + Node & node, std::string node_name, std::string node_namespace); /// Get service names and types for which a remote node has clients. /** - * Raises ValueError if pynode is not a node capsule * Raises NodeNameNonExistentError if the remote node was not found * Raises RCLError if there is an rcl error * - * \param[in] pynode Capsule pointing to the node. + * \param[in] node node to get client topic names and types * \param[in] node_name Name of the remote node to query. * \param[in] node_namespace Namespace of the remote node to query. * \return List of tuples, where the first element of each tuple is the service @@ -103,14 +101,13 @@ graph_get_service_names_and_types_by_node( */ py::list graph_get_client_names_and_types_by_node( - py::capsule pynode, std::string node_name, std::string node_namespace); + Node & node, std::string node_name, std::string node_namespace); /// Get all topic names and types in the ROS graph. /** - * Raises ValueError if pynode is not a node capsule * Raises RCLError if there is an rcl error * - * \param[in] pynode Capsule pointing to the node to query the ROS graph. + * \param[in] node node to get topic names and types * \param[in] no_demangle Whether to demangle topic names following ROS * conventions or not. * \return List of tuples, where the first element of each tuple is the topic @@ -119,21 +116,20 @@ graph_get_client_names_and_types_by_node( * \see rcl_get_topic_names_and_types */ py::list -graph_get_topic_names_and_types(py::capsule pynode, bool no_demangle); +graph_get_topic_names_and_types(Node & node, bool no_demangle); /// Get all service names and types in the ROS graph. /** - * Raises ValueError if pynode is not a node capsule * Raises RCLError if there is an rcl error * - * \param[in] pynode Capsule pointing to the node to query the ROS graph. + * \param[in] node Node to get all topic service names and types * \return List of tuples, where the first element of each tuple is the service * name (string) and the second element is a list of service types (list of * strings). * \see rcl_get_service_names_and_types */ py::list -graph_get_service_names_and_types(py::capsule pynode); +graph_get_service_names_and_types(Node & node); /// Return a list of publishers on a given topic. /** @@ -143,7 +139,7 @@ graph_get_service_names_and_types(py::capsule pynode); * Raises NotImplementedError if the call is not supported by RMW * Raises RCLError if there is an rcl error * - * \param[in] pynode Capsule pointing to the node to get the namespace from. + * \param[in] node Node to get topic publisher info * \param[in] topic_name the topic name to get the publishers for. * \param[in] no_mangle if `true`, `topic_name` needs to be a valid middleware topic name, * otherwise it should be a valid ROS topic name. @@ -151,7 +147,7 @@ graph_get_service_names_and_types(py::capsule pynode); */ py::list graph_get_publishers_info_by_topic( - py::capsule pynode, const char * topic_name, bool no_mangle); + Node & node, const char * topic_name, bool no_mangle); /// Return a list of subscriptions on a given topic. /** @@ -161,7 +157,7 @@ graph_get_publishers_info_by_topic( * Raises NotImplementedError if the call is not supported by RMW * Raises RCLError if there is an rcl error * - * \param[in] pynode Capsule pointing to the node to get the namespace from. + * \param[in] node node to get topic subscriber info * \param[in] topic_name the topic name to get the subscriptions for. * \param[in] no_mangle if `true`, `topic_name` needs to be a valid middleware topic name, * otherwise it should be a valid ROS topic name. @@ -169,7 +165,7 @@ graph_get_publishers_info_by_topic( */ py::list graph_get_subscriptions_info_by_topic( - py::capsule pynode, const char * topic_name, bool no_mangle); + Node & node, const char * topic_name, bool no_mangle); } // namespace rclpy diff --git a/rclpy/src/rclpy/guard_condition.cpp b/rclpy/src/rclpy/guard_condition.cpp index 5c0d4a302..4c41cd4c8 100644 --- a/rclpy/src/rclpy/guard_condition.cpp +++ b/rclpy/src/rclpy/guard_condition.cpp @@ -31,14 +31,9 @@ namespace rclpy { -GuardCondition::GuardCondition(py::capsule pycontext) +GuardCondition::GuardCondition(Context & context) +: context_(context) { - auto context = static_cast( - rclpy_handle_get_pointer_from_capsule(pycontext.ptr(), "rcl_context_t")); - if (!context) { - throw py::error_already_set(); - } - rcl_guard_condition_ = std::shared_ptr( new rcl_guard_condition_t, [](rcl_guard_condition_t * guard_condition) @@ -58,8 +53,9 @@ GuardCondition::GuardCondition(py::capsule pycontext) *rcl_guard_condition_ = rcl_get_zero_initialized_guard_condition(); rcl_guard_condition_options_t gc_options = rcl_guard_condition_get_default_options(); - rcl_ret_t ret = rcl_guard_condition_init(rcl_guard_condition_.get(), context, gc_options); - if (ret != RCL_RET_OK) { + rcl_ret_t ret = rcl_guard_condition_init( + rcl_guard_condition_.get(), context.rcl_ptr(), gc_options); + if (RCL_RET_OK != ret) { throw RCLError("failed to create guard_condition"); } } @@ -68,6 +64,7 @@ void GuardCondition::destroy() { rcl_guard_condition_.reset(); + context_.destroy(); } void @@ -83,7 +80,7 @@ GuardCondition::trigger_guard_condition() void define_guard_condition(py::object module) { py::class_>(module, "GuardCondition") - .def(py::init()) + .def(py::init()) .def_property_readonly( "pointer", [](const GuardCondition & guard_condition) { return reinterpret_cast(guard_condition.rcl_ptr()); diff --git a/rclpy/src/rclpy/guard_condition.hpp b/rclpy/src/rclpy/guard_condition.hpp index ef71bbb81..d77f9f0a8 100644 --- a/rclpy/src/rclpy/guard_condition.hpp +++ b/rclpy/src/rclpy/guard_condition.hpp @@ -22,6 +22,7 @@ #include +#include "context.hpp" #include "destroyable.hpp" #include "rclpy_common/exceptions.hpp" #include "rclpy_common/handle.h" @@ -38,7 +39,7 @@ class GuardCondition : public Destroyable, public std::enable_shared_from_this rcl_guard_condition_; /// Handle destructor for guard condition diff --git a/rclpy/src/rclpy/init.cpp b/rclpy/src/rclpy/init.cpp deleted file mode 100644 index 73019815c..000000000 --- a/rclpy/src/rclpy/init.cpp +++ /dev/null @@ -1,154 +0,0 @@ -// Copyright 2021 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 pybind11 before rclpy_common/handle.h includes Python.h -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "rclpy_common/handle.h" - -#include "rclpy_common/exceptions.hpp" - -#include "init.hpp" - -namespace rclpy -{ -struct InitOptions -{ - explicit InitOptions(rcl_allocator_t allocator) - { - rcl_options = rcl_get_zero_initialized_init_options(); - rcl_ret_t ret = rcl_init_options_init(&rcl_options, allocator); - if (RCL_RET_OK != ret) { - throw RCLError("Failed to initialize init options"); - } - } - - ~InitOptions() - { - rcl_ret_t ret = rcl_init_options_fini(&rcl_options); - if (RCL_RET_OK != ret) { - RCUTILS_SAFE_FWRITE_TO_STDERR( - "[rclpy|" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: " - "failed to fini rcl_init_options_t in destructor:"); - RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); - RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); - rcl_reset_error(); - } - } - - rcl_init_options_t rcl_options; -}; - -void -init(py::list pyargs, py::capsule pycontext, size_t domain_id) -{ - auto context = static_cast( - rclpy_handle_get_pointer_from_capsule(pycontext.ptr(), "rcl_context_t")); - if (!context) { - throw py::error_already_set(); - } - - // turn the arguments into an array of C-style strings - std::vector arg_c_values(pyargs.size()); - for (size_t i = 0; i < pyargs.size(); ++i) { - // CPython owns const char * memory - no need to free it - arg_c_values[i] = PyUnicode_AsUTF8(pyargs[i].ptr()); - if (!arg_c_values[i]) { - throw py::error_already_set(); - } - } - - InitOptions init_options(rcl_get_default_allocator()); - - // Set domain id - rcl_ret_t ret = rcl_init_options_set_domain_id(&init_options.rcl_options, domain_id); - if (RCL_RET_OK != ret) { - throw RCLError("failed to set domain id to init options"); - } - - if (arg_c_values.size() > static_cast(std::numeric_limits::max())) { - throw std::range_error("Too many cli arguments"); - } - int argc = static_cast(arg_c_values.size()); - const char ** argv = argc > 0 ? &(arg_c_values[0]) : nullptr; - ret = rcl_init(argc, argv, &init_options.rcl_options, context); - if (RCL_RET_OK != ret) { - throw RCLError("failed to initialize rcl"); - } - - throw_if_unparsed_ros_args(pyargs, context->global_arguments); -} - -void -shutdown(py::capsule pycontext) -{ - auto context = static_cast( - rclpy_handle_get_pointer_from_capsule(pycontext.ptr(), "rcl_context_t")); - if (!context) { - throw py::error_already_set(); - } - - rcl_ret_t ret = rcl_shutdown(context); - if (RCL_RET_OK != ret) { - throw RCLError("failed to shutdown"); - } -} - -void -throw_if_unparsed_ros_args(py::list pyargs, const rcl_arguments_t & rcl_args) -{ - int unparsed_ros_args_count = rcl_arguments_get_count_unparsed_ros(&rcl_args); - - if (unparsed_ros_args_count < 0) { - throw std::runtime_error("failed to count unparsed arguments"); - } - if (0 == unparsed_ros_args_count) { - return; - } - - rcl_allocator_t allocator = rcl_get_default_allocator(); - - int * unparsed_indices_c = nullptr; - rcl_ret_t ret = rcl_arguments_get_unparsed_ros(&rcl_args, allocator, &unparsed_indices_c); - if (RCL_RET_OK != ret) { - throw RCLError("failed to get unparsed arguments"); - } - - auto deallocator = [&](int ptr[]) {allocator.deallocate(ptr, allocator.state);}; - auto unparsed_indices = std::unique_ptr( - unparsed_indices_c, deallocator); - - py::list unparsed_args; - for (int i = 0; i < unparsed_ros_args_count; ++i) { - int index = unparsed_indices_c[i]; - if (index < 0 || static_cast(index) >= pyargs.size()) { - throw std::runtime_error("got invalid unparsed ROS arg index"); - } - unparsed_args.append(pyargs[index]); - } - - throw UnknownROSArgsError(static_cast(py::repr(unparsed_args))); -} -} // namespace rclpy diff --git a/rclpy/src/rclpy/init.hpp b/rclpy/src/rclpy/init.hpp deleted file mode 100644 index e5c4f829d..000000000 --- a/rclpy/src/rclpy/init.hpp +++ /dev/null @@ -1,50 +0,0 @@ -// Copyright 2021 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 RCLPY__INIT_HPP_ -#define RCLPY__INIT_HPP_ - -#include - -#include - -#include -#include - -namespace py = pybind11; - -namespace rclpy -{ -/// Initialize rcl with default options, ignoring parameters -/** - * Raises RCLError if rcl could not be initialized - * Raises UnknownROSArgsError if any CLI arguments could not be parsed - * Raises RuntimeError if an internal error happens - */ -void -init(py::list pyargs, py::capsule pycontext, size_t domain_id); - -/// Request shutdown of the client library -/** - * Raises RCLError if the library could not be shutdown - */ -void -shutdown(py::capsule pycontext); - -/// Throw UnparsedROSArgsError with a message saying which args are unparsed. -void -throw_if_unparsed_ros_args(py::list pyargs, const rcl_arguments_t & rcl_args); -} // namespace rclpy - -#endif // RCLPY__INIT_HPP_ diff --git a/rclpy/src/rclpy/logging.cpp b/rclpy/src/rclpy/logging.cpp index f95fb8503..f6d642149 100644 --- a/rclpy/src/rclpy/logging.cpp +++ b/rclpy/src/rclpy/logging.cpp @@ -62,18 +62,13 @@ rclpy_thread_safe_logging_output_handler( } void -logging_configure(py::object pycontext) +logging_configure(Context & context) { - auto context = static_cast( - rclpy_handle_get_pointer_from_capsule(pycontext.ptr(), "rcl_context_t")); - if (!context) { - throw py::error_already_set(); - } rcl_allocator_t allocator = rcl_get_default_allocator(); rclpy::LoggingGuard scoped_logging_guard; rcl_ret_t ret = rcl_logging_configure_with_output_handler( - &context->global_arguments, + &context.rcl_ptr()->global_arguments, &allocator, rclpy_thread_safe_logging_output_handler); if (RCL_RET_OK != ret) { diff --git a/rclpy/src/rclpy/logging.hpp b/rclpy/src/rclpy/logging.hpp index c5152df8f..f97c9cb34 100644 --- a/rclpy/src/rclpy/logging.hpp +++ b/rclpy/src/rclpy/logging.hpp @@ -19,6 +19,8 @@ #include +#include "context.hpp" + namespace py = pybind11; namespace rclpy @@ -40,7 +42,7 @@ class LoggingGuard * \param[in] pycontext a context instance to use to retrieve global CLI arguments */ void -logging_configure(py::object pycontext); +logging_configure(Context & _context); /// Finalize rcl logging void diff --git a/rclpy/src/rclpy/names.cpp b/rclpy/src/rclpy/names.cpp index 5f4fdf23a..e265af6ce 100644 --- a/rclpy/src/rclpy/names.cpp +++ b/rclpy/src/rclpy/names.cpp @@ -209,23 +209,17 @@ expand_topic_name(const char * topic, const char * node_name, const char * node_ } std::string -remap_topic_name(py::capsule pynode, const char * topic_name) +remap_topic_name(Node & node, const char * topic_name) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - // Get the node options - const rcl_node_options_t * node_options = rcl_node_get_options(node); + const rcl_node_options_t * node_options = rcl_node_get_options(node.rcl_ptr()); if (nullptr == node_options) { throw RCLError("failed to get node options"); } const rcl_arguments_t * global_args = nullptr; if (node_options->use_global_arguments) { - global_args = &(node->context->global_arguments); + global_args = &(node.rcl_ptr()->context->global_arguments); } char * output_cstr = nullptr; @@ -233,8 +227,8 @@ remap_topic_name(py::capsule pynode, const char * topic_name) &(node_options->arguments), global_args, topic_name, - rcl_node_get_name(node), - rcl_node_get_namespace(node), + rcl_node_get_name(node.rcl_ptr()), + rcl_node_get_namespace(node.rcl_ptr()), node_options->allocator, &output_cstr); if (RCL_RET_OK != ret) { @@ -256,22 +250,16 @@ remap_topic_name(py::capsule pynode, const char * topic_name) } std::string -resolve_name(py::capsule pynode, const char * topic_name, bool only_expand, bool is_service) +resolve_name(Node & node, const char * topic_name, bool only_expand, bool is_service) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - const rcl_node_options_t * node_options = rcl_node_get_options(node); + const rcl_node_options_t * node_options = rcl_node_get_options(node.rcl_ptr()); if (nullptr == node_options) { throw RCLError("failed to get node options"); } char * output_cstr = nullptr; rcl_ret_t ret = rcl_node_resolve_name( - node, + node.rcl_ptr(), topic_name, node_options->allocator, is_service, diff --git a/rclpy/src/rclpy/names.hpp b/rclpy/src/rclpy/names.hpp index e79e33440..6ae90e822 100644 --- a/rclpy/src/rclpy/names.hpp +++ b/rclpy/src/rclpy/names.hpp @@ -19,6 +19,8 @@ #include +#include "node.hpp" + namespace py = pybind11; namespace rclpy @@ -94,26 +96,26 @@ expand_topic_name(const char * topic, const char * node_name, const char * node_ * Raises ValueError if the capsule is not the correct type * Raises RCLError if an unexpected error happens * - * \param[in] pynode Capsule pointing to the node + * \param[in] Node node to remap the topic name * \param[in] topic_name topic string to be remapped * \return remapped topic name */ std::string -remap_topic_name(py::capsule pynode, const char * topic_name); +remap_topic_name(Node & node, const char * topic_name); /// Expand and remap a topic name /** * Raises ValueError if the capsule is not the correct type * Raises RCLError if an unexpected error happens * - * \param[in] pynode Capsule pointing to the node + * \param[in] node node to expand and remap a topic name * \param[in] topic_name topic string to be remapped * \param[in] only_expand when `false`, remapping rules are ignored * \param[in] is_service `true` for service names, `false` for topic names * \return expanded and remapped topic name */ std::string -resolve_name(py::capsule pynode, const char * topic_name, bool only_expand, bool is_service); +resolve_name(Node & node, const char * topic_name, bool only_expand, bool is_service); } // namespace rclpy #endif // RCLPY__NAMES_HPP_ diff --git a/rclpy/src/rclpy/node.cpp b/rclpy/src/rclpy/node.cpp index 43d233012..c41138c92 100644 --- a/rclpy/src/rclpy/node.cpp +++ b/rclpy/src/rclpy/node.cpp @@ -15,6 +15,7 @@ // Include pybind11 before rclpy_common/handle.h includes Python.h #include +#include #include #include #include @@ -34,24 +35,16 @@ #include "rclpy_common/exceptions.hpp" #include "rclpy_common/handle.h" -#include "init.hpp" #include "logging.hpp" #include "node.hpp" #include "utils.hpp" - namespace rclpy { const char * -get_node_fully_qualified_name(py::capsule pynode) +Node::get_fully_qualified_name() { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - const char * fully_qualified_node_name = rcl_node_get_fully_qualified_name(node); + const char * fully_qualified_node_name = rcl_node_get_fully_qualified_name(rcl_node_.get()); if (!fully_qualified_node_name) { throw RCLError("Fully qualified name not set"); } @@ -60,15 +53,9 @@ get_node_fully_qualified_name(py::capsule pynode) } const char * -get_node_logger_name(py::capsule pynode) +Node::logger_name() { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - const char * node_logger_name = rcl_node_get_logger_name(node); + const char * node_logger_name = rcl_node_get_logger_name(rcl_node_.get()); if (!node_logger_name) { throw RCLError("Logger name not set"); } @@ -77,15 +64,9 @@ get_node_logger_name(py::capsule pynode) } const char * -get_node_name(py::capsule pynode) +Node::get_node_name() { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - const char * node_name = rcl_node_get_name(node); + const char * node_name = rcl_node_get_name(rcl_node_.get()); if (!node_name) { throw RCLError("Node name not set"); } @@ -94,15 +75,9 @@ get_node_name(py::capsule pynode) } const char * -get_node_namespace(py::capsule pynode) +Node::get_namespace() { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - const char * node_namespace = rcl_node_get_namespace(node); + const char * node_namespace = rcl_node_get_namespace(rcl_node_.get()); if (!node_namespace) { throw RCLError("Node namespace not set"); } @@ -111,16 +86,10 @@ get_node_namespace(py::capsule pynode) } size_t -get_count_publishers(py::capsule pynode, const char * topic_name) +Node::get_count_publishers(const char * topic_name) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - size_t count = 0; - rcl_ret_t ret = rcl_count_publishers(node, topic_name, &count); + rcl_ret_t ret = rcl_count_publishers(rcl_node_.get(), topic_name, &count); if (RCL_RET_OK != ret) { throw RCLError("Error in rcl_count_publishers"); } @@ -129,16 +98,10 @@ get_count_publishers(py::capsule pynode, const char * topic_name) } size_t -get_count_subscribers(py::capsule pynode, const char * topic_name) +Node::get_count_subscribers(const char * topic_name) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - size_t count = 0; - rcl_ret_t ret = rcl_count_subscribers(node, topic_name, &count); + rcl_ret_t ret = rcl_count_subscribers(rcl_node_.get(), topic_name, &count); if (RCL_RET_OK != ret) { throw RCLError("Error in rcl_count_subscribers"); } @@ -147,14 +110,8 @@ get_count_subscribers(py::capsule pynode, const char * topic_name) } py::list -get_node_names_impl(py::capsule pynode, bool get_enclaves) +Node::get_names_impl(bool get_enclaves) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - rcl_allocator_t allocator = rcl_get_default_allocator(); rcutils_string_array_t node_names = rcutils_get_zero_initialized_string_array(); rcutils_string_array_t node_namespaces = rcutils_get_zero_initialized_string_array(); @@ -163,10 +120,10 @@ get_node_names_impl(py::capsule pynode, bool get_enclaves) rcl_ret_t ret = RCL_RET_OK; if (get_enclaves) { ret = rcl_get_node_names_with_enclaves( - node, allocator, &node_names, &node_namespaces, &enclaves); + rcl_node_.get(), allocator, &node_names, &node_namespaces, &enclaves); } else { ret = rcl_get_node_names( - node, allocator, &node_names, &node_namespaces); + rcl_node_.get(), allocator, &node_names, &node_namespaces); } if (RCL_RET_OK != ret) { throw RCLError("Failed to get node names"); @@ -176,29 +133,32 @@ get_node_names_impl(py::capsule pynode, bool get_enclaves) { rcutils_ret_t fini_ret = rcutils_string_array_fini(&node_names); if (RCUTILS_RET_OK != fini_ret) { - RCUTILS_SAFE_FWRITE_TO_STDERR( - "[rclpy|" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: " - "failed to fini node names during error handling: "); - RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); - RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + // Warning should use line number of the current stack frame + int stack_level = 1; + PyErr_WarnFormat( + PyExc_RuntimeWarning, stack_level, + "[rclpy| %s : %s ]: failed to fini node names during error handling: %s", + RCUTILS_STRINGIFY(__FILE__), RCUTILS_STRINGIFY(__LINE__), rcl_get_error_string().str); rcl_reset_error(); } fini_ret = rcutils_string_array_fini(&node_namespaces); if (RCUTILS_RET_OK != fini_ret) { - RCUTILS_SAFE_FWRITE_TO_STDERR( - "[rclpy|" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: " - "failed to fini node namespaces during error handling: "); - RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); - RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + // Warning should use line number of the current stack frame + int stack_level = 1; + PyErr_WarnFormat( + PyExc_RuntimeWarning, stack_level, + "[rclpy| %s : %s ]: failed to fini node namespaces during error handling: %s", + RCUTILS_STRINGIFY(__FILE__), RCUTILS_STRINGIFY(__LINE__), rcl_get_error_string().str); rcl_reset_error(); } fini_ret = rcutils_string_array_fini(&enclaves); if (RCUTILS_RET_OK != fini_ret) { - RCUTILS_SAFE_FWRITE_TO_STDERR( - "[rclpy|" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: " - "failed to fini enclaves string array during error handling: "); - RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); - RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + // Warning should use line number of the current stack frame + int stack_level = 1; + PyErr_WarnFormat( + PyExc_RuntimeWarning, stack_level, + "[rclpy| %s : %s ]: failed to fini enclaves string array during error handling: %s", + RCUTILS_STRINGIFY(__FILE__), RCUTILS_STRINGIFY(__LINE__), rcl_get_error_string().str); rcl_reset_error(); } }); @@ -221,15 +181,15 @@ get_node_names_impl(py::capsule pynode, bool get_enclaves) } py::list -get_node_names_and_namespaces(py::capsule pynode) +Node::get_node_names_and_namespaces() { - return get_node_names_impl(pynode, false); + return get_names_impl(false); } py::list -get_node_names_and_namespaces_with_enclaves(py::capsule pynode) +Node::get_node_names_and_namespaces_with_enclaves() { - return get_node_names_impl(pynode, true); + return get_names_impl(true); } /// Create an rclpy.parameter.Parameter from an rcl_variant_t @@ -362,22 +322,16 @@ _parse_param_overrides( } py::dict -get_node_parameters(py::object pyparameter_cls, py::capsule pynode) +Node::get_parameters(py::object pyparameter_cls) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - py::dict params_by_node_name; py::object parameter_type_cls = pyparameter_cls.attr("Type"); - const rcl_node_options_t * node_options = rcl_node_get_options(node); + const rcl_node_options_t * node_options = rcl_node_get_options(rcl_node_.get()); if (node_options->use_global_arguments) { _parse_param_overrides( - &(node->context->global_arguments), pyparameter_cls, + &(rcl_node_.get()->context->global_arguments), pyparameter_cls, parameter_type_cls, params_by_node_name); } @@ -385,7 +339,7 @@ get_node_parameters(py::object pyparameter_cls, py::capsule pynode) &(node_options->arguments), pyparameter_cls, parameter_type_cls, params_by_node_name); - const char * node_fqn = rcl_node_get_fully_qualified_name(node); + const char * node_fqn = rcl_node_get_fully_qualified_name(rcl_node_.get()); if (!node_fqn) { throw RCLError("failed to get node fully qualified name"); } @@ -409,49 +363,22 @@ get_node_parameters(py::object pyparameter_cls, py::capsule pynode) return node_params; } -/// Handle destructor for node -static -void -_rclpy_destroy_node(void * p) +void Node::destroy() { - rclpy::LoggingGuard scoped_logging_guard; - auto node = static_cast(p); - if (!node) { - // Warning should use line number of the current stack frame - int stack_level = 1; - PyErr_WarnFormat( - PyExc_RuntimeWarning, stack_level, "_rclpy_destroy_node got a NULL pointer"); - return; - } - - rcl_ret_t ret = rcl_node_fini(node); - if (RCL_RET_OK != ret) { - // Warning should use line number of the current stack frame - int stack_level = 1; - PyErr_WarnFormat( - PyExc_RuntimeWarning, stack_level, "Failed to fini node: %s", - rcl_get_error_string().str); - } - PyMem_Free(node); + rcl_node_.reset(); + context_.destroy(); } -py::capsule -create_node( +Node::Node( const char * node_name, const char * namespace_, - py::capsule pycontext, + Context & context, py::object pycli_args, bool use_global_arguments, bool enable_rosout) +: context_(context) { rcl_ret_t ret; - - auto context = static_cast( - rclpy_handle_get_pointer_from_capsule(pycontext.ptr(), "rcl_context_t")); - if (!context) { - throw py::error_already_set(); - } - rcl_arguments_t arguments = rcl_get_zero_initialized_arguments(); // turn the arguments into an array of C-style strings @@ -503,14 +430,23 @@ create_node( throw_if_unparsed_ros_args(pyargs, arguments); - auto deleter = [](rcl_node_t * ptr) {_rclpy_destroy_node(ptr);}; - auto node = std::unique_ptr( - static_cast(PyMem_Malloc(sizeof(rcl_node_t))), - deleter); - if (!node) { - throw std::bad_alloc(); - } - *node = rcl_get_zero_initialized_node(); + rcl_node_ = std::shared_ptr( + new rcl_node_t, + [](rcl_node_t * node) + { + rcl_ret_t ret = rcl_node_fini(node); + if (RCL_RET_OK != ret) { + // Warning should use line number of the current stack frame + int stack_level = 1; + PyErr_WarnFormat( + PyExc_RuntimeWarning, stack_level, "Failed to fini node: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + delete node; + }); + + *rcl_node_ = rcl_get_zero_initialized_node(); rcl_node_options_t options = rcl_node_get_default_options(); options.use_global_arguments = use_global_arguments; options.arguments = arguments; @@ -518,7 +454,8 @@ create_node( { rclpy::LoggingGuard scoped_logging_guard; - ret = rcl_node_init(node.get(), node_name, namespace_, context, &options); + ret = rcl_node_init( + rcl_node_.get(), node_name, namespace_, context.rcl_ptr(), &options); } if (RCL_RET_BAD_ALLOC == ret) { @@ -534,22 +471,107 @@ create_node( if (RCL_RET_OK != ret) { throw RCLError("error creating node"); } +} + +py::list +Node::get_action_client_names_and_types_by_node( + const char * remote_node_name, const char * remote_node_namespace) +{ + rcl_names_and_types_t names_and_types = rcl_get_zero_initialized_names_and_types(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_action_get_client_names_and_types_by_node( + rcl_node_.get(), + &allocator, + remote_node_name, + remote_node_namespace, + &names_and_types); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to get action client names and type"); + } + + return convert_to_py_names_and_types(&names_and_types); +} - PyObject * pynode_c = rclpy_create_handle_capsule(node.get(), "rcl_node_t", _rclpy_destroy_node); - if (!pynode_c) { - throw py::error_already_set(); +py::list +Node::get_action_server_names_and_types_by_node( + const char * remote_node_name, const char * remote_node_namespace) +{ + rcl_names_and_types_t names_and_types = rcl_get_zero_initialized_names_and_types(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_action_get_server_names_and_types_by_node( + rcl_node_.get(), + &allocator, + remote_node_name, + remote_node_namespace, + &names_and_types); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to get action server names and type"); } - auto pynode = py::reinterpret_steal(pynode_c); - // pynode now owns the rcl_node_t - node.release(); - - auto node_handle = static_cast(pynode); - auto context_handle = static_cast(pycontext); - _rclpy_handle_add_dependency(node_handle, context_handle); - if (PyErr_Occurred()) { - throw py::error_already_set(); + + return convert_to_py_names_and_types(&names_and_types); +} + +py::list +Node::get_action_names_and_types() +{ + rcl_names_and_types_t names_and_types = rcl_get_zero_initialized_names_and_types(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_action_get_names_and_types(rcl_node_.get(), &allocator, &names_and_types); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to get action names and type"); } - return pynode; + return convert_to_py_names_and_types(&names_and_types); +} + +void +define_node(py::object module) +{ + py::class_>(module, "Node") + .def(py::init()) + .def_property_readonly( + "pointer", [](const Node & node) { + return reinterpret_cast(node.rcl_ptr()); + }, + "Get the address of the entity as an integer") + .def( + "get_fully_qualified_name", &Node::get_fully_qualified_name, + "Get the fully qualified name of the node.") + .def( + "logger_name", &Node::logger_name, + "Get the name of the logger associated with a node.") + .def( + "get_node_name", &Node::get_node_name, + "Get the name of a node.") + .def( + "get_namespace", &Node::get_namespace, + "Get the namespace of a node.") + .def( + "get_count_publishers", &Node::get_count_publishers, + "Returns the count of all the publishers known for that topic in the entire ROS graph.") + .def( + "get_count_subscribers", &Node::get_count_subscribers, + "Returns the count of all the subscribers known for that topic in the entire ROS graph.") + .def( + "get_node_names_and_namespaces", &Node::get_node_names_and_namespaces, + "Get the list of nodes discovered by the provided node") + .def( + "get_node_names_and_namespaces_with_enclaves", + &Node::get_node_names_and_namespaces_with_enclaves, + "Get the list of nodes discovered by the provided node, with their respective enclaves.") + .def( + "get_action_client_names_and_types_by_node", + &Node::get_action_client_names_and_types_by_node, + "Get action client names and types by node.") + .def( + "get_action_server_names_and_types_by_node", + &Node::get_action_server_names_and_types_by_node, + "Get action server names and types by node.") + .def( + "get_action_names_and_types", &Node::get_action_names_and_types, + "Get action names and types.") + .def( + "get_parameters", &Node::get_parameters, + "Get a list of parameters for the current node"); } } // namespace rclpy diff --git a/rclpy/src/rclpy/node.hpp b/rclpy/src/rclpy/node.hpp index e09a463b3..fb6eeb289 100644 --- a/rclpy/src/rclpy/node.hpp +++ b/rclpy/src/rclpy/node.hpp @@ -17,157 +17,189 @@ #include +#include // rcl_names_and_types_t + +#include + +#include "context.hpp" +#include "destroyable.hpp" + namespace py = pybind11; namespace rclpy { -/// Get the fully qualified name of the node. -/** - * Raises ValueError if pynode is not a node capsule - * Raises RCLError if name is not set - * - * \param[in] pynode Capsule pointing to the node - * \return String containing the fully qualified name of the node - */ -const char * -get_node_fully_qualified_name(py::capsule pynode); - -/// Get the name of the logger associated with a node. -/** - * Raises ValueError if pynode is not a node capsule - * Raises RCLError if logger name is not set - * - * \param[in] pynode Capsule pointing to the node to get the logger name of - * \return logger_name - */ -const char * -get_node_logger_name(py::capsule pynode); - -/// Get the name of a node. -/** - * Raises ValueError if pynode is not a node capsule - * Raises RCLError if name is not set - * - * \param[in] pynode Capsule pointing to the node to get the name from - * \return name of the node - */ -const char * -get_node_name(py::capsule pynode); - -/// Get the namespace of a node. -/** - * Raises ValueError if pynode is not a node capsule - * Raises RCLError if namespace is not set - * - * \param[in] pynode Capsule pointing to the node to get the namespace from - * \return namespace - */ -const char * -get_node_namespace(py::capsule pynode); - -/// Returns the count of all the publishers known for that topic in the entire ROS graph. -/* - * Raises ValueError if pynode is not a node capsule - * Raises RCLError if an error occurs in rcl - * - * \param[in] pynode pynode Capsule pointing to a node - * \param[in] topic_name Name of the topic to count the number of publishers - * \return the count of all the publishers known for that topic in the entire ROS graph. - */ -size_t -get_count_publishers(py::capsule pynode, const char * topic_name); - -/// Returns the count of all the subscribers known for that topic in the entire ROS graph -/* - * - * Raises ValueError if pynode is not a node capsule - * Raises RCLError if an error occurs in rcl - * - * \param[in] pynode pynode Capsule pointing to a node - * \param[in] topic_name Name of the topic to count the number of subscribers - * \return the count of all the subscribers known for that topic in the entire ROS graph - */ -size_t -get_count_subscribers(py::capsule pynode, const char * topic_name); - -/// Get the list of nodes discovered by the provided node -/** - * Raises ValueError if pynode is not a node capsule - * Raises RCLError if the names are unavailable. - * - * \param[in] module the Python module this function is part of - * \param[in] pynode Capsule pointing to the node - * \param[in] get_enclaves specifies if the output includes the enclaves names - * or not - * \return Python list of tuples, containing: - * node name, node namespace, and - * enclave if `get_enclaves` is true. - */ -py::list -get_node_names_impl(py::capsule pynode, bool get_enclaves); - -/// Get the list of nodes discovered by the provided node -/** - * Raises ValueError if pynode is not a node capsule - * Raises RCLError if the names are unavailable. - * - * \param[in] pynode Capsule pointing to the node - * \return Python list of tuples where each tuple contains the two strings: - * the node name and node namespace - */ -py::list -get_node_names_and_namespaces(py::capsule pynode); - -/// Get the list of nodes discovered by the provided node, with their respective enclaves. -/** - * Raises ValueError if pynode is not a node capsule - * Raises RCLError if the names are unavailable. - * - * \param[in] pynode Capsule pointing to the node - * \return Python list of tuples where each tuple contains three strings: - * node name, node namespace, and enclave. - */ -py::list -get_node_names_and_namespaces_with_enclaves(py::capsule pynode); - -/// Get a list of parameters for the current node -/** - * Raises ValueError if the argument is not a node handle - * Raises RCLError if any rcl function call fails - * Raises AttributeError if pyparameter_cls doesn't have a 'Type' attribute - * Raises RuntimeError if assumptions about rcl structures are violated - * - * \param[in] pyparameter_cls The rclpy.parameter.Parameter class object. - * \param[in] node_capsule Capsule pointing to the node handle - * \return A dict mapping parameter names to rclpy.parameter.Parameter (may be empty). - */ -py::dict -get_node_parameters(py::object pyparameter_cls, py::capsule pynode); - -/// Create a node -/** - * Raises ValueError if the node name or namespace is invalid - * Raises RCLError if the node could not be initialized for an unexpected reason - * Raises RCLInvalidROSArgsError if the given CLI arguments could not be parsed - * Raises UnknownROSArgsError if there are unknown CLI arguments given - * Raises MemoryError if memory could not be allocated for the node - * - * \param[in] node_name name of the node to be created - * \param[in] namespace namespace for the node - * \param[in] pycontext Capsule for an rcl_context_t - * \param[in] pycli_args a sequence of command line arguments for just this node, or None - * \param[in] use_global_arguments if true then the node will also use cli arguments on pycontext - * \param[in] enable rosout if true then enable rosout logging - * \return Capsule of the pointer to the created rcl_node_t * structure - */ -py::capsule -create_node( - const char * node_name, - const char * namespace_, - py::capsule pycontext, - py::object pycli_args, - bool use_global_arguments, - bool enable_rosout); -} // namespace rclpy +class Node : public Destroyable, public std::enable_shared_from_this +{ +public: + /// Create a node + /** + * Raises ValueError if the node name or namespace is invalid + * Raises RCLError if the node could not be initialized for an unexpected reason + * Raises RCLInvalidROSArgsError if the given CLI arguments could not be parsed + * Raises UnknownROSArgsError if there are unknown CLI arguments given + * Raises MemoryError if memory could not be allocated for the node + * + * \param[in] node_name name of the node to be created + * \param[in] namespace namespace for the node + * \param[in] context Context + * \param[in] pycli_args a sequence of command line arguments for just this node, or None + * \param[in] use_global_arguments if true then the node will also use cli arguments on context + * \param[in] enable rosout if true then enable rosout logging + */ + Node( + const char * node_name, + const char * namespace_, + Context & context, + py::object pycli_args, + bool use_global_arguments, + bool enable_rosout); + + /// Get the fully qualified name of the node. + /** + * Raises RCLError if name is not set + * + * \return String containing the fully qualified name of the node + */ + const char * + get_fully_qualified_name(); + + /// Get the name of the logger associated with a node. + /** + * Raises RCLError if logger name is not set + * + * \return logger_name + */ + const char * + logger_name(); + + /// Get the name of a node. + /** + * Raises RCLError if name is not set + * + * \return name of the node + */ + const char * + get_node_name(); + + /// Get the namespace of a node. + /** + * Raises RCLError if namespace is not set + * + * \return namespace + */ + const char * + get_namespace(); + + /// Returns the count of all the publishers known for that topic in the entire ROS graph. + /* + * Raises RCLError if an error occurs in rcl + * + * \param[in] topic_name Name of the topic to count the number of publishers + * \return the count of all the publishers known for that topic in the entire ROS graph. + */ + size_t + get_count_publishers(const char * topic_name); + + /// Returns the count of all the subscribers known for that topic in the entire ROS graph + /* + * + * Raises RCLError if an error occurs in rcl + * + * \param[in] topic_name Name of the topic to count the number of subscribers + * \return the count of all the subscribers known for that topic in the entire ROS graph + */ + size_t + get_count_subscribers(const char * topic_name); + /// Get the list of nodes discovered by the provided node + /** + * Raises RCLError if the names are unavailable. + * + * \return Python list of tuples where each tuple contains the two strings: + * the node name and node namespace + */ + py::list + get_node_names_and_namespaces(); + + /// Get the list of nodes discovered by the provided node, with their respective enclaves. + /** + * Raises RCLError if the names are unavailable. + * + * \return Python list of tuples where each tuple contains three strings: + * node name, node namespace, and enclave. + */ + py::list + get_node_names_and_namespaces_with_enclaves(); + + /// Get a list of parameters for the current node + /** + * Raises RCLError if any rcl function call fails + * Raises AttributeError if pyparameter_cls doesn't have a 'Type' attribute + * Raises RuntimeError if assumptions about rcl structures are violated + * + * \param[in] pyparameter_cls The rclpy.parameter.Parameter class object. + * \return A dict mapping parameter names to rclpy.parameter.Parameter (may be empty). + */ + py::dict + get_parameters(py::object pyparameter_cls); + + /// Get action client names and types by node. + /* + * \param[in] remote_node_name the node name of the actions to return + * \param[in] remote_node_namespace the node namespace of the actions to return + * \return list of action client names and their types + */ + py::list + get_action_client_names_and_types_by_node( + const char * remote_node_name, const char * remote_node_namespace); + + /// Get action server names and types by node. + /* + * \param[in] remote_node_name the node name of the actions to return + * \param[in] remote_node_namespace the node namespace of the actions to return + * \return list of action server names and their types + */ + py::list + get_action_server_names_and_types_by_node( + const char * remote_node_name, const char * remote_node_namespace); + + /// Get action names and types. + /* + * \return list of action names and types + */ + py::list + get_action_names_and_types(); + + /// Get rcl_node_t pointer + rcl_node_t * + rcl_ptr() const + { + return rcl_node_.get(); + } + + /// Force an early destruction of this object + void + destroy() override; + +private: + /// Get the list of nodes discovered by the provided node + /** + * Raises RCLError if the names are unavailable. + * + * \param[in] get_enclaves specifies if the output includes the enclaves names + * or not + * \return Python list of tuples, containing: + * node name, node namespace, and + * enclave if `get_enclaves` is true. + */ + py::list + get_names_impl(bool get_enclaves); + + Context context_; + std::shared_ptr rcl_node_; +}; + +void +define_node(py::object module); +} // namespace rclpy #endif // RCLPY__NODE_HPP_ diff --git a/rclpy/src/rclpy/publisher.cpp b/rclpy/src/rclpy/publisher.cpp index c9caf916c..467d8f56f 100644 --- a/rclpy/src/rclpy/publisher.cpp +++ b/rclpy/src/rclpy/publisher.cpp @@ -21,7 +21,6 @@ #include #include "rclpy_common/common.h" -#include "rclpy_common/handle.h" #include "rclpy_common/exceptions.hpp" @@ -30,12 +29,10 @@ namespace rclpy { Publisher::Publisher( - py::capsule pynode, py::object pymsg_type, std::string topic, + Node & node, py::object pymsg_type, std::string topic, py::object pyqos_profile) -: node_handle_(std::make_shared(pynode)) +: node_(node) { - auto node = node_handle_->cast("rcl_node_t"); - auto msg_type = static_cast( rclpy_common_get_type_support(pymsg_type.ptr())); if (!msg_type) { @@ -50,11 +47,10 @@ Publisher::Publisher( rcl_publisher_ = std::shared_ptr( new rcl_publisher_t, - [this](rcl_publisher_t * publisher) + [node](rcl_publisher_t * publisher) { - auto node = node_handle_->cast_or_warn("rcl_node_t"); - - rcl_ret_t ret = rcl_publisher_fini(publisher, node); + // Intentionally capturing node by value so shared_ptr can be transfered to copies + rcl_ret_t ret = rcl_publisher_fini(publisher, node.rcl_ptr()); if (RCL_RET_OK != ret) { // Warning should use line number of the current stack frame int stack_level = 1; @@ -69,7 +65,7 @@ Publisher::Publisher( *rcl_publisher_ = rcl_get_zero_initialized_publisher(); rcl_ret_t ret = rcl_publisher_init( - rcl_publisher_.get(), node, msg_type, + rcl_publisher_.get(), node_.rcl_ptr(), msg_type, topic.c_str(), &publisher_ops); if (RCL_RET_OK != ret) { if (RCL_RET_TOPIC_NAME_INVALID == ret) { @@ -85,15 +81,13 @@ Publisher::Publisher( void Publisher::destroy() { rcl_publisher_.reset(); - node_handle_.reset(); + node_.destroy(); } const char * Publisher::get_logger_name() { - auto node = node_handle_->cast("rcl_node_t"); - - const char * node_logger_name = rcl_node_get_logger_name(node); + const char * node_logger_name = rcl_node_get_logger_name(node_.rcl_ptr()); if (!node_logger_name) { throw RCLError("Node logger name not set"); } @@ -158,7 +152,7 @@ void define_publisher(py::object module) { py::class_>(module, "Publisher") - .def(py::init()) + .def(py::init()) .def_property_readonly( "pointer", [](const Publisher & publisher) { return reinterpret_cast(publisher.rcl_ptr()); diff --git a/rclpy/src/rclpy/publisher.hpp b/rclpy/src/rclpy/publisher.hpp index ce91b9d4d..b40b9a2d6 100644 --- a/rclpy/src/rclpy/publisher.hpp +++ b/rclpy/src/rclpy/publisher.hpp @@ -24,7 +24,7 @@ #include #include "destroyable.hpp" -#include "handle.hpp" +#include "node.hpp" namespace py = pybind11; @@ -44,13 +44,13 @@ class Publisher : public Destroyable, public std::enable_shared_from_this node_handle_; + Node node_; std::shared_ptr rcl_publisher_; }; /// Define a pybind11 wrapper for an rclpy::Service diff --git a/rclpy/src/rclpy/qos_event.cpp b/rclpy/src/rclpy/qos_event.cpp index d22d24e06..10abd191f 100644 --- a/rclpy/src/rclpy/qos_event.cpp +++ b/rclpy/src/rclpy/qos_event.cpp @@ -21,6 +21,7 @@ #include #include +#include #include // NOLINT #include "rclpy_common/common.h" @@ -57,22 +58,20 @@ create_zero_initialized_event() void QoSEvent::destroy() { - grandparent_pub_handle_.reset(); - grandparent_sub_handle_.reset(); rcl_event_.reset(); + std::visit([](auto & t) {t.destroy();}, grandparent_); } QoSEvent::QoSEvent( rclpy::Subscription & subscription, rcl_subscription_event_type_t event_type) -: event_type_(event_type) +: event_type_(event_type), grandparent_(subscription) { - grandparent_sub_handle_ = subscription.shared_from_this(); - // Create a subscription event rcl_event_ = create_zero_initialized_event(); rcl_ret_t ret = rcl_subscription_event_init( - rcl_event_.get(), grandparent_sub_handle_->rcl_ptr(), event_type); + rcl_event_.get(), + std::get(grandparent_).rcl_ptr(), event_type); if (RCL_RET_BAD_ALLOC == ret) { rcl_reset_error(); throw std::bad_alloc(); @@ -87,15 +86,14 @@ QoSEvent::QoSEvent( QoSEvent::QoSEvent( rclpy::Publisher & publisher, rcl_publisher_event_type_t event_type) -: event_type_(event_type) +: event_type_(event_type), grandparent_(publisher) { - grandparent_pub_handle_ = publisher.shared_from_this(); - // Create a publisher event rcl_event_ = create_zero_initialized_event(); rcl_ret_t ret = rcl_publisher_event_init( - rcl_event_.get(), grandparent_pub_handle_->rcl_ptr(), event_type); + rcl_event_.get(), + std::get(grandparent_).rcl_ptr(), event_type); if (RCL_RET_BAD_ALLOC == ret) { rcl_reset_error(); throw std::bad_alloc(); diff --git a/rclpy/src/rclpy/qos_event.hpp b/rclpy/src/rclpy/qos_event.hpp index 1c218a220..7d7bbcd96 100644 --- a/rclpy/src/rclpy/qos_event.hpp +++ b/rclpy/src/rclpy/qos_event.hpp @@ -91,10 +91,9 @@ class QoSEvent : public Destroyable, public std::enable_shared_from_this grandparent_pub_handle_; - std::shared_ptr grandparent_sub_handle_; - std::shared_ptr rcl_event_; std::variant event_type_; + std::variant grandparent_; + std::shared_ptr rcl_event_; }; /// Define a pybind11 wrapper for an rclpy::QoSEvent diff --git a/rclpy/src/rclpy/service.cpp b/rclpy/src/rclpy/service.cpp index c8cbf6efb..2f4acf093 100644 --- a/rclpy/src/rclpy/service.cpp +++ b/rclpy/src/rclpy/service.cpp @@ -35,16 +35,14 @@ void Service::destroy() { rcl_service_.reset(); - node_handle_.reset(); + node_.destroy(); } Service::Service( - py::capsule pynode, py::object pysrv_type, std::string service_name, + Node & node, py::object pysrv_type, std::string service_name, py::object pyqos_profile) -: node_handle_(std::make_shared(pynode)) +: node_(node) { - auto node = node_handle_->cast("rcl_node_t"); - auto srv_type = static_cast( rclpy_common_get_type_support(pysrv_type.ptr())); if (!srv_type) { @@ -60,11 +58,10 @@ Service::Service( // Create a client rcl_service_ = std::shared_ptr( new rcl_service_t, - [this](rcl_service_t * service) + [node](rcl_service_t * service) { - auto node = node_handle_->cast_or_warn("rcl_node_t"); - - rcl_ret_t ret = rcl_service_fini(service, node); + // Intentionally capture node by copy so shared_ptr can be transfered to copies + rcl_ret_t ret = rcl_service_fini(service, node.rcl_ptr()); if (RCL_RET_OK != ret) { // Warning should use line number of the current stack frame int stack_level = 1; @@ -79,7 +76,7 @@ Service::Service( *rcl_service_ = rcl_get_zero_initialized_service(); rcl_ret_t ret = rcl_service_init( - rcl_service_.get(), node, srv_type, + rcl_service_.get(), node_.rcl_ptr(), srv_type, service_name.c_str(), &service_ops); if (RCL_RET_OK != ret) { if (ret == RCL_RET_SERVICE_NAME_INVALID) { @@ -120,8 +117,7 @@ Service::service_take_request(py::object pyrequest_type) rmw_service_info_t header; py::tuple result_tuple(2); - rcl_ret_t ret = rcl_take_request_with_info( - rcl_service_.get(), &header, taken_request.get()); + rcl_ret_t ret = rcl_take_request_with_info(rcl_service_.get(), &header, taken_request.get()); if (ret == RCL_RET_SERVICE_TAKE_FAILED) { result_tuple[0] = py::none(); result_tuple[1] = py::none(); @@ -140,7 +136,7 @@ void define_service(py::object module) { py::class_>(module, "Service") - .def(py::init()) + .def(py::init()) .def_property_readonly( "pointer", [](const Service & service) { return reinterpret_cast(service.rcl_ptr()); diff --git a/rclpy/src/rclpy/service.hpp b/rclpy/src/rclpy/service.hpp index cae0afba0..6d8b0f298 100644 --- a/rclpy/src/rclpy/service.hpp +++ b/rclpy/src/rclpy/service.hpp @@ -25,6 +25,7 @@ #include "destroyable.hpp" #include "handle.hpp" +#include "node.hpp" #include "rclpy_common/exceptions.hpp" #include "utils.hpp" @@ -45,14 +46,14 @@ class Service : public Destroyable, public std::enable_shared_from_this * Raises ValueError if the capsules are not the correct types * Raises RCLError if the service could not be created * - * \param[in] pynode Capsule pointing to the node to add the service to + * \param[in] node node to add the service to * \param[in] pysrv_type Service module associated with the service * \param[in] service_name Python object for the service name * \param[in] pyqos_profile QoSProfile Python object for this service * \return capsule containing the rcl_service_t */ Service( - py::capsule pynode, py::object pysrv_type, std::string service_name, + Node & node, py::object pysrv_type, std::string service_name, py::object pyqos_profile); ~Service() = default; @@ -93,8 +94,7 @@ class Service : public Destroyable, public std::enable_shared_from_this destroy() override; private: - // TODO(ahcorde) replace with std::shared_ptr when rclpy::Node exists - std::shared_ptr node_handle_; + Node node_; std::shared_ptr rcl_service_; }; diff --git a/rclpy/src/rclpy/subscription.cpp b/rclpy/src/rclpy/subscription.cpp index 41dfa2d52..2b2f11cff 100644 --- a/rclpy/src/rclpy/subscription.cpp +++ b/rclpy/src/rclpy/subscription.cpp @@ -23,7 +23,6 @@ #include #include "rclpy_common/common.h" -#include "rclpy_common/handle.h" #include "rclpy_common/exceptions.hpp" @@ -36,12 +35,10 @@ using pybind11::literals::operator""_a; namespace rclpy { Subscription::Subscription( - py::capsule pynode, py::object pymsg_type, std::string topic, + Node & node, py::object pymsg_type, std::string topic, py::object pyqos_profile) -: node_handle_(std::make_shared(pynode)) +: node_(node) { - auto node = node_handle_->cast("rcl_node_t"); - auto msg_type = static_cast( rclpy_common_get_type_support(pymsg_type.ptr())); if (!msg_type) { @@ -56,11 +53,10 @@ Subscription::Subscription( rcl_subscription_ = std::shared_ptr( new rcl_subscription_t, - [this](rcl_subscription_t * subscription) + [node](rcl_subscription_t * subscription) { - auto node = node_handle_->cast_or_warn("rcl_node_t"); - - rcl_ret_t ret = rcl_subscription_fini(subscription, node); + // Intentionally capture node by copy so shared_ptr can be transfered to copies + rcl_ret_t ret = rcl_subscription_fini(subscription, node.rcl_ptr()); if (RCL_RET_OK != ret) { // Warning should use line number of the current stack frame int stack_level = 1; @@ -75,7 +71,7 @@ Subscription::Subscription( *rcl_subscription_ = rcl_get_zero_initialized_subscription(); rcl_ret_t ret = rcl_subscription_init( - rcl_subscription_.get(), node, msg_type, + rcl_subscription_.get(), node_.rcl_ptr(), msg_type, topic.c_str(), &subscription_ops); if (ret != RCL_RET_OK) { if (ret == RCL_RET_TOPIC_NAME_INVALID) { @@ -91,7 +87,7 @@ Subscription::Subscription( void Subscription::destroy() { rcl_subscription_.reset(); - node_handle_.reset(); + node_.destroy(); } py::object @@ -144,9 +140,7 @@ Subscription::take_message(py::object pymsg_type, bool raw) const char * Subscription::get_logger_name() { - auto node = node_handle_->cast("rcl_node_t"); - - const char * node_logger_name = rcl_node_get_logger_name(node); + const char * node_logger_name = rcl_node_get_logger_name(node_.rcl_ptr()); if (!node_logger_name) { throw RCLError("Node logger name not set"); } @@ -168,7 +162,7 @@ void define_subscription(py::object module) { py::class_>(module, "Subscription") - .def(py::init()) + .def(py::init()) .def_property_readonly( "pointer", [](const Subscription & subscription) { return reinterpret_cast(subscription.rcl_ptr()); diff --git a/rclpy/src/rclpy/subscription.hpp b/rclpy/src/rclpy/subscription.hpp index bafabaf4e..21640a16e 100644 --- a/rclpy/src/rclpy/subscription.hpp +++ b/rclpy/src/rclpy/subscription.hpp @@ -24,7 +24,7 @@ #include #include "destroyable.hpp" -#include "handle.hpp" +#include "node.hpp" namespace py = pybind11; @@ -43,13 +43,13 @@ class Subscription : public Destroyable, public std::enable_shared_from_this node_handle_; + Node node_; std::shared_ptr rcl_subscription_; }; /// Define a pybind11 wrapper for an rclpy::Service diff --git a/rclpy/src/rclpy/timer.cpp b/rclpy/src/rclpy/timer.cpp index 997c3fb06..a424dd013 100644 --- a/rclpy/src/rclpy/timer.cpp +++ b/rclpy/src/rclpy/timer.cpp @@ -37,20 +37,14 @@ void Timer::destroy() { rcl_timer_.reset(); - clock_handle_.reset(); + clock_.destroy(); + context_.destroy(); } Timer::Timer( - Clock & rclcy_clock, py::capsule pycontext, int64_t period_nsec) + Clock & clock, Context & context, int64_t period_nsec) +: context_(context), clock_(clock) { - clock_handle_ = rclcy_clock.shared_from_this(); - - auto context = static_cast( - rclpy_handle_get_pointer_from_capsule(pycontext.ptr(), "rcl_context_t")); - if (!context) { - throw py::error_already_set(); - } - // Create a client rcl_timer_ = std::shared_ptr( new rcl_timer_t, @@ -72,7 +66,7 @@ Timer::Timer( rcl_allocator_t allocator = rcl_get_default_allocator(); rcl_ret_t ret = rcl_timer_init( - rcl_timer_.get(), clock_handle_->rcl_ptr(), context, + rcl_timer_.get(), clock_.rcl_ptr(), context.rcl_ptr(), period_nsec, NULL, allocator); if (RCL_RET_OK != ret) { @@ -169,7 +163,7 @@ void define_timer(py::object module) { py::class_>(module, "Timer") - .def(py::init()) + .def(py::init()) .def_property_readonly( "pointer", [](const Timer & timer) { return reinterpret_cast(timer.rcl_ptr()); diff --git a/rclpy/src/rclpy/timer.hpp b/rclpy/src/rclpy/timer.hpp index 00b2559e6..55c37b292 100644 --- a/rclpy/src/rclpy/timer.hpp +++ b/rclpy/src/rclpy/timer.hpp @@ -22,6 +22,7 @@ #include #include "clock.hpp" +#include "context.hpp" #include "destroyable.hpp" #include "handle.hpp" @@ -46,7 +47,7 @@ class Timer : public Destroyable, public std::enable_shared_from_this * \param[in] period_nsec the period of the timer in nanoseconds * \return a timer capsule */ - Timer(Clock & clock, py::capsule pycontext, int64_t period_nsec); + Timer(Clock & clock, Context & context, int64_t period_nsec); ~Timer() = default; @@ -133,7 +134,8 @@ class Timer : public Destroyable, public std::enable_shared_from_this void destroy() override; private: - std::shared_ptr clock_handle_; + Context context_; + Clock clock_; std::shared_ptr rcl_timer_; }; diff --git a/rclpy/src/rclpy/utils.cpp b/rclpy/src/rclpy/utils.cpp index 39adef07a..6f2cf2c0d 100644 --- a/rclpy/src/rclpy/utils.cpp +++ b/rclpy/src/rclpy/utils.cpp @@ -17,17 +17,18 @@ #include #include +#include #include #include #include +#include #include #include "rclpy_common/common.h" #include "rclpy_common/handle.h" #include "rclpy_common/exceptions.hpp" -#include "init.hpp" #include "utils.hpp" namespace rclpy @@ -225,4 +226,54 @@ remove_ros_args(py::object pycli_args) return result_args; } + +void +throw_if_unparsed_ros_args(py::list pyargs, const rcl_arguments_t & rcl_args) +{ + int unparsed_ros_args_count = rcl_arguments_get_count_unparsed_ros(&rcl_args); + + if (unparsed_ros_args_count < 0) { + throw std::runtime_error("failed to count unparsed arguments"); + } + if (0 == unparsed_ros_args_count) { + return; + } + + rcl_allocator_t allocator = rcl_get_default_allocator(); + + int * unparsed_indices_c = nullptr; + rcl_ret_t ret = rcl_arguments_get_unparsed_ros(&rcl_args, allocator, &unparsed_indices_c); + if (RCL_RET_OK != ret) { + throw RCLError("failed to get unparsed arguments"); + } + + auto deallocator = [&](int ptr[]) {allocator.deallocate(ptr, allocator.state);}; + auto unparsed_indices = std::unique_ptr( + unparsed_indices_c, deallocator); + + py::list unparsed_args; + for (int i = 0; i < unparsed_ros_args_count; ++i) { + int index = unparsed_indices_c[i]; + if (index < 0 || static_cast(index) >= pyargs.size()) { + throw std::runtime_error("got invalid unparsed ROS arg index"); + } + unparsed_args.append(pyargs[index]); + } + + throw UnknownROSArgsError(static_cast(py::repr(unparsed_args))); +} + +py::dict +rclpy_action_get_rmw_qos_profile(const char * rmw_profile) +{ + PyObject * pyqos_profile = NULL; + if (0 == strcmp(rmw_profile, "rcl_action_qos_profile_status_default")) { + pyqos_profile = rclpy_common_convert_to_qos_dict(&rcl_action_qos_profile_status_default); + } else { + std::string error_text = "Requested unknown rmw_qos_profile: "; + error_text += rmw_profile; + throw std::runtime_error(error_text); + } + return py::reinterpret_steal(pyqos_profile); +} } // namespace rclpy diff --git a/rclpy/src/rclpy/utils.hpp b/rclpy/src/rclpy/utils.hpp index 1520005a1..0f4ae104e 100644 --- a/rclpy/src/rclpy/utils.hpp +++ b/rclpy/src/rclpy/utils.hpp @@ -21,6 +21,7 @@ #include +#include "rclpy_common/common.h" #include "publisher.hpp" namespace py = pybind11; @@ -105,6 +106,21 @@ assert_liveliness(rclpy::Publisher * publisher); py::list remove_ros_args(py::object pycli_args); +/// Throw UnparsedROSArgsError with a message saying which args are unparsed. +void +throw_if_unparsed_ros_args(py::list pyargs, const rcl_arguments_t & rcl_args); + +/// Fetch a predefined qos_profile from rcl_action and convert it to a Python QoSProfile object. +/** + * Raises RuntimeError if the QoS profile is unknown. + * + * This function takes a string defining a rmw_qos_profile_t and returns the + * corresponding Python QoSProfile object. + * \param[in] rmw_profile String with the name of the profile to load. + * \return QoSProfile object. + */ +py::dict +rclpy_action_get_rmw_qos_profile(const char * rmw_profile); } // namespace rclpy #endif // RCLPY__UTILS_HPP_ diff --git a/rclpy/src/rclpy/wait_set.cpp b/rclpy/src/rclpy/wait_set.cpp index e901b8ec4..2e837be15 100644 --- a/rclpy/src/rclpy/wait_set.cpp +++ b/rclpy/src/rclpy/wait_set.cpp @@ -32,27 +32,6 @@ namespace rclpy { -WaitSet::WaitSet() -{ - // Create a client - rcl_wait_set_ = std::shared_ptr( - new rcl_wait_set_t, - [](rcl_wait_set_t * waitset) - { - rcl_ret_t ret = rcl_wait_set_fini(waitset); - if (RCL_RET_OK != ret) { - // Warning should use line number of the current stack frame - int stack_level = 1; - PyErr_WarnFormat( - PyExc_RuntimeWarning, stack_level, "Failed to fini wait set: %s", - rcl_get_error_string().str); - rcl_reset_error(); - } - delete waitset; - }); - *rcl_wait_set_ = rcl_get_zero_initialized_wait_set(); -} - WaitSet::WaitSet( size_t number_of_subscriptions, size_t number_of_guard_conditions, @@ -60,7 +39,8 @@ WaitSet::WaitSet( size_t number_of_clients, size_t number_of_services, size_t number_of_events, - py::capsule pycontext) + Context & context) +: context_(context) { // Create a client rcl_wait_set_ = std::shared_ptr( @@ -80,12 +60,6 @@ WaitSet::WaitSet( }); *rcl_wait_set_ = rcl_get_zero_initialized_wait_set(); - auto context = static_cast( - rclpy_handle_get_pointer_from_capsule(pycontext.ptr(), "rcl_context_t")); - if (!context) { - throw py::error_already_set(); - } - rcl_ret_t ret = rcl_wait_set_init( rcl_wait_set_.get(), number_of_subscriptions, @@ -94,7 +68,7 @@ WaitSet::WaitSet( number_of_clients, number_of_services, number_of_events, - context, + context.rcl_ptr(), rcl_get_default_allocator()); if (RCL_RET_OK != ret) { throw RCLError("failed to initialize wait set"); @@ -105,6 +79,7 @@ void WaitSet::destroy() { rcl_wait_set_.reset(); + context_.destroy(); } void @@ -116,34 +91,6 @@ WaitSet::clear_entities() } } -size_t -WaitSet::add_entity(const std::string & entity_type, py::capsule pyentity) -{ - rcl_ret_t ret = RCL_RET_ERROR; - size_t index; - - if ("guard_condition" == entity_type) { - auto guard_condition = static_cast( - rclpy_handle_get_pointer_from_capsule(pyentity.ptr(), "rcl_guard_condition_t")); - if (!guard_condition) { - throw py::error_already_set(); - } - ret = rcl_wait_set_add_guard_condition(rcl_wait_set_.get(), guard_condition, &index); - } else { - std::string error_text{"'"}; - error_text += entity_type; - error_text += "' is not a known entity"; - throw std::runtime_error(error_text); - } - if (ret != RCL_RET_OK) { - std::string error_text{"failed to add'"}; - error_text += entity_type; - error_text += "' to waitset"; - throw RCLError(error_text); - } - return index; -} - size_t WaitSet::add_service(const Service & service) { @@ -312,8 +259,7 @@ WaitSet::wait(int64_t timeout) void define_waitset(py::object module) { py::class_>(module, "WaitSet") - .def(py::init()) - .def(py::init<>()) + .def(py::init()) .def_property_readonly( "pointer", [](const WaitSet & waitset) { return reinterpret_cast(waitset.rcl_ptr()); @@ -322,9 +268,6 @@ void define_waitset(py::object module) .def( "clear_entities", &WaitSet::clear_entities, "Clear all the pointers in the wait set") - .def( - "add_entity", &WaitSet::add_entity, - "Add an entity to the wait set structure") .def( "add_service", &WaitSet::add_service, "Add a service to the wait set structure") diff --git a/rclpy/src/rclpy/wait_set.hpp b/rclpy/src/rclpy/wait_set.hpp index 2ba9efd8e..7d74cb1fc 100644 --- a/rclpy/src/rclpy/wait_set.hpp +++ b/rclpy/src/rclpy/wait_set.hpp @@ -21,6 +21,7 @@ #include #include "client.hpp" +#include "context.hpp" #include "destroyable.hpp" #include "guard_condition.hpp" #include "qos_event.hpp" @@ -57,7 +58,7 @@ class WaitSet : public Destroyable, public std::enable_shared_from_this size_t number_of_clients, size_t number_of_services, size_t number_of_events, - py::capsule pycontext); + Context & context); /// Clear all the pointers in the wait set /** @@ -66,18 +67,6 @@ class WaitSet : public Destroyable, public std::enable_shared_from_this void clear_entities(); - /// Add an entity to the wait set structure - /** - * Raises RuntimeError if the entity type is unknown - * Raises RCLError if any lower level error occurs - * - * \param[in] entity_type string defining the entity ["subscription, client, service"] - * \param[in] pyentity Capsule pointing to the entity to add - * \return Index in waitset entity was added at - */ - size_t - add_entity(const std::string & entity_type, py::capsule pyentity); - /// Add a service to the wait set structure /** * Raises RCLError if any lower level error occurs @@ -182,6 +171,7 @@ class WaitSet : public Destroyable, public std::enable_shared_from_this void destroy() override; private: + Context context_; std::shared_ptr rcl_wait_set_; }; diff --git a/rclpy/src/rclpy_common/include/rclpy_common/common.h b/rclpy/src/rclpy_common/include/rclpy_common/common.h index 606f5ad0e..db18e9586 100644 --- a/rclpy/src/rclpy_common/include/rclpy_common/common.h +++ b/rclpy/src/rclpy_common/include/rclpy_common/common.h @@ -32,16 +32,6 @@ typedef void destroy_ros_message_signature (void *); typedef bool convert_from_py_signature (PyObject *, void *); typedef PyObject * convert_to_py_signature (void *); -/// Finalize names and types struct with error setting. -/** - * \param[in] names_and_types The struct to finalize. - * \return `true` if finalized successfully, `false` otherwise. - * If `false`, then a Python error is set. - */ -RCLPY_COMMON_PUBLIC -bool -rclpy_names_and_types_fini(rcl_names_and_types_t * names_and_types); - /// Get the type support structure for a Python ROS message type. /** * \param[in] pymsg_type The Python ROS message type. @@ -51,17 +41,6 @@ RCLPY_COMMON_PUBLIC void * rclpy_common_get_type_support(PyObject * pymsg_type); -/// Convert a C rcl_names_and_types_t into a Python list. -/** - * \param names_and_types The names and types struct to convert. - * \return A PyList of PyTuples. The first element of each tuple is a string for the - * name and the second element is a list of strings for the types. - * \return `NULL` if there is an error. No Python error is set. - */ -RCLPY_COMMON_PUBLIC -PyObject * -rclpy_convert_to_py_names_and_types(rcl_names_and_types_t * topic_names_and_types); - /// Convert a C rmw_qos_profile_t into a Python dictionary with qos profile args. /** * \param[in] profile Pointer to a rmw_qos_profile_t to convert diff --git a/rclpy/src/rclpy_common/src/common.c b/rclpy/src/rclpy_common/src/common.c index e0788fdd8..e96319360 100644 --- a/rclpy/src/rclpy_common/src/common.c +++ b/rclpy/src/rclpy_common/src/common.c @@ -55,23 +55,6 @@ cleanup_rclpy_qos_profile(rclpy_qos_profile_t * profile) Py_XDECREF(profile->avoid_ros_namespace_conventions); } -bool -rclpy_names_and_types_fini(rcl_names_and_types_t * names_and_types) -{ - if (!names_and_types) { - return true; - } - rcl_ret_t ret = rcl_names_and_types_fini(names_and_types); - if (ret != RCL_RET_OK) { - PyErr_Format( - PyExc_RuntimeError, - "Failed to destroy rcl_names_and_types_t: %s", rcl_get_error_string().str); - rcl_reset_error(); - return false; - } - return true; -} - void * rclpy_common_get_type_support(PyObject * pymsg_type) { @@ -91,55 +74,6 @@ rclpy_common_get_type_support(PyObject * pymsg_type) return ts; } -PyObject * -rclpy_convert_to_py_names_and_types(rcl_names_and_types_t * names_and_types) -{ - if (!names_and_types) { - return NULL; - } - - PyObject * pynames_and_types = PyList_New(names_and_types->names.size); - if (!pynames_and_types) { - return NULL; - } - - size_t i; - for (i = 0; i < names_and_types->names.size; ++i) { - PyObject * pytuple = PyTuple_New(2); - if (!pytuple) { - Py_DECREF(pynames_and_types); - return NULL; - } - PyObject * pyname = PyUnicode_FromString(names_and_types->names.data[i]); - if (!pyname) { - Py_DECREF(pynames_and_types); - Py_DECREF(pytuple); - return NULL; - } - PyTuple_SET_ITEM(pytuple, 0, pyname); - PyObject * pytypes_list = PyList_New(names_and_types->types[i].size); - if (!pytypes_list) { - Py_DECREF(pynames_and_types); - Py_DECREF(pytuple); - return NULL; - } - size_t j; - for (j = 0; j < names_and_types->types[i].size; ++j) { - PyObject * pytype = PyUnicode_FromString(names_and_types->types[i].data[j]); - if (!pytype) { - Py_DECREF(pynames_and_types); - Py_DECREF(pytuple); - Py_DECREF(pytypes_list); - return NULL; - } - PyList_SET_ITEM(pytypes_list, j, pytype); - } - PyTuple_SET_ITEM(pytuple, 1, pytypes_list); - PyList_SET_ITEM(pynames_and_types, i, pytuple); - } - return pynames_and_types; -} - static PyObject * _convert_rmw_time_to_py_duration(const rmw_time_t * duration) diff --git a/rclpy/test/test_destruction.py b/rclpy/test/test_destruction.py index 14b93925f..e16d11fe7 100644 --- a/rclpy/test/test_destruction.py +++ b/rclpy/test/test_destruction.py @@ -38,8 +38,7 @@ def test_destroy_node_twice(): try: node = rclpy.create_node('test_node2', context=context) node.destroy_node() - with pytest.raises(InvalidHandle): - node.destroy_node() + node.destroy_node() finally: rclpy.shutdown(context=context) @@ -162,11 +161,8 @@ def test_destroy_node_asap(): try: node = rclpy.create_node('test_destroy_subscription_asap', context=context) - with node.handle: - node.destroy_node() - # handle valid because it's still being used - with node.handle: - pass + + node.destroy_node() with pytest.raises(InvalidHandle): # handle invalid because it was destroyed when no one was using it diff --git a/rclpy/test/test_handle.py b/rclpy/test/test_handle.py index 6c9cab2e8..99b016280 100644 --- a/rclpy/test/test_handle.py +++ b/rclpy/test/test_handle.py @@ -23,7 +23,7 @@ def test_handle_destroyed_immediately(): try: node = rclpy.create_node('test_handle_destroyed_immediately', context=context) - node.handle.destroy() + node.destroy_node() with pytest.raises(InvalidHandle): with node.handle: pass @@ -38,9 +38,7 @@ def test_handle_destroyed_when_not_used(): try: node = rclpy.create_node('test_handle_destroyed_when_not_used', context=context) with node.handle: - node.handle.destroy() - with node.handle: - pass + node.destroy_node() with pytest.raises(InvalidHandle): with node.handle: diff --git a/rclpy/test/test_qos_event.py b/rclpy/test/test_qos_event.py index 34a810468..a94b95865 100644 --- a/rclpy/test/test_qos_event.py +++ b/rclpy/test/test_qos_event.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +import gc import unittest from unittest.mock import Mock @@ -50,6 +51,11 @@ def setUp(self): self.is_fastrtps = 'rmw_fastrtps' in get_rmw_implementation_identifier() def tearDown(self): + # These tests create a bunch of events by hand instead of using Node APIs, + # so they won't be cleaned up when calling `node.destroy_node()`, but they could still + # keep the node alive from one test to the next. + # Invoke the garbage collector to destroy them. + gc.collect() self.node.destroy_node() rclpy.shutdown(context=self.context) @@ -237,8 +243,8 @@ def test_call_publisher_rclpy_event_apis(self): # Go through the exposed apis and ensure that things don't explode when called # Make no assumptions about being able to actually receive the events publisher = self.node.create_publisher(EmptyMsg, self.topic_name, 10) - with self.context.handle as context_handle: - wait_set = _rclpy.WaitSet(0, 0, 0, 0, 0, 3, context_handle) + with self.context.handle: + wait_set = _rclpy.WaitSet(0, 0, 0, 0, 0, 3, self.context.handle) deadline_event_handle = self._create_event_handle( publisher, QoSPublisherEventType.RCL_PUBLISHER_OFFERED_DEADLINE_MISSED) @@ -309,8 +315,8 @@ def test_call_subscription_rclpy_event_apis(self): # Go through the exposed apis and ensure that things don't explode when called # Make no assumptions about being able to actually receive the events subscription = self.node.create_subscription(EmptyMsg, self.topic_name, Mock(), 10) - with self.context.handle as context_handle: - wait_set = _rclpy.WaitSet(0, 0, 0, 0, 0, 3, context_handle) + with self.context.handle: + wait_set = _rclpy.WaitSet(0, 0, 0, 0, 0, 3, self.context.handle) deadline_event_handle = self._create_event_handle( subscription, QoSSubscriptionEventType.RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED) diff --git a/rclpy/test/test_subscription.py b/rclpy/test/test_subscription.py index e12b4e7a8..b3f994b71 100644 --- a/rclpy/test/test_subscription.py +++ b/rclpy/test/test_subscription.py @@ -16,6 +16,7 @@ import rclpy from rclpy.node import Node + from test_msgs.msg import Empty @@ -48,7 +49,13 @@ def make_mock_subscription(namespace, topic_name, cli_args=None): ('/example/topic', 'ns', '/example/topic'), ]) def test_get_subscription_topic_name(topic_name, namespace, expected): - sub = make_mock_subscription(namespace, topic_name) + node = Node('node_name', namespace=namespace, cli_args=None) + sub = node.create_subscription( + msg_type=Empty, + topic=topic_name, + callback=lambda _: None, + qos_profile=10, + ) assert sub.topic_name == expected @@ -61,5 +68,11 @@ def test_get_subscription_topic_name(topic_name, namespace, expected): '/ns/new_topic'), ]) def test_get_subscription_topic_name_after_remapping(topic_name, namespace, cli_args, expected): - sub = make_mock_subscription(namespace, topic_name, cli_args) + node = Node('node_name', namespace=namespace, cli_args=cli_args) + sub = node.create_subscription( + msg_type=Empty, + topic=topic_name, + callback=lambda _: None, + qos_profile=10, + ) assert sub.topic_name == expected diff --git a/rclpy/test/test_waitable.py b/rclpy/test/test_waitable.py index c7f422de0..f79841c51 100644 --- a/rclpy/test/test_waitable.py +++ b/rclpy/test/test_waitable.py @@ -41,9 +41,9 @@ class ClientWaitable(Waitable): def __init__(self, node): super().__init__(ReentrantCallbackGroup()) - with node.handle as node_capsule: + with node.handle: self.client = _rclpy.Client( - node_capsule, EmptySrv, 'test_client', QoSProfile(depth=10).get_c_qos_profile()) + node.handle, EmptySrv, 'test_client', QoSProfile(depth=10).get_c_qos_profile()) self.client_index = None self.client_is_ready = False @@ -84,9 +84,9 @@ class ServerWaitable(Waitable): def __init__(self, node): super().__init__(ReentrantCallbackGroup()) - with node.handle as node_capsule: + with node.handle: self.server = _rclpy.Service( - node_capsule, EmptySrv, 'test_server', QoSProfile(depth=10).get_c_qos_profile()) + node.handle, EmptySrv, 'test_server', QoSProfile(depth=10).get_c_qos_profile()) self.server_index = None self.server_is_ready = False @@ -129,9 +129,9 @@ def __init__(self, node): self._clock = Clock(clock_type=ClockType.STEADY_TIME) period_nanoseconds = 10000 - with self._clock.handle, node.context.handle as context_capsule: + with self._clock.handle, node.context.handle: self.timer = _rclpy.Timer( - self._clock.handle, context_capsule, period_nanoseconds) + self._clock.handle, node.context.handle, period_nanoseconds) self.timer_index = None self.timer_is_ready = False @@ -173,9 +173,9 @@ class SubscriptionWaitable(Waitable): def __init__(self, node): super().__init__(ReentrantCallbackGroup()) - with node.handle as node_capsule: + with node.handle: self.subscription = _rclpy.Subscription( - node_capsule, EmptyMsg, 'test_topic', QoSProfile(depth=10).get_c_qos_profile()) + node.handle, EmptyMsg, 'test_topic', QoSProfile(depth=10).get_c_qos_profile()) self.subscription_index = None self.subscription_is_ready = False @@ -219,8 +219,8 @@ class GuardConditionWaitable(Waitable): def __init__(self, node): super().__init__(ReentrantCallbackGroup()) - with node.context.handle as context_capsule: - self.guard_condition = _rclpy.GuardCondition(context_capsule) + with node.context.handle: + self.guard_condition = _rclpy.GuardCondition(node.context.handle) self.guard_condition_index = None self.guard_is_ready = False