Skip to content

Commit

Permalink
Replace deprecated spin_until_future_complete with `spin_until_comp…
Browse files Browse the repository at this point in the history
…lete`

Signed-off-by: Hubert Liberacki <[email protected]>
  • Loading branch information
hliberacki committed Mar 31, 2022
1 parent 770bb42 commit 00b4317
Show file tree
Hide file tree
Showing 9 changed files with 76 additions and 52 deletions.
15 changes: 9 additions & 6 deletions ros2action/ros2action/verb/send_goal.py
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,8 @@ def add_arguments(self, parser, cli_name):
arg = parser.add_argument(
'goal',
help="Goal request values in YAML format (e.g. '{order: 10}')")
arg.completer = ActionGoalPrototypeCompleter(action_type_key='action_type')
arg.completer = ActionGoalPrototypeCompleter(
action_type_key='action_type')
parser.add_argument(
'-f', '--feedback', action='store_true',
help='Echo feedback messages for the goal')
Expand Down Expand Up @@ -106,7 +107,7 @@ def send_goal(action_name, action_type, goal_values, feedback_callback):

print('Sending goal:\n {}'.format(message_to_yaml(goal)))
goal_future = action_client.send_goal_async(goal, feedback_callback)
rclpy.spin_until_future_complete(node, goal_future)
rclpy.spin_until_complete(node, goal_future)

goal_handle = goal_future.result()

Expand All @@ -120,10 +121,11 @@ def send_goal(action_name, action_type, goal_values, feedback_callback):
goal_handle = None
return

print('Goal accepted with ID: {}\n'.format(bytes(goal_handle.goal_id.uuid).hex()))
print('Goal accepted with ID: {}\n'.format(
bytes(goal_handle.goal_id.uuid).hex()))

result_future = goal_handle.get_result_async()
rclpy.spin_until_future_complete(node, result_future)
rclpy.spin_until_complete(node, result_future)

result = result_future.result()

Expand All @@ -135,15 +137,16 @@ def send_goal(action_name, action_type, goal_values, feedback_callback):
goal_handle = None

print('Result:\n {}'.format(message_to_yaml(result.result)))
print('Goal finished with status: {}'.format(_goal_status_to_string(result.status)))
print('Goal finished with status: {}'.format(
_goal_status_to_string(result.status)))
finally:
# Cancel the goal if it's still active
if (goal_handle is not None and
(GoalStatus.STATUS_ACCEPTED == goal_handle.status or
GoalStatus.STATUS_EXECUTING == goal_handle.status)):
print('Canceling goal...')
cancel_future = goal_handle.cancel_goal_async()
rclpy.spin_until_future_complete(node, cancel_future)
rclpy.spin_until_complete(node, cancel_future)

cancel_response = cancel_future.result()

Expand Down
18 changes: 11 additions & 7 deletions ros2component/ros2component/api/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,8 @@ def get_package_component_types(*, package_name=None):
"""
if not has_resource(COMPONENTS_RESOURCE_TYPE, package_name):
return []
component_registry, _ = get_resource(COMPONENTS_RESOURCE_TYPE, package_name)
component_registry, _ = get_resource(
COMPONENTS_RESOURCE_TYPE, package_name)
return [line.split(';')[0] for line in component_registry.splitlines()]


Expand Down Expand Up @@ -174,7 +175,7 @@ def _resume(to_completion=False):

timer = node.create_timer(timer_period_sec=0.1, callback=resume)
try:
rclpy.spin_until_future_complete(node, future, timeout_sec=5.0)
rclpy.spin_until_complete(node, future, timeout_sec=5.0)
if not future.done():
resume(to_completion=True)
return dict(future.result())
Expand Down Expand Up @@ -244,10 +245,11 @@ def load_component_into_container(
arg_msg.name = name
request.extra_arguments.append(arg_msg)
future = load_node_client.call_async(request)
rclpy.spin_until_future_complete(node, future)
rclpy.spin_until_complete(node, future)
response = future.result()
if not response.success:
raise RuntimeError('Failed to load component: ' + response.error_message.capitalize())
raise RuntimeError('Failed to load component: ' +
response.error_message.capitalize())
return response.unique_id, response.full_node_name
finally:
node.destroy_client(load_node_client)
Expand All @@ -274,7 +276,7 @@ def unload_component_from_container(*, node, remote_container_node_name, compone
request = composition_interfaces.srv.UnloadNode.Request()
request.unique_id = uid
future = unload_node_client.call_async(request)
rclpy.spin_until_future_complete(node, future)
rclpy.spin_until_complete(node, future)
response = future.result()
yield uid, not response.success, response.error_message
finally:
Expand Down Expand Up @@ -349,7 +351,8 @@ def add_component_arguments(parser):
argument = parser.add_argument(
'plugin_name', help='Type name of the component to be loaded'
)
argument.completer = ComponentTypeNameCompleter(package_name_key='package_name')
argument.completer = ComponentTypeNameCompleter(
package_name_key='package_name')
parser.add_argument('-n', '--node-name', help='Component node name')
parser.add_argument('--node-namespace', help='Component node namespace')
parser.add_argument('--log-level', help='Component node log level')
Expand All @@ -374,7 +377,8 @@ def run_standalone_container(*, container_node_name):
except PackageNotFound:
raise RuntimeError("Package 'rclcpp_components' not found")

executable_path = next((p for p in paths if 'component_container' in p), None)
executable_path = next(
(p for p in paths if 'component_container' in p), None)
if executable_path is None:
raise RuntimeError('No component container node found!')

Expand Down
6 changes: 3 additions & 3 deletions ros2lifecycle/ros2lifecycle/api/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,7 @@ def call_get_states(*, node, node_names):

# wait for all responses
for future in futures.values():
rclpy.spin_until_future_complete(node, future)
rclpy.spin_until_complete(node, future)

# return current state or exception for each node
states = {}
Expand Down Expand Up @@ -112,7 +112,7 @@ def _call_get_transitions(node, states, service_name):

# wait for all responses
for future in futures.values():
rclpy.spin_until_future_complete(node, future)
rclpy.spin_until_complete(node, future)

# return transitions from current state or exception for each node
transitions = {}
Expand Down Expand Up @@ -157,7 +157,7 @@ def call_change_states(*, node, transitions):

# wait for all responses
for future in futures.values():
rclpy.spin_until_future_complete(node, future)
rclpy.spin_until_complete(node, future)

# return success flag or exception for each node
results = {}
Expand Down
21 changes: 12 additions & 9 deletions ros2param/ros2param/api/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -102,19 +102,21 @@ def parse_parameter_dict(*, namespace, parameter_dict):
# Unroll nested parameters
if type(param_value) == dict:
parameters += parse_parameter_dict(
namespace=full_param_name + PARAMETER_SEPARATOR_STRING,
parameter_dict=param_value)
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))
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)
parameters = parse_parameter_dict(
namespace='', parameter_dict=parameter_dict)
response = call_set_parameters(
node=node, node_name=node_name, parameters=parameters)

Expand Down Expand Up @@ -156,7 +158,8 @@ def load_parameter_file(*, node, node_name, parameter_file, use_wildcard):
'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)
load_parameter_dict(node=node, node_name=node_name,
parameter_dict=param_dict)


def call_describe_parameters(*, node, node_name, parameter_names=None):
Expand All @@ -173,7 +176,7 @@ def call_describe_parameters(*, node, node_name, parameter_names=None):
if parameter_names:
request.names = parameter_names
future = client.call_async(request)
rclpy.spin_until_future_complete(node, future)
rclpy.spin_until_complete(node, future)

# handle response
response = future.result()
Expand All @@ -192,7 +195,7 @@ def call_get_parameters(*, node, node_name, parameter_names):
request = GetParameters.Request()
request.names = parameter_names
future = client.call_async(request)
rclpy.spin_until_future_complete(node, future)
rclpy.spin_until_complete(node, future)

# handle response
response = future.result()
Expand All @@ -211,7 +214,7 @@ def call_set_parameters(*, node, node_name, parameters):
request = SetParameters.Request()
request.parameters = parameters
future = client.call_async(request)
rclpy.spin_until_future_complete(node, future)
rclpy.spin_until_complete(node, future)

# handle response
response = future.result()
Expand All @@ -229,7 +232,7 @@ def call_list_parameters(*, node, node_name, prefix=None):

request = ListParameters.Request()
future = client.call_async(request)
rclpy.spin_until_future_complete(node, future)
rclpy.spin_until_complete(node, future)

# handle response
response = future.result()
Expand Down
11 changes: 7 additions & 4 deletions ros2param/ros2param/verb/dump.py
Original file line number Diff line number Diff line change
Expand Up @@ -81,7 +81,8 @@ def insert_dict(self, dictionary, key, value):
def main(self, *, args): # noqa: D102

with NodeStrategy(args) as node:
node_names = get_node_names(node=node, include_hidden_nodes=args.include_hidden_nodes)
node_names = get_node_names(
node=node, include_hidden_nodes=args.include_hidden_nodes)

absolute_node_name = get_absolute_node_name(args.node_name)
node_name = parse_node_name(absolute_node_name)
Expand All @@ -107,15 +108,16 @@ def main(self, *, args): # noqa: D102
future = client.call_async(request)

# wait for response
rclpy.spin_until_future_complete(node, future)
rclpy.spin_until_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)
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:
Expand All @@ -142,6 +144,7 @@ def main(self, *, args): # noqa: D102
else:
file_name = absolute_node_name.replace('/', '__')

print('Saving to: ', os.path.join(args.output_dir, file_name + '.yaml'))
print('Saving to: ', os.path.join(
args.output_dir, file_name + '.yaml'))
with open(os.path.join(args.output_dir, file_name + '.yaml'), 'w') as yaml_file:
yaml.dump(yaml_output, yaml_file, default_flow_style=False)
5 changes: 3 additions & 2 deletions ros2param/ros2param/verb/list.py
Original file line number Diff line number Diff line change
Expand Up @@ -103,7 +103,7 @@ def main(self, *, args): # noqa: D102

# wait for all responses
for future in futures.values():
rclpy.spin_until_future_complete(node, future, timeout_sec=1.0)
rclpy.spin_until_complete(node, future, timeout_sec=1.0)

# print responses
for node_name in sorted(futures.keys()):
Expand All @@ -117,7 +117,8 @@ def main(self, *, args): # noqa: D102
response = future.result()
sorted_names = sorted(response.result.names)
if regex_filter is not None:
sorted_names = [name for name in sorted_names if regex_filter.match(name)]
sorted_names = [
name for name in sorted_names if regex_filter.match(name)]
if not args.node_name and sorted_names:
print(f'{node_name.full_name}:')
# get descriptors for the node if needs to print parameter type
Expand Down
8 changes: 5 additions & 3 deletions ros2service/ros2service/verb/call.py
Original file line number Diff line number Diff line change
Expand Up @@ -81,7 +81,8 @@ def requester(service_type, service_name, values, period):

rclpy.init()

node = rclpy.create_node(NODE_NAME_PREFIX + '_requester_%s_%s' % (package_name, srv_name))
node = rclpy.create_node(
NODE_NAME_PREFIX + '_requester_%s_%s' % (package_name, srv_name))

cli = node.create_client(srv_module, service_name)

Expand All @@ -100,11 +101,12 @@ def requester(service_type, service_name, values, period):
print('requester: making request: %r\n' % request)
last_call = time.time()
future = cli.call_async(request)
rclpy.spin_until_future_complete(node, future)
rclpy.spin_until_complete(node, future)
if future.result() is not None:
print('response:\n%r\n' % future.result())
else:
raise RuntimeError('Exception while calling service: %r' % future.exception())
raise RuntimeError(
'Exception while calling service: %r' % future.exception())
if period is None or not rclpy.ok():
break
time_until_next_period = (last_call + period) - time.time()
Expand Down
14 changes: 9 additions & 5 deletions ros2topic/ros2topic/verb/echo.py
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,8 @@ def add_arguments(self, parser, cli_name):
choices=rclpy.qos.QoSPresetProfiles.short_keys(),
help='Quality of service preset profile to subscribe with (default: {})'
.format(default_profile_str))
default_profile = rclpy.qos.QoSPresetProfiles.get_from_short_key(default_profile_str)
default_profile = rclpy.qos.QoSPresetProfiles.get_from_short_key(
default_profile_str)
parser.add_argument(
'--qos-depth', metavar='N', type=int,
help='Queue size setting to subscribe with '
Expand Down Expand Up @@ -273,7 +274,7 @@ def subscribe_and_spin(
raw=raw)

if self.future is not None:
rclpy.spin_until_future_complete(node, self.future)
rclpy.spin_until_complete(node, self.future)
else:
rclpy.spin(node)

Expand All @@ -284,7 +285,8 @@ def _subscriber_callback(self, msg):
try:
submsg = getattr(submsg, field)
except AttributeError as ex:
raise RuntimeError(f"Invalid field '{'.'.join(self.field)}': {ex}")
raise RuntimeError(
f"Invalid field '{'.'.join(self.field)}': {ex}")

# Evaluate the current msg against the supplied expression
if self.filter_fn is not None and not self.filter_fn(submsg):
Expand All @@ -294,7 +296,8 @@ def _subscriber_callback(self, msg):
self.future.set_result(True)

if self.csv:
self.print_func(submsg, self.truncate_length, self.no_arr, self.no_str)
self.print_func(submsg, self.truncate_length,
self.no_arr, self.no_str)
else:
self.print_func(
submsg, self.truncate_length, self.no_arr, self.no_str, self.flow_style)
Expand All @@ -319,7 +322,8 @@ def _print_yaml(msg, truncate_length, noarr, nostr, flowstyle):

def _print_csv(msg, truncate_length, noarr, nostr):
if hasattr(msg, '__slots__'):
print(message_to_csv(msg, truncate_length=truncate_length, no_arr=noarr, no_str=nostr))
print(message_to_csv(msg, truncate_length=truncate_length,
no_arr=noarr, no_str=nostr))
else:
print(msg)

Expand Down
Loading

0 comments on commit 00b4317

Please sign in to comment.