diff --git a/rclpy/CMakeLists.txt b/rclpy/CMakeLists.txt index 690ef6dd5..e11db0992 100644 --- a/rclpy/CMakeLists.txt +++ b/rclpy/CMakeLists.txt @@ -194,6 +194,7 @@ if(BUILD_TESTING) test/test_handle.py test/test_init_shutdown.py test/test_logging.py + test/test_logging_rosout.py test/test_messages.py test/test_node.py test/test_parameter.py diff --git a/rclpy/rclpy/__init__.py b/rclpy/rclpy/__init__.py index 38f6cd406..83b2e14d3 100644 --- a/rclpy/rclpy/__init__.py +++ b/rclpy/rclpy/__init__.py @@ -110,6 +110,7 @@ def create_node( cli_args: List[str] = None, namespace: str = None, use_global_arguments: bool = True, + enable_rosout: bool = True, start_parameter_services: bool = True, parameter_overrides: List[Parameter] = None, allow_undeclared_parameters: bool = False, @@ -127,6 +128,7 @@ def create_node( (node name, topics, etc). :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 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. @@ -141,6 +143,7 @@ def create_node( return Node( node_name, context=context, cli_args=cli_args, namespace=namespace, use_global_arguments=use_global_arguments, + enable_rosout=enable_rosout, 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 94afaf499..6a12b3c5d 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -108,6 +108,7 @@ def __init__( cli_args: List[str] = None, namespace: str = None, use_global_arguments: bool = True, + enable_rosout: bool = True, start_parameter_services: bool = True, parameter_overrides: List[Parameter] = None, allow_undeclared_parameters: bool = False, @@ -126,6 +127,7 @@ def __init__( Validated by :func:`validate_namespace`. :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 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 @@ -157,7 +159,12 @@ def __init__( raise NotInitializedException('cannot create node') try: self.__handle = Handle(_rclpy.rclpy_create_node( - node_name, namespace, self._context.handle, cli_args, use_global_arguments + node_name, + namespace, + 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 diff --git a/rclpy/src/rclpy/_rclpy.c b/rclpy/src/rclpy/_rclpy.c index 4d24d510b..6fa7f7228 100644 --- a/rclpy/src/rclpy/_rclpy.c +++ b/rclpy/src/rclpy/_rclpy.c @@ -637,9 +637,16 @@ rclpy_create_node(PyObject * Py_UNUSED(self), PyObject * args) PyObject * pycontext; PyObject * py_cli_args; int use_global_arguments; + int enable_rosout; if (!PyArg_ParseTuple( - args, "ssOOp", &node_name, &namespace_, &pycontext, &py_cli_args, &use_global_arguments)) + args, "ssOOpp", + &node_name, + &namespace_, + &pycontext, + &py_cli_args, + &use_global_arguments, + &enable_rosout)) { return NULL; } @@ -666,6 +673,7 @@ rclpy_create_node(PyObject * Py_UNUSED(self), PyObject * args) rcl_node_options_t options = rcl_node_get_default_options(); options.use_global_arguments = use_global_arguments; options.arguments = arguments; + options.enable_rosout = enable_rosout; ret = rcl_node_init(node, node_name, namespace_, context, &options); if (ret != RCL_RET_OK) { if (ret == RCL_RET_BAD_ALLOC) { diff --git a/rclpy/test/test_logging_rosout.py b/rclpy/test/test_logging_rosout.py new file mode 100644 index 000000000..41ac6c9c2 --- /dev/null +++ b/rclpy/test/test_logging_rosout.py @@ -0,0 +1,85 @@ +# 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. + +import pytest + +from rcl_interfaces.msg import Log +import rclpy +from rclpy.executors import SingleThreadedExecutor + +TEST_PARAMETERS = [ + ('enable_global_rosout_enable_node_rosout', True, True, True), + ('enable_global_rosout_disable_node_rosout', True, False, False), + ('disable_global_rosout_enable_node_rosout', False, True, False), + ('disable_global_rosout_disable_node_rosout', False, False, False), +] + +raw_subscription_msg = None # None=No result yet + + +def raw_subscription_callback(msg): + global raw_subscription_msg + raw_subscription_msg = msg + + +@pytest.mark.parametrize( + 'name,enable_global_rosout_logs,enable_node_rosout,expected_data', + TEST_PARAMETERS) +def test_enable_rosout( + name, + enable_global_rosout_logs, + enable_node_rosout, + expected_data +): + if enable_global_rosout_logs: + args = ['--ros-args', '--enable-rosout-logs'] + else: + args = ['--ros-args', '--disable-rosout-logs'] + + context = rclpy.context.Context() + rclpy.init(context=context, args=args) + executor = SingleThreadedExecutor(context=context) + + # create node + node = rclpy.create_node( + node_name='my_node_'+name, + namespace='/my_ns', + enable_rosout=enable_node_rosout, + context=context + ) + executor.add_node(node) + + global raw_subscription_msg + raw_subscription_msg = None + # create subscriber of 'rosout' topic + node.create_subscription( + Log, + 'rosout', + raw_subscription_callback, + 1, + raw=True + ) + + node.get_logger().info('SOMETHING') + executor.spin_once(timeout_sec=1) + + if expected_data: + assert (raw_subscription_msg is not None) + assert (type(raw_subscription_msg) == bytes) + assert (len(raw_subscription_msg) != 0) + else: + assert (raw_subscription_msg is None) + + node.destroy_node() + rclpy.shutdown(context=context)