def cb(msg):
nonlocal truncate_length, noarr, nostr
print(message_to_yaml(msg,
truncate_length=truncate_length,
no_arr=noarr,
no_str=nostr),
end='---\n')
def _print_yaml(msg, truncate_length, noarr, nostr):
if hasattr(msg, '__slots__'):
print(
message_to_yaml(
msg, truncate_length=truncate_length, no_arr=noarr, no_str=nostr),
end='---\n')
else:
print(msg, end='\n---\n')
def interface_to_yaml(identifier):
interface = utilities.get_interface(identifier)
if utilities.is_action(interface):
instance = interface.Goal()
elif utilities.is_service(interface):
instance = interface.Request()
else:
instance = interface()
return message_to_yaml(instance)
def cb(msg):
nonlocal truncate_length, noarr, nostr
if isinstance(msg, bytes):
print(msg, end='\n---\n')
else:
print(message_to_yaml(msg,
truncate_length=truncate_length,
no_arr=noarr,
no_str=nostr),
end='---\n')
def test_primitives():
# Smoke-test the formatters on a bunch of messages
msgs = []
msgs.extend(message_fixtures.get_msg_arrays())
msgs.extend(message_fixtures.get_msg_basic_types())
msgs.extend(message_fixtures.get_msg_bounded_sequences())
msgs.extend(message_fixtures.get_msg_builtins())
msgs.extend(message_fixtures.get_msg_constants())
msgs.extend(message_fixtures.get_msg_defaults())
msgs.extend(message_fixtures.get_msg_empty())
msgs.extend(message_fixtures.get_msg_multi_nested())
msgs.extend(message_fixtures.get_msg_nested())
msgs.extend(message_fixtures.get_msg_strings())
msgs.extend(message_fixtures.get_msg_unbounded_sequences())
for m in msgs:
message_to_csv(m, 100)
message_to_csv(m, None)
message_to_ordereddict(m, 100)
message_to_ordereddict(m, None)
message_to_yaml(m, 100)
message_to_yaml(m, None)
def _subscriber_callback(self, msg, info):
submsg = msg
if self.field is not None:
for field in self.field:
try:
submsg = getattr(submsg, field)
except AttributeError as 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):
return
if self.future is not None:
self.future.set_result(True)
if not hasattr(submsg, '__slots__'):
# raw
if self.include_message_info:
print('---Got new message, message info:---')
print(info)
print('---Message data:---')
print(submsg, end='\n---\n')
return
if self.csv:
to_print = message_to_csv(submsg,
truncate_length=self.truncate_length,
no_arr=self.no_arr,
no_str=self.no_str)
if self.include_message_info:
to_print = f'{",".join(str(x) for x in info.values())},{to_print}'
print(to_print)
return
# yaml
if self.include_message_info:
print(yaml.dump(info), end='---\n')
print(message_to_yaml(submsg,
truncate_length=self.truncate_length,
no_arr=self.no_arr,
no_str=self.no_str,
flow_style=self.flow_style),
end='---\n')
def send_goal(action_name, action_type, goal_values, feedback_callback):
goal_handle = None
node = None
action_client = None
try:
try:
# TODO(jacobperron): This logic should come from a rosidl related package
parts = action_type.split('/')
if len(parts) == 1:
raise ValueError()
if len(parts) == 2:
parts = [parts[0], 'action', parts[1]]
package_name = parts[0]
action_type = parts[-1]
if not all(parts):
raise ValueError()
except ValueError:
raise RuntimeError('The passed action type is invalid')
module = importlib.import_module('.'.join(parts[:-1]))
action_module = getattr(module, action_type)
goal_dict = yaml.safe_load(goal_values)
rclpy.init()
node_name = NODE_NAME_PREFIX + '_send_goal_{}_{}'.format(
package_name, action_type)
node = rclpy.create_node(node_name)
action_client = ActionClient(node, action_module, action_name)
goal = action_module.Goal()
try:
set_message_fields(goal, goal_dict)
except Exception as ex:
return 'Failed to populate message fields: {!r}'.format(ex)
print('Waiting for an action server to become available...')
action_client.wait_for_server()
print('Sending goal:\n {}'.format(message_to_yaml(goal, None)))
goal_future = action_client.send_goal_async(goal, feedback_callback)
rclpy.spin_until_future_complete(node, goal_future)
goal_handle = goal_future.result()
if goal_handle is None:
raise RuntimeError('Exception while sending goal: {!r}'.format(
goal_future.exception()))
if not goal_handle.accepted:
print('Goal was rejected.')
# no need to potentially cancel the goal anymore
goal_handle = None
return
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)
result = result_future.result()
if result is None:
raise RuntimeError('Exception while getting result: {!r}'.format(
result_future.exception()))
# no need to potentially cancel the goal anymore
goal_handle = None
print('Result:\n {}'.format(message_to_yaml(result.result, None)))
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)
cancel_response = cancel_future.result()
if cancel_response is None:
raise RuntimeError(
'Exception while canceling goal: {!r}'.format(
cancel_future.exception()))
if len(cancel_response.goals_canceling) == 0:
raise RuntimeError('Failed to cancel goal')
if len(cancel_response.goals_canceling) > 1:
raise RuntimeError('More than one goal canceled')
if cancel_response.goals_canceling[
0].goal_id != goal_handle.goal_id:
raise RuntimeError('Canceled goal with incorrect goal ID')
print('Goal canceled.')
if action_client is not None:
action_client.destroy()
if node is not None:
node.destroy_node()
if rclpy.ok():
rclpy.shutdown()
def _feedback_callback(feedback):
print('Feedback:\n {}'.format(message_to_yaml(feedback.feedback, None)))
def __call__(self, prefix, parsed_args, **kwargs):
message = get_message(getattr(parsed_args, self.topic_type_key))
return [message_to_yaml(message())]
def __call__(self, prefix, parsed_args, **kwargs):
action = get_action(getattr(parsed_args, self.action_type_key))
return [message_to_yaml(action.Goal())]
def cb(msg):
nonlocal truncate_length
print(message_to_yaml(msg, truncate_length), end='---\n')
def __call__(self, prefix, parsed_args, **kwargs):
service = get_service(getattr(parsed_args, self.service_type_key))
return [message_to_yaml(service.Request())]
def send_goal(action_name, action_type, goal_values, feedback_callback):
goal_handle = None
node = None
action_client = None
try:
try:
action_module = get_action(action_type)
except (AttributeError, ModuleNotFoundError, ValueError):
raise RuntimeError('The passed action type is invalid')
goal_dict = yaml.safe_load(goal_values)
rclpy.init()
node_name = f"{NODE_NAME_PREFIX}_send_goal_{action_type.replace('/', '_')}"
node = rclpy.create_node(node_name)
action_client = ActionClient(node, action_module, action_name)
goal = action_module.Goal()
try:
set_message_fields(goal, goal_dict)
except Exception as ex:
return 'Failed to populate message fields: {!r}'.format(ex)
print('Waiting for an action server to become available...')
action_client.wait_for_server()
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)
goal_handle = goal_future.result()
if goal_handle is None:
raise RuntimeError(
'Exception while sending goal: {!r}'.format(goal_future.exception()))
if not goal_handle.accepted:
print('Goal was rejected.')
# no need to potentially cancel the goal anymore
goal_handle = None
return
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)
result = result_future.result()
if result is None:
raise RuntimeError(
'Exception while getting result: {!r}'.format(result_future.exception()))
# no need to potentially cancel the goal anymore
goal_handle = None
print('Result:\n {}'.format(message_to_yaml(result.result)))
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)
cancel_response = cancel_future.result()
if cancel_response is None:
raise RuntimeError(
'Exception while canceling goal: {!r}'.format(cancel_future.exception()))
if len(cancel_response.goals_canceling) == 0:
raise RuntimeError('Failed to cancel goal')
if len(cancel_response.goals_canceling) > 1:
raise RuntimeError('More than one goal canceled')
if cancel_response.goals_canceling[0].goal_id != goal_handle.goal_id:
raise RuntimeError('Canceled goal with incorrect goal ID')
print('Goal canceled.')
if action_client is not None:
action_client.destroy()
if node is not None:
node.destroy_node()
if rclpy.ok():
rclpy.shutdown()
