Skip to content

Commit

Permalink
refactor: make ros2param use rclpy.parameter_client (#716)
Browse files Browse the repository at this point in the history 
Refactors ros2param to use rclpy.parameter_client (ros2/rclpy@3053a8a). 



Co-authored-by: Jacob Perron <[email protected]>
Signed-off-by: Brian Chen <[email protected]>
  • Loading branch information
@ihasdapie @jacobperron
ihasdapie and jacobperron authored Jul 22, 2022
1 parent c7d3204 commit fc02ffe
Show file tree
Hide file tree
Showing 6 changed files with 96 additions and 252 deletions.
225 changes: 55 additions & 170 deletions ros2param/ros2param/api/__init__.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,113 +13,44 @@
# limitations under the License.

import sys
import warnings

from rcl_interfaces.msg import Parameter
from rcl_interfaces.msg import ParameterType
from rcl_interfaces.msg import ParameterValue
from rcl_interfaces.srv import DescribeParameters
from rcl_interfaces.srv import GetParameters
from rcl_interfaces.srv import ListParameters
from rcl_interfaces.srv import SetParameters
import rclpy
from rclpy.parameter import PARAMETER_SEPARATOR_STRING
from rclpy.parameter import get_parameter_value as rclpy_get_parameter_value
from rclpy.parameter import parameter_dict_from_yaml_file
from rclpy.parameter import parameter_value_to_python
from rclpy.parameter_client import AsyncParameterClient
from ros2cli.node.direct import DirectNode

import yaml

def get_parameter_value(*, string_value):
warnings.warn('get_parameter_value() is deprecated. '
'Use rclpy.parameter.get_parameter_value instead')
return rclpy_get_parameter_value(string_value)


def get_value(*, parameter_value):
"""Get the value from a ParameterValue."""
if parameter_value.type == ParameterType.PARAMETER_BOOL:
value = parameter_value.bool_value
elif parameter_value.type == ParameterType.PARAMETER_INTEGER:
value = parameter_value.integer_value
elif parameter_value.type == ParameterType.PARAMETER_DOUBLE:
value = parameter_value.double_value
elif parameter_value.type == ParameterType.PARAMETER_STRING:
value = parameter_value.string_value
elif parameter_value.type == ParameterType.PARAMETER_BYTE_ARRAY:
value = list(parameter_value.byte_array_value)
elif parameter_value.type == ParameterType.PARAMETER_BOOL_ARRAY:
value = list(parameter_value.bool_array_value)
elif parameter_value.type == ParameterType.PARAMETER_INTEGER_ARRAY:
value = list(parameter_value.integer_array_value)
elif parameter_value.type == ParameterType.PARAMETER_DOUBLE_ARRAY:
value = list(parameter_value.double_array_value)
elif parameter_value.type == ParameterType.PARAMETER_STRING_ARRAY:
value = list(parameter_value.string_array_value)
elif parameter_value.type == ParameterType.PARAMETER_NOT_SET:
value = None
else:
value = None

return value


def get_parameter_value(*, string_value):
"""Guess the desired type of the parameter based on the string value."""
value = ParameterValue()
value = None
try:
yaml_value = yaml.safe_load(string_value)
except yaml.parser.ParserError:
yaml_value = string_value

if isinstance(yaml_value, bool):
value.type = ParameterType.PARAMETER_BOOL
value.bool_value = yaml_value
elif isinstance(yaml_value, int):
value.type = ParameterType.PARAMETER_INTEGER
value.integer_value = yaml_value
elif isinstance(yaml_value, float):
value.type = ParameterType.PARAMETER_DOUBLE
value.double_value = yaml_value
elif isinstance(yaml_value, list):
if all((isinstance(v, bool) for v in yaml_value)):
value.type = ParameterType.PARAMETER_BOOL_ARRAY
value.bool_array_value = yaml_value
elif all((isinstance(v, int) for v in yaml_value)):
value.type = ParameterType.PARAMETER_INTEGER_ARRAY
value.integer_array_value = yaml_value
elif all((isinstance(v, float) for v in yaml_value)):
value.type = ParameterType.PARAMETER_DOUBLE_ARRAY
value.double_array_value = yaml_value
elif all((isinstance(v, str) for v in yaml_value)):
value.type = ParameterType.PARAMETER_STRING_ARRAY
value.string_array_value = yaml_value
else:
value.type = ParameterType.PARAMETER_STRING
value.string_value = string_value
else:
value.type = ParameterType.PARAMETER_STRING
value.string_value = yaml_value
value = parameter_value_to_python(parameter_value)
except RuntimeError as e:
print(f'Runtime error {e} raised')
return value


def parse_parameter_dict(*, namespace, parameter_dict):
parameters = []
for param_name, param_value in parameter_dict.items():
full_param_name = namespace + param_name
# Unroll nested parameters
if type(param_value) == dict:
parameters += parse_parameter_dict(
namespace=full_param_name + PARAMETER_SEPARATOR_STRING,
parameter_dict=param_value)
else:
parameter = Parameter()
parameter.name = full_param_name
parameter.value = get_parameter_value(string_value=str(param_value))
parameters.append(parameter)
return parameters


def load_parameter_dict(*, node, node_name, parameter_dict):

parameters = parse_parameter_dict(namespace='', parameter_dict=parameter_dict)
response = call_set_parameters(
node=node, node_name=node_name, parameters=parameters)

# output response
assert len(response.results) == len(parameters)
def load_parameter_file(*, node, node_name, parameter_file, use_wildcard):
client = AsyncParameterClient(node, node_name)
ready = client.wait_for_services(timeout_sec=5.0)
if not ready:
raise RuntimeError('Wait for service timed out waiting for '
f'parameter services for node {node_name}')
future = client.load_parameter_file(parameter_file, use_wildcard)
parameters = list(parameter_dict_from_yaml_file(parameter_file, use_wildcard).values())
rclpy.spin_until_future_complete(node, future)
response = future.result()
assert len(response.results) == len(parameters), 'Not all parameters set'
for i in range(0, len(response.results)):
result = response.results[i]
param_name = parameters[i].name
Expand All @@ -135,105 +66,59 @@ def load_parameter_dict(*, node, node_name, parameter_dict):
print(msg, file=sys.stderr)


def load_parameter_file(*, node, node_name, parameter_file, use_wildcard):
# Remove leading slash and namespaces
with open(parameter_file, 'r') as f:
param_file = yaml.safe_load(f)
param_keys = []
if use_wildcard and '/**' in param_file:
param_keys.append('/**')
if node_name in param_file:
param_keys.append(node_name)

if param_keys == []:
raise RuntimeError('Param file does not contain parameters for {}, '
' only for nodes: {}' .format(node_name, param_file.keys()))
param_dict = {}
for k in param_keys:
value = param_file[k]
if type(value) != dict or 'ros__parameters' not in value:
raise RuntimeError('Invalid structure of parameter file for node {}'
'expected same format as provided by ros2 param dump'
.format(k))
param_dict.update(value['ros__parameters'])
load_parameter_dict(node=node, node_name=node_name, parameter_dict=param_dict)


def call_describe_parameters(*, node, node_name, parameter_names=None):
# create client
client = node.create_client(
DescribeParameters, f'{node_name}/describe_parameters')

# call as soon as ready
ready = client.wait_for_service(timeout_sec=5.0)
client = AsyncParameterClient(node, node_name)
ready = client.wait_for_services(timeout_sec=5.0)
if not ready:
raise RuntimeError('Wait for service timed out')

request = DescribeParameters.Request()
if parameter_names:
request.names = parameter_names
future = client.call_async(request)
raise RuntimeError('Wait for service timed out waiting for '
f'parameter services for node {node_name}')
future = client.describe_parameters(parameter_names)
rclpy.spin_until_future_complete(node, future)

# handle response
response = future.result()
if response is None:
raise RuntimeError('Exception while calling service of node '
f'{node_name}: {future.exception()}')
return response


def call_get_parameters(*, node, node_name, parameter_names):
# create client
client = node.create_client(GetParameters, f'{node_name}/get_parameters')

# call as soon as ready
ready = client.wait_for_service(timeout_sec=5.0)
client = AsyncParameterClient(node, node_name)
ready = client.wait_for_services(timeout_sec=5.0)
if not ready:
raise RuntimeError('Wait for service timed out')

request = GetParameters.Request()
request.names = parameter_names
future = client.call_async(request)
raise RuntimeError('Wait for service timed out waiting for '
f'parameter services for node {node_name}')
future = client.get_parameters(parameter_names)
rclpy.spin_until_future_complete(node, future)

# handle response
response = future.result()
if response is None:
raise RuntimeError('Exception while calling service of node '
f'{node_name}: {future.exception()}')
return response


def call_set_parameters(*, node, node_name, parameters):
# create client
client = node.create_client(SetParameters, f'{node_name}/set_parameters')

# call as soon as ready
ready = client.wait_for_service(timeout_sec=5.0)
client = AsyncParameterClient(node, node_name)
ready = client.wait_for_services(timeout_sec=5.0)
if not ready:
raise RuntimeError('Wait for service timed out')

request = SetParameters.Request()
request.parameters = parameters
future = client.call_async(request)
raise RuntimeError('Wait for service timed out waiting for '
f'parameter services for node {node_name}')
future = client.set_parameters(parameters)
rclpy.spin_until_future_complete(node, future)

# handle response
response = future.result()
if response is None:
raise RuntimeError('Exception while calling service of node '
f'{node_name}: {future.exception()}')
return response


def call_list_parameters(*, node, node_name, prefix=None):
# create client
client = node.create_client(ListParameters, f'{node_name}/list_parameters')

# call as soon as ready
ready = client.wait_for_service(timeout_sec=5.0)
def call_list_parameters(*, node, node_name, prefixes=None):
client = AsyncParameterClient(node, node_name)
ready = client.wait_for_services(timeout_sec=5.0)
if not ready:
raise RuntimeError('Wait for service timed out')

request = ListParameters.Request()
future = client.call_async(request)
return None
future = client.list_parameters(prefixes=prefixes)
rclpy.spin_until_future_complete(node, future)

# handle response
response = future.result()
return response.result.names
return future


def get_parameter_type_string(parameter_type):
Expand Down
51 changes: 19 additions & 32 deletions ros2param/ros2param/verb/dump.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,24 +15,19 @@
import os
import sys

from rcl_interfaces.srv import ListParameters

import rclpy
from rclpy.parameter import PARAMETER_SEPARATOR_STRING

from ros2cli.node.direct import DirectNode
from ros2cli.node.strategy import add_arguments
from ros2cli.node.strategy import NodeStrategy

from ros2node.api import get_absolute_node_name
from ros2node.api import get_node_names
from ros2node.api import NodeNameCompleter
from ros2node.api import parse_node_name

from ros2param.api import call_get_parameters
from ros2param.api import call_list_parameters
from ros2param.api import get_value
from ros2param.verb import VerbExtension

import yaml


Expand All @@ -57,17 +52,17 @@ def add_arguments(self, parser, cli_name): # noqa: D102
help='DEPRECATED: Does nothing.')

@staticmethod
def get_parameter_value(node, node_name, param):
def get_parameter_values(node, node_name, params):
response = call_get_parameters(
node=node, node_name=node_name,
parameter_names=[param])
parameter_names=params)

# requested parameter not set
if not response.values:
return '# Parameter not set'

# extract type specific value
return get_value(parameter_value=response.values[0])
return [get_value(parameter_value=i) for i in response.values]

def insert_dict(self, dictionary, key, value):
split = key.split(PARAMETER_SEPARATOR_STRING, 1)
Expand All @@ -94,36 +89,28 @@ def main(self, *, args): # noqa: D102
f"'{args.output_dir}' is not a valid directory.")

with DirectNode(args) as node:
# create client
service_name = f'{absolute_node_name}/list_parameters'
client = node.create_client(ListParameters, service_name)

client.wait_for_service()

if not client.service_is_ready():
raise RuntimeError(f"Could not reach service '{service_name}'")

request = ListParameters.Request()
future = client.call_async(request)

# wait for response
rclpy.spin_until_future_complete(node, future)

yaml_output = {node_name.full_name: {'ros__parameters': {}}}

# retrieve values
if future.result() is not None:
response = future.result()
for param_name in sorted(response.result.names):
pval = self.get_parameter_value(node, absolute_node_name, param_name)
self.insert_dict(
yaml_output[node_name.full_name]['ros__parameters'], param_name, pval)
else:
e = future.exception()
response = call_list_parameters(node=node, node_name=absolute_node_name)
if response is None:
raise RuntimeError(
'Wait for service timed out waiting for '
f'parameter services for node {node_name.full_name}')
elif response.result() is None:
e = response.exception()
raise RuntimeError(
'Exception while calling service of node '
f"'{node_name.full_name}': {e}")

response = response.result().result.names
response = sorted(response)
parameter_values = self.get_parameter_values(node, absolute_node_name, response)

for param_name, pval in zip(response, parameter_values):
self.insert_dict(
yaml_output[node_name.full_name]['ros__parameters'], param_name, pval)

if args.print:
print(
"WARNING: '--print' is deprecated; this utility prints to stdout by default",
Expand Down
Loading

0 comments on commit fc02ffe

Please sign in to comment.