From 30da3f4996649dac4a5e851255704fe60f0a8a86 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Sun, 3 Nov 2024 13:10:57 -0800 Subject: [PATCH 1/4] add QoS Profile/Depth support to Node. Signed-off-by: Tomoya Fujita --- rclpy/rclpy/__init__.py | 9 +++++++++ rclpy/rclpy/node.py | 11 ++++++++++- rclpy/src/rclpy/node.cpp | 9 +++++++-- rclpy/src/rclpy/node.hpp | 4 +++- rclpy/test/test_create_node.py | 24 ++++++++++++++++++++++++ 5 files changed, 53 insertions(+), 4 deletions(-) diff --git a/rclpy/rclpy/__init__.py b/rclpy/rclpy/__init__.py index 51dc4ebda..9eced626b 100644 --- a/rclpy/rclpy/__init__.py +++ b/rclpy/rclpy/__init__.py @@ -44,10 +44,13 @@ from typing import List from typing import Optional from typing import Type +from typing import Union from typing import TYPE_CHECKING from rclpy.context import Context from rclpy.parameter import Parameter +from rclpy.qos import QoSProfile +from rclpy.qos import qos_profile_system_default from rclpy.signals import install_signal_handlers from rclpy.signals import SignalHandlerOptions from rclpy.signals import uninstall_signal_handlers @@ -214,6 +217,7 @@ def create_node( namespace: Optional[str] = None, use_global_arguments: bool = True, enable_rosout: bool = True, + rosout_qos_profile: Optional[Union[QoSProfile, int]] = qos_profile_system_default, start_parameter_services: bool = True, parameter_overrides: Optional[List[Parameter]] = None, allow_undeclared_parameters: bool = False, @@ -233,6 +237,10 @@ def create_node( :param use_global_arguments: ``False`` if the node should ignore process-wide command line arguments. :param enable_rosout: ``False`` if the node should ignore rosout logging. + :param rosout_qos_profile: A QoSProfile or a history depth to apply to rosout publisher. + In the case that a history depth is provided, the QoS history is set to KEEP_LAST, + the QoS history depth is set to the value of the parameter, + and all other QoS settings are set to their default values. :param start_parameter_services: ``False`` if the node should not create parameter services. :param parameter_overrides: A list of :class:`.Parameter` which are used to override the initial values of parameters declared on this node. @@ -251,6 +259,7 @@ def create_node( node_name, context=context, cli_args=cli_args, namespace=namespace, use_global_arguments=use_global_arguments, enable_rosout=enable_rosout, + rosout_qos_profile=rosout_qos_profile, start_parameter_services=start_parameter_services, parameter_overrides=parameter_overrides, allow_undeclared_parameters=allow_undeclared_parameters, diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index 87128694b..4ddfa7552 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -75,6 +75,7 @@ from rclpy.qos import qos_profile_parameter_events from rclpy.qos import qos_profile_services_default from rclpy.qos import QoSProfile +from rclpy.qos import qos_profile_system_default from rclpy.qos_overriding_options import _declare_qos_parameters from rclpy.qos_overriding_options import QoSOverridingOptions from rclpy.service import Service @@ -139,6 +140,7 @@ def __init__( namespace: Optional[str] = None, use_global_arguments: bool = True, enable_rosout: bool = True, + rosout_qos_profile: Optional[Union[QoSProfile, int]] = qos_profile_system_default, start_parameter_services: bool = True, parameter_overrides: Optional[List[Parameter[Any]]] = None, allow_undeclared_parameters: bool = False, @@ -159,6 +161,10 @@ def __init__( :param use_global_arguments: ``False`` if the node should ignore process-wide command line args. :param enable_rosout: ``False`` if the node should ignore rosout logging. + :param rosout_qos_profile: A QoSProfile or a history depth to apply to the rosout publisher. + In the case that a history depth is provided, the QoS history is set to KEEP_LAST + the QoS history depth is set to the value of the parameter, + and all other QoS settings are set to their default value. :param start_parameter_services: ``False`` if the node should not create parameter services. :param parameter_overrides: A list of overrides for initial values for parameters declared @@ -196,6 +202,8 @@ def __init__( if self._context.handle is None or not self._context.ok(): raise NotInitializedException('cannot create node') + rosout_qos_profile = self._validate_qos_or_depth_parameter(rosout_qos_profile) + with self._context.handle: try: self.__node = _rclpy.Node( @@ -204,7 +212,8 @@ def __init__( self._context.handle, cli_args, use_global_arguments, - enable_rosout + enable_rosout, + rosout_qos_profile.get_c_qos_profile() ) except ValueError: # these will raise more specific errors if the name or namespace is bad diff --git a/rclpy/src/rclpy/node.cpp b/rclpy/src/rclpy/node.cpp index 7ee0b9c3a..cfb30e4b0 100644 --- a/rclpy/src/rclpy/node.cpp +++ b/rclpy/src/rclpy/node.cpp @@ -408,7 +408,8 @@ Node::Node( Context & context, py::object pycli_args, bool use_global_arguments, - bool enable_rosout) + bool enable_rosout, + py::object rosout_qos_profile) : context_(context) { rcl_ret_t ret; @@ -500,6 +501,10 @@ Node::Node( options.arguments = arguments; options.enable_rosout = enable_rosout; + if (!rosout_qos_profile.is_none()) { + options.rosout_qos = rosout_qos_profile.cast(); + } + ret = rcl_node_init( rcl_node_.get(), node_name, namespace_, context.rcl_ptr(), &options); @@ -581,7 +586,7 @@ void define_node(py::object module) { py::class_>(module, "Node") - .def(py::init()) + .def(py::init()) .def_property_readonly( "pointer", [](const Node & node) { return reinterpret_cast(node.rcl_ptr()); diff --git a/rclpy/src/rclpy/node.hpp b/rclpy/src/rclpy/node.hpp index 0f53adc61..16143b994 100644 --- a/rclpy/src/rclpy/node.hpp +++ b/rclpy/src/rclpy/node.hpp @@ -45,6 +45,7 @@ class Node : public Destroyable, public std::enable_shared_from_this * \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 + * \param[in] rosout_qos_profile rmw_qos_profile_t object for rosout publisher. */ Node( const char * node_name, @@ -52,7 +53,8 @@ class Node : public Destroyable, public std::enable_shared_from_this Context & context, py::object pycli_args, bool use_global_arguments, - bool enable_rosout); + bool enable_rosout, + py::object rosout_qos_profile); /// Get the fully qualified name of the node. /** diff --git a/rclpy/test/test_create_node.py b/rclpy/test/test_create_node.py index e0c62c4c3..e9fd80a10 100644 --- a/rclpy/test/test_create_node.py +++ b/rclpy/test/test_create_node.py @@ -19,6 +19,7 @@ from rclpy.exceptions import InvalidNamespaceException from rclpy.exceptions import InvalidNodeNameException from rclpy.parameter import Parameter +from rclpy.qos import qos_profile_sensor_data class TestCreateNode(unittest.TestCase): @@ -82,6 +83,29 @@ def test_create_node_with_parameter_overrides(self) -> None: ] ).destroy_node() + def test_create_node_disable_rosout(self): + node_name = 'create_node_test_disable_rosout' + namespace = '/ns' + node = rclpy.create_node( + node_name, namespace=namespace, context=self.context, enable_rosout=False) + node.destroy_node() + + def test_create_node_rosout_qos_profile(self): + node_name = 'create_node_test_rosout_rosout_qos_profile' + namespace = '/ns' + node = rclpy.create_node( + node_name, namespace=namespace, context=self.context, enable_rosout=True, + rosout_qos_profile=qos_profile_sensor_data) + node.destroy_node() + + def test_create_node_rosout_qos_depth(self): + node_name = 'create_node_test_rosout_rosout_qos_depth' + namespace = '/ns' + node = rclpy.create_node( + node_name, namespace=namespace, context=self.context, enable_rosout=True, + rosout_qos_profile=10) + node.destroy_node() + if __name__ == '__main__': unittest.main() From 6030f1a3149ffc214a9d2d38cde0bdb84dec3085 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Mon, 4 Nov 2024 12:06:21 -0800 Subject: [PATCH 2/4] address flake8 warnings. Signed-off-by: Tomoya Fujita --- rclpy/rclpy/__init__.py | 4 ++-- rclpy/rclpy/node.py | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rclpy/rclpy/__init__.py b/rclpy/rclpy/__init__.py index 9eced626b..39d589e7a 100644 --- a/rclpy/rclpy/__init__.py +++ b/rclpy/rclpy/__init__.py @@ -44,13 +44,13 @@ from typing import List from typing import Optional from typing import Type -from typing import Union from typing import TYPE_CHECKING +from typing import Union from rclpy.context import Context from rclpy.parameter import Parameter -from rclpy.qos import QoSProfile from rclpy.qos import qos_profile_system_default +from rclpy.qos import QoSProfile from rclpy.signals import install_signal_handlers from rclpy.signals import SignalHandlerOptions from rclpy.signals import uninstall_signal_handlers diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index 4ddfa7552..e66e82ae9 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -74,8 +74,8 @@ from rclpy.publisher import Publisher from rclpy.qos import qos_profile_parameter_events from rclpy.qos import qos_profile_services_default -from rclpy.qos import QoSProfile from rclpy.qos import qos_profile_system_default +from rclpy.qos import QoSProfile from rclpy.qos_overriding_options import _declare_qos_parameters from rclpy.qos_overriding_options import QoSOverridingOptions from rclpy.service import Service @@ -161,7 +161,7 @@ def __init__( :param use_global_arguments: ``False`` if the node should ignore process-wide command line args. :param enable_rosout: ``False`` if the node should ignore rosout logging. - :param rosout_qos_profile: A QoSProfile or a history depth to apply to the rosout publisher. + :param rosout_qos_profile: A QoSProfile or a history depth to apply to rosout publisher. In the case that a history depth is provided, the QoS history is set to KEEP_LAST the QoS history depth is set to the value of the parameter, and all other QoS settings are set to their default value. From 1e5a670dbbbe4081676ddbd0d963522443178a11 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Wed, 6 Nov 2024 12:29:50 -0800 Subject: [PATCH 3/4] support qos_profile_rosout_default. Signed-off-by: Tomoya Fujita --- rclpy/rclpy/__init__.py | 4 +- rclpy/rclpy/impl/_rclpy_pybind11.pyi | 3 +- rclpy/rclpy/node.py | 4 +- rclpy/rclpy/qos.py | 5 ++- rclpy/src/rclpy/qos.cpp | 3 ++ rclpy/test/test_create_node.py | 67 +++++++++++++++++++++++----- 6 files changed, 68 insertions(+), 18 deletions(-) diff --git a/rclpy/rclpy/__init__.py b/rclpy/rclpy/__init__.py index 39d589e7a..abdd91e8d 100644 --- a/rclpy/rclpy/__init__.py +++ b/rclpy/rclpy/__init__.py @@ -49,7 +49,7 @@ from rclpy.context import Context from rclpy.parameter import Parameter -from rclpy.qos import qos_profile_system_default +from rclpy.qos import qos_profile_rosout_default from rclpy.qos import QoSProfile from rclpy.signals import install_signal_handlers from rclpy.signals import SignalHandlerOptions @@ -217,7 +217,7 @@ def create_node( namespace: Optional[str] = None, use_global_arguments: bool = True, enable_rosout: bool = True, - rosout_qos_profile: Optional[Union[QoSProfile, int]] = qos_profile_system_default, + rosout_qos_profile: Optional[Union[QoSProfile, int]] = qos_profile_rosout_default, start_parameter_services: bool = True, parameter_overrides: Optional[List[Parameter]] = None, allow_undeclared_parameters: bool = False, diff --git a/rclpy/rclpy/impl/_rclpy_pybind11.pyi b/rclpy/rclpy/impl/_rclpy_pybind11.pyi index f348b2519..6d4a0ca9b 100644 --- a/rclpy/rclpy/impl/_rclpy_pybind11.pyi +++ b/rclpy/rclpy/impl/_rclpy_pybind11.pyi @@ -430,7 +430,8 @@ class Timer(Destroyable): PredefinedQosProfileTNames = Literal['qos_profile_sensor_data', 'qos_profile_default', 'qos_profile_system_default', 'qos_profile_services_default', 'qos_profile_unknown', 'qos_profile_parameters', - 'qos_profile_parameter_events', 'qos_profile_best_available'] + 'qos_profile_parameter_events', 'qos_profile_best_available', + 'qos_profile_rosout_default'] class rmw_qos_profile_dict(TypedDict): diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index e66e82ae9..403525acc 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -74,7 +74,7 @@ from rclpy.publisher import Publisher from rclpy.qos import qos_profile_parameter_events from rclpy.qos import qos_profile_services_default -from rclpy.qos import qos_profile_system_default +from rclpy.qos import qos_profile_rosout_default from rclpy.qos import QoSProfile from rclpy.qos_overriding_options import _declare_qos_parameters from rclpy.qos_overriding_options import QoSOverridingOptions @@ -140,7 +140,7 @@ def __init__( namespace: Optional[str] = None, use_global_arguments: bool = True, enable_rosout: bool = True, - rosout_qos_profile: Optional[Union[QoSProfile, int]] = qos_profile_system_default, + rosout_qos_profile: Optional[Union[QoSProfile, int]] = qos_profile_rosout_default, start_parameter_services: bool = True, parameter_overrides: Optional[List[Parameter[Any]]] = None, allow_undeclared_parameters: bool = False, diff --git a/rclpy/rclpy/qos.py b/rclpy/rclpy/qos.py index 4c9dc0745..d80f3d162 100644 --- a/rclpy/rclpy/qos.py +++ b/rclpy/rclpy/qos.py @@ -479,13 +479,15 @@ class LivelinessPolicy(QoSPolicyEnum): #: can occur due to races with discovery. qos_profile_best_available = QoSProfile(**_rclpy.rmw_qos_profile_t.predefined( 'qos_profile_best_available').to_dict()) - # Separate rcl_action profile defined at # ros2/rcl : rcl/rcl_action/include/rcl_action/default_qos.h # #: For actions, using reliable reliability, transient-local durability. qos_profile_action_status_default = QoSProfile(**_rclpy.rclpy_action_get_rmw_qos_profile( 'rcl_action_qos_profile_status_default')) +#: The default qos profile setting for topic /rosout publisher. +qos_profile_rosout_default = QoSProfile(**_rclpy.rmw_qos_profile_t.predefined( + 'qos_profile_rosout_default').to_dict()) class QoSPresetProfiles(Enum): @@ -498,6 +500,7 @@ class QoSPresetProfiles(Enum): PARAMETER_EVENTS = qos_profile_parameter_events ACTION_STATUS_DEFAULT = qos_profile_action_status_default BEST_AVAILABLE = qos_profile_best_available + ROSOUT_DEFAULT = qos_profile_rosout_default """Noted that the following are duplicated from QoSPolicyEnum. diff --git a/rclpy/src/rclpy/qos.cpp b/rclpy/src/rclpy/qos.cpp index 324b78013..76778a4d6 100644 --- a/rclpy/src/rclpy/qos.cpp +++ b/rclpy/src/rclpy/qos.cpp @@ -152,6 +152,9 @@ predefined_qos_profile_from_name(const char * qos_profile_name) if (0 == strcmp(qos_profile_name, "qos_profile_best_available")) { return rmw_qos_profile_best_available; } + if (0 == strcmp(qos_profile_name, "qos_profile_rosout_default")) { + return rmw_qos_profile_rosout_default; + } std::string error_text = "Requested unknown rmw_qos_profile: "; error_text += qos_profile_name; diff --git a/rclpy/test/test_create_node.py b/rclpy/test/test_create_node.py index e9fd80a10..308d1f186 100644 --- a/rclpy/test/test_create_node.py +++ b/rclpy/test/test_create_node.py @@ -16,11 +16,15 @@ import rclpy +from rclpy.duration import Duration from rclpy.exceptions import InvalidNamespaceException from rclpy.exceptions import InvalidNodeNameException from rclpy.parameter import Parameter -from rclpy.qos import qos_profile_sensor_data - +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSLivelinessPolicy +from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy class TestCreateNode(unittest.TestCase): @@ -33,6 +37,34 @@ def setUpClass(cls): def tearDownClass(cls): rclpy.shutdown(context=cls.context) + def assert_qos_equal(self, expected_qos_profile, actual_qos_profile, *, is_publisher): + # Depth and history are skipped because they are not retrieved. + self.assertEqual( + expected_qos_profile.durability, + actual_qos_profile.durability, + 'Durability is unequal') + self.assertEqual( + expected_qos_profile.reliability, + actual_qos_profile.reliability, + 'Reliability is unequal') + if is_publisher: + self.assertEqual( + expected_qos_profile.lifespan, + actual_qos_profile.lifespan, + 'lifespan is unequal') + self.assertEqual( + expected_qos_profile.deadline, + actual_qos_profile.deadline, + 'Deadline is unequal') + self.assertEqual( + expected_qos_profile.liveliness, + actual_qos_profile.liveliness, + 'liveliness is unequal') + self.assertEqual( + expected_qos_profile.liveliness_lease_duration, + actual_qos_profile.liveliness_lease_duration, + 'liveliness_lease_duration is unequal') + def test_create_node(self) -> None: node_name = 'create_node_test' rclpy.create_node(node_name, context=self.context).destroy_node() @@ -88,22 +120,33 @@ def test_create_node_disable_rosout(self): namespace = '/ns' node = rclpy.create_node( node_name, namespace=namespace, context=self.context, enable_rosout=False) + # topic /rosout publisher should not exist + self.assertFalse(node.get_publishers_info_by_topic('/rosout')) node.destroy_node() def test_create_node_rosout_qos_profile(self): - node_name = 'create_node_test_rosout_rosout_qos_profile' - namespace = '/ns' - node = rclpy.create_node( - node_name, namespace=namespace, context=self.context, enable_rosout=True, - rosout_qos_profile=qos_profile_sensor_data) - node.destroy_node() - - def test_create_node_rosout_qos_depth(self): - node_name = 'create_node_test_rosout_rosout_qos_depth' + test_qos_profile = QoSProfile( + depth=10, + history=QoSHistoryPolicy.KEEP_ALL, + deadline=Duration(seconds=1, nanoseconds=12345), + lifespan=Duration(seconds=20, nanoseconds=9887665), + reliability=QoSReliabilityPolicy.BEST_EFFORT, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + liveliness_lease_duration=Duration(seconds=5, nanoseconds=23456), + liveliness=QoSLivelinessPolicy.MANUAL_BY_TOPIC) + node_name = 'create_node_test_rosout_qos_profile' namespace = '/ns' node = rclpy.create_node( node_name, namespace=namespace, context=self.context, enable_rosout=True, - rosout_qos_profile=10) + rosout_qos_profile=test_qos_profile) + publisher_list = node.get_publishers_info_by_topic('/rosout') + # only test node /rosout topic publisher should exist + self.assertEqual(1, len(publisher_list)) + self.assertEqual(node.get_name(), publisher_list[0].node_name) + self.assertEqual(node.get_namespace(), publisher_list[0].node_namespace) + actual_qos_profile = publisher_list[0].qos_profile + # QoS should match except depth and history cz that are not retrieved from rmw. + self.assert_qos_equal(test_qos_profile, actual_qos_profile, is_publisher=True) node.destroy_node() From 774696e8ad123a9c702e560566a99fd7eb67c02d Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Wed, 6 Nov 2024 12:51:43 -0800 Subject: [PATCH 4/4] address flake8 warnings. Signed-off-by: Tomoya Fujita --- rclpy/rclpy/node.py | 2 +- rclpy/test/test_create_node.py | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index 403525acc..528e8500c 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -73,8 +73,8 @@ from rclpy.parameter_service import ParameterService from rclpy.publisher import Publisher from rclpy.qos import qos_profile_parameter_events -from rclpy.qos import qos_profile_services_default from rclpy.qos import qos_profile_rosout_default +from rclpy.qos import qos_profile_services_default from rclpy.qos import QoSProfile from rclpy.qos_overriding_options import _declare_qos_parameters from rclpy.qos_overriding_options import QoSOverridingOptions diff --git a/rclpy/test/test_create_node.py b/rclpy/test/test_create_node.py index 308d1f186..248ccfaf4 100644 --- a/rclpy/test/test_create_node.py +++ b/rclpy/test/test_create_node.py @@ -26,6 +26,7 @@ from rclpy.qos import QoSProfile from rclpy.qos import QoSReliabilityPolicy + class TestCreateNode(unittest.TestCase): @classmethod