Skip to content

Commit

Permalink
Implementing new parameter API.
Browse files Browse the repository at this point in the history
- See issue #321 for details.

Signed-off-by: Juan Ignacio Ubeira <jubeira@ekumenlabs.com>
  • Loading branch information
Juan Ignacio Ubeira committed Apr 30, 2019
1 parent 8971234 commit 89eb719
Show file tree
Hide file tree
Showing 12 changed files with 370 additions and 32 deletions.
7 changes: 5 additions & 2 deletions rclpy/rclpy/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -111,7 +111,8 @@ def create_node(
namespace: str = None,
use_global_arguments: bool = True,
start_parameter_services: bool = True,
initial_parameters: List[Parameter] = None
initial_parameters: List[Parameter] = None,
allow_undeclared_parameters: bool = False
) -> 'Node':
"""
Create an instance of :class:`.Node`.
Expand All @@ -126,6 +127,7 @@ def create_node(
arguments.
:param start_parameter_services: ``False`` if the node should not create parameter services.
:param initial_parameters: A list of :class:`.Parameter` to be set during node creation.
:param allow_undeclared_parameters: True if undeclared parameters are allowed, False otherwise.
:return: An instance of the newly created node.
"""
# imported locally to avoid loading extensions on module import
Expand All @@ -134,7 +136,8 @@ def create_node(
node_name, context=context, cli_args=cli_args, namespace=namespace,
use_global_arguments=use_global_arguments,
start_parameter_services=start_parameter_services,
initial_parameters=initial_parameters)
initial_parameters=initial_parameters,
allow_undeclared_parameters=allow_undeclared_parameters)


def spin_once(node: 'Node', *, executor: 'Executor' = None, timeout_sec: float = None) -> None:
Expand Down
36 changes: 36 additions & 0 deletions rclpy/rclpy/exceptions.py
Original file line number Diff line number Diff line change
Expand Up @@ -65,3 +65,39 @@ class InvalidServiceNameException(NameValidationException):

def __init__(self, name, error_msg, invalid_index, *args):
NameValidationException.__init__(self, 'service name', name, error_msg, invalid_index)


# TODO(jubeira): Polish parameter-related exceptions; improve error messages.
class ParameterNotDeclaredException(Exception):
"""Raised when handling an undeclared parameter when it is not allowed."""

def __init__(self, *args):
Exception.__init__(self, 'Invalid access to undeclared parameter', *args)


class ParameterAlreadyDeclaredException(Exception):
"""Raised when declaring a parameter that had been declared before."""

def __init__(self, *args):
Exception.__init__(self, 'Parameter already declared.', *args)


class InvalidParameterException(Exception):
"""Raised when a parameter to be declared has an invalid name."""

def __init__(self, *args):
Exception.__init__(self, 'Invalid parameter name.', *args)


class InvalidParameterValueException(Exception):
"""Raised when a parameter to be declared is rejected by a callback."""

def __init__(self, *args):
Exception.__init__(self, 'Invalid parameter value.', *args)


class ParameterImmutableException(Exception):
"""Raised when a read-only parameter is modified."""

def __init__(self, *args):
Exception.__init__(self, 'Attempted to modify an read-only parameter.', *args)
275 changes: 258 additions & 17 deletions rclpy/rclpy/node.py

Large diffs are not rendered by default.

17 changes: 14 additions & 3 deletions rclpy/rclpy/parameter.py
Original file line number Diff line number Diff line change
Expand Up @@ -86,7 +86,14 @@ def from_parameter_msg(cls, param_msg):
value = param_msg.value.string_array_value
return cls(param_msg.name, type_, value)

def __init__(self, name, type_, value=None):
def __init__(self, name, type_, value=None, descriptor=ParameterDescriptor()):
"""
Constructor.
The name and type in the given rcl_interfaces.msg.ParameterDescriptor
are ignored, and should be specified using the named arguments to this
function.
"""
if not isinstance(type_, Parameter.Type):
raise TypeError("type must be an instance of '{}'".format(repr(Parameter.Type)))

Expand All @@ -96,6 +103,9 @@ def __init__(self, name, type_, value=None):
self._type_ = type_
self._name = name
self._value = value
descriptor.name = name
descriptor.type = type_.value
self._descriptor = descriptor

@property
def name(self):
Expand All @@ -109,8 +119,9 @@ def type_(self):
def value(self):
return self._value

def get_descriptor(self):
return ParameterDescriptor(name=self.name, type=self.type_.value)
@property
def descriptor(self):
return self._descriptor

def get_parameter_value(self):
parameter_value = ParameterValue(type=self.type_.value)
Expand Down
8 changes: 4 additions & 4 deletions rclpy/rclpy/parameter_service.py
Original file line number Diff line number Diff line change
Expand Up @@ -62,19 +62,19 @@ def __init__(self, node):

def _describe_parameters_callback(self, request, response):
for name in request.names:
p = self._node.get_parameter(name)
response.descriptors.append(p.get_descriptor())
p = self._node.get_parameter_or(name)
response.descriptors.append(p.descriptor)
return response

def _get_parameters_callback(self, request, response):
for name in request.names:
p = self._node.get_parameter(name)
p = self._node.get_parameter_or(name)
response.values.append(p.get_parameter_value())
return response

def _get_parameter_types_callback(self, request, response):
for name in request.names:
response.types.append(self._node.get_parameter(name).get_parameter_type())
response.types.append(self._node.get_parameter_or(name).type_)
return response

def _list_parameters_callback(self, request, response):
Expand Down
2 changes: 1 addition & 1 deletion rclpy/rclpy/time_source.py
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,7 @@ def attach_node(self, node):
self.detach_node()
self._node = node

use_sim_time_param = node.get_parameter('use_sim_time')
use_sim_time_param = node.get_parameter_or('use_sim_time')
if use_sim_time_param.type_ != Parameter.Type.NOT_SET:
if use_sim_time_param.type_ == Parameter.Type.BOOL:
self.ros_time_is_active = use_sim_time_param.value
Expand Down
39 changes: 39 additions & 0 deletions rclpy/rclpy/validate_parameter_name.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,39 @@
# 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.

from rclpy.exceptions import InvalidParameterException
from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy


def validate_parameter_name(name):
"""
Validate a given parameter name, and raise an exception if invalid.
The name does not have to be fully-qualified and is not expanded.
If the name is invalid then rclpy.exceptions.InvalidParameterNameException
will be raised.
:param name str: topic or service name to be validated
:param is_service bool: if true, InvalidServiceNameException is raised
:returns: True when it is valid
:raises: InvalidParameterNameException: when the name is invalid
"""
# TODO(jubeira): define whether this is an appropriate method to validate
# parameter names.
result = _rclpy.rclpy_get_validation_error_for_topic_name(name)
if result is None:
return True
error_msg, invalid_index = result
raise InvalidParameterException(name, error_msg, invalid_index)
3 changes: 2 additions & 1 deletion rclpy/test/test_create_node.py
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,8 @@ def test_create_node_with_namespace(self):
def test_create_node_with_empty_namespace(self):
node_name = 'create_node_test'
namespace = ''
node = rclpy.create_node(node_name, namespace=namespace, context=self.context)
node = rclpy.create_node(
node_name, namespace=namespace, context=self.context)
self.assertEqual('/', node.get_namespace())
node.destroy_node()

Expand Down
4 changes: 3 additions & 1 deletion rclpy/test/test_node.py
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,9 @@ class TestNode(unittest.TestCase):
def setUpClass(cls):
cls.context = rclpy.context.Context()
rclpy.init(context=cls.context)
cls.node = rclpy.create_node(TEST_NODE, namespace=TEST_NAMESPACE, context=cls.context)
cls.node = rclpy.create_node(
TEST_NODE, namespace=TEST_NAMESPACE, context=cls.context,
allow_undeclared_parameters=True)

@classmethod
def tearDownClass(cls):
Expand Down
3 changes: 2 additions & 1 deletion rclpy/test/test_parameters_callback.py
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,8 @@ def tearDownClass(cls):
rclpy.shutdown(context=cls.context)

def setUp(self):
self.node = rclpy.create_node('parameters_callback_node', context=self.context)
self.node = rclpy.create_node(
'parameters_callback_node', context=self.context, allow_undeclared_parameters=True)

def tearDown(self):
self.node.destroy_node()
Expand Down
4 changes: 3 additions & 1 deletion rclpy/test/test_time_source.py
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,9 @@ class TestTimeSource(unittest.TestCase):
def setUp(self):
self.context = rclpy.context.Context()
rclpy.init(context=self.context)
self.node = rclpy.create_node('TestTimeSource', namespace='/rclpy', context=self.context)
self.node = rclpy.create_node(
'TestTimeSource', namespace='/rclpy', context=self.context,
allow_undeclared_parameters=True)

def tearDown(self):
self.node.destroy_node()
Expand Down
4 changes: 3 additions & 1 deletion rclpy/test/test_waitable.py
Original file line number Diff line number Diff line change
Expand Up @@ -282,7 +282,9 @@ class TestWaitable(unittest.TestCase):
def setUpClass(cls):
cls.context = rclpy.context.Context()
rclpy.init(context=cls.context)
cls.node = rclpy.create_node('TestWaitable', namespace='/rclpy/test', context=cls.context)
cls.node = rclpy.create_node(
'TestWaitable', namespace='/rclpy/test', context=cls.context,
allow_undeclared_parameters=True)
cls.executor = SingleThreadedExecutor(context=cls.context)
cls.executor.add_node(cls.node)

Expand Down

0 comments on commit 89eb719

Please sign in to comment.