def publisher(
node: Node,
message_type: MsgType,
topic_name: str,
values: dict,
period: float,
print_nth: int,
times: int,
wait_matching_subscriptions: int,
qos_profile: QoSProfile,
keep_alive: float,
) -> Optional[str]:
"""Initialize a node with a single publisher and run its publish loop (maybe only once)."""
try:
msg_module = get_message(message_type)
except (AttributeError, ModuleNotFoundError, ValueError):
raise RuntimeError('The passed message type is invalid')
values_dictionary = yaml.safe_load(values)
if not isinstance(values_dictionary, dict):
return 'The passed value needs to be a dictionary in YAML format'
pub = node.create_publisher(msg_module, topic_name, qos_profile)
times_since_last_log = 0
while pub.get_subscription_count() < wait_matching_subscriptions:
# Print a message reporting we're waiting each 1s, check condition each 100ms.
if not times_since_last_log:
print(
f'Waiting for at least {wait_matching_subscriptions} matching subscription(s)...'
)
times_since_last_log = (times_since_last_log + 1) % 10
time.sleep(0.1)
msg = msg_module()
try:
set_message_fields(msg, values_dictionary)
except Exception as e:
return 'Failed to populate field: {0}'.format(e)
print('publisher: beginning loop')
count = 0
def timer_callback():
nonlocal count
count += 1
if print_nth and count % print_nth == 0:
print('publishing #%d: %r\n' % (count, msg))
pub.publish(msg)
timer_callback()
if times != 1:
timer = node.create_timer(period, timer_callback)
while times == 0 or count < times:
rclpy.spin_once(node)
# give some time for the messages to reach the wire before exiting
time.sleep(keep_alive)
node.destroy_timer(timer)
else:
# give some time for the messages to reach the wire before exiting
time.sleep(keep_alive)
def run_action(self, c, joy_state):
cmd = self.command_list[c]
goal = self.get_interface_type(cmd['interface_type'], '.action').Goal()
for target, value in cmd['action_goal'].items():
set_message_fields(goal, {target: value})
# No need to wait
self.al_clients[cmd['action_name']].send_goal_async(goal)
def requester(service_type, service_name, values, period):
# TODO(wjwwood) this logic should come from a rosidl related package
try:
parts = service_type.split('/')
if len(parts) == 2:
parts = [parts[0], 'srv', parts[1]]
package_name = parts[0]
module = importlib.import_module('.'.join(parts[:-1]))
srv_name = parts[-1]
srv_module = getattr(module, srv_name)
except (AttributeError, ModuleNotFoundError, ValueError):
raise RuntimeError('The passed service type is invalid')
try:
srv_module.Request
srv_module.Response
except AttributeError:
raise RuntimeError('The passed type is not a service')
values_dictionary = yaml.safe_load(values)
rclpy.init()
node = rclpy.create_node(NODE_NAME_PREFIX + '_requester_%s_%s' %
(package_name, srv_name))
cli = node.create_client(srv_module, service_name)
request = srv_module.Request()
try:
set_message_fields(request, values_dictionary)
except Exception as e:
return 'Failed to populate field: {0}'.format(e)
if not cli.service_is_ready():
print('waiting for service to become available...')
cli.wait_for_service()
while True:
print('requester: making request: %r\n' % request)
last_call = time.time()
future = cli.call_async(request)
rclpy.spin_until_future_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())
if period is None or not rclpy.ok():
break
time_until_next_period = (last_call + period) - time.time()
if time_until_next_period > 0:
time.sleep(time_until_next_period)
node.destroy_node()
rclpy.shutdown()
def run_service(self, c, joy_state):
cmd = self.command_list[c]
request = self.get_interface_type(cmd['interface_type'],
'.srv').Request()
for target, value in cmd['service_request'].items():
set_message_fields(request, {target: value})
if not self.srv_clients[cmd['service_name']](request):
self.get_logger().info(
'Not sending new service request for command {} '
'because previous request has not finished'.format(c))
def requester(service_type, service_name, values, period):
# TODO(wjwwood) this logic should come from a rosidl related package
try:
package_name, srv_name = service_type.split('/', 2)
if not package_name or not srv_name:
raise ValueError()
except ValueError:
raise RuntimeError('The passed service type is invalid')
# TODO(sloretz) node API to get topic types should indicate if action or srv
middle_module = 'srv'
if service_name.endswith('/_action/get_result') or service_name.endswith(
'/_action/send_goal'):
middle_module = 'action'
module = importlib.import_module(package_name + '.' + middle_module)
srv_module = getattr(module, srv_name)
values_dictionary = yaml.safe_load(values)
rclpy.init()
node = rclpy.create_node(NODE_NAME_PREFIX + '_requester_%s_%s' %
(package_name, srv_name))
cli = node.create_client(srv_module, service_name)
request = srv_module.Request()
try:
set_message_fields(request, values_dictionary)
except Exception as e:
return 'Failed to populate field: {0}'.format(e)
if not cli.service_is_ready():
print('waiting for service to become available...')
cli.wait_for_service()
while True:
print('requester: making request: %r\n' % request)
last_call = time.time()
future = cli.call_async(request)
rclpy.spin_until_future_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())
if period is None or not rclpy.ok():
break
time_until_next_period = (last_call + period) - time.time()
if time_until_next_period > 0:
time.sleep(time_until_next_period)
node.destroy_node()
rclpy.shutdown()
def test_set_message_fields_invalid():
msg = message_fixtures.get_msg_basic_types()[0]
invalid_field = {}
invalid_field['test_invalid_field'] = 42
with pytest.raises(AttributeError):
set_message_fields(msg, invalid_field)
invalid_type = {}
invalid_type['int32_value'] = 'this is not an integer'
with pytest.raises(ValueError):
set_message_fields(msg, invalid_type)
def publisher(
node: Node,
message_type: MsgType,
topic_name: str,
values: dict,
period: float,
print_nth: int,
once: bool,
qos_profile: QoSProfile,
) -> Optional[str]:
"""Initialize a node with a single publisher and run its publish loop (maybe only once)."""
msg_module = get_message(message_type)
values_dictionary = yaml.safe_load(values)
if not isinstance(values_dictionary, dict):
return 'The passed value needs to be a dictionary in YAML format'
publisher_callbacks = PublisherEventCallbacks(
incompatible_qos=handle_incompatible_qos_event)
try:
pub = node.create_publisher(msg_module,
topic_name,
qos_profile,
event_callbacks=publisher_callbacks)
except UnsupportedEventTypeError:
pub = node.create_publisher(msg_module, topic_name, qos_profile)
msg = msg_module()
try:
set_message_fields(msg, values_dictionary)
except Exception as e:
return 'Failed to populate field: {0}'.format(e)
print('publisher: beginning loop')
count = 0
def timer_callback():
nonlocal count
count += 1
if print_nth and count % print_nth == 0:
print('publishing #%d: %r\n' % (count, msg))
pub.publish(msg)
timer = node.create_timer(period, timer_callback)
if once:
rclpy.spin_once(node)
time.sleep(
0.1) # make sure the message reaches the wire before exiting
else:
rclpy.spin(node)
node.destroy_timer(timer)
def publisher(message_type, topic_name, values, node_name, period, print_nth,
once, qos_profile, qos_reliability, qos_durability):
msg_module = import_message_type(topic_name, message_type)
values_dictionary = yaml.safe_load(values)
if not isinstance(values_dictionary, dict):
return 'The passed value needs to be a dictionary in YAML format'
if not node_name:
node_name = NODE_NAME_PREFIX + '_publisher_%s' % (message_type.replace(
'/', '_'), )
rclpy.init()
# Build a QoS profile based on user-supplied arguments
profile = rclpy.qos.QoSPresetProfiles.get_from_short_key(qos_profile)
if qos_durability:
profile.durability = rclpy.qos.QoSDurabilityPolicy.get_from_short_key(
qos_durability)
if qos_reliability:
profile.reliability = rclpy.qos.QoSReliabilityPolicy.get_from_short_key(
qos_reliability)
node = rclpy.create_node(node_name)
pub = node.create_publisher(msg_module, topic_name, profile)
msg = msg_module()
try:
set_message_fields(msg, values_dictionary)
except Exception as e:
return 'Failed to populate field: {0}'.format(e)
print('publisher: beginning loop')
count = 0
def timer_callback():
nonlocal count
count += 1
if print_nth and count % print_nth == 0:
print('publishing #%d: %r\n' % (count, msg))
pub.publish(msg)
timer = node.create_timer(period, timer_callback)
if once:
rclpy.spin_once(node)
time.sleep(
0.1) # make sure the message reaches the wire before exiting
else:
rclpy.spin(node)
node.destroy_timer(timer)
node.destroy_node()
rclpy.shutdown()
def test_set_message_fields_full():
msg_list = message_fixtures.get_msg_basic_types()
msg0 = msg_list[0]
msg1 = msg_list[1]
# Set msg0 values to the values of msg1
values = {}
for _attr in msg1.__slots__:
# Remove underscore prefix
attr = _attr[1:]
values[attr] = getattr(msg1, attr)
set_message_fields(msg0, values)
assert msg0 == msg1
def test_set_message_fields_nested_type():
msg_basic_types = message_fixtures.get_msg_basic_types()[0]
msg0 = message_fixtures.get_msg_nested()[0]
msg0.basic_types_value.bool_value = False
msg0.basic_types_value.char_value = 3
msg0.basic_types_value.int32_value = 42
assert msg0.basic_types_value != msg_basic_types
test_values = {}
test_values['basic_types_value'] = msg_basic_types
set_message_fields(msg0, test_values)
assert msg0.basic_types_value == msg_basic_types
def test_set_nested_namespaced_fields():
unbounded_sequence_msg = message_fixtures.get_msg_unbounded_sequences()[1]
test_values = {
'basic_types_values': [{
'float64_value': 42.42,
'int8_value': 42
}, {
'float64_value': 11.11,
'int8_value': 11
}]
}
set_message_fields(unbounded_sequence_msg, test_values)
assert unbounded_sequence_msg.basic_types_values[0].float64_value == 42.42
assert unbounded_sequence_msg.basic_types_values[0].int8_value == 42
assert unbounded_sequence_msg.basic_types_values[0].uint8_value == 0
assert unbounded_sequence_msg.basic_types_values[1].float64_value == 11.11
assert unbounded_sequence_msg.basic_types_values[1].int8_value == 11
assert unbounded_sequence_msg.basic_types_values[1].uint8_value == 0
arrays_msg = message_fixtures.get_msg_arrays()[0]
test_values = {
'basic_types_values': [
{
'float64_value': 42.42,
'int8_value': 42
},
{
'float64_value': 11.11,
'int8_value': 11
},
{
'float64_value': 22.22,
'int8_value': 22
},
]
}
set_message_fields(arrays_msg, test_values)
assert arrays_msg.basic_types_values[0].float64_value == 42.42
assert arrays_msg.basic_types_values[0].int8_value == 42
assert arrays_msg.basic_types_values[0].uint8_value == 0
assert arrays_msg.basic_types_values[1].float64_value == 11.11
assert arrays_msg.basic_types_values[1].int8_value == 11
assert arrays_msg.basic_types_values[1].uint8_value == 0
assert arrays_msg.basic_types_values[2].float64_value == 22.22
assert arrays_msg.basic_types_values[2].int8_value == 22
assert arrays_msg.basic_types_values[2].uint8_value == 0
def call_service(node, service_name, service_type, service_args):
"""Call a service. Mostly extracted from rosservice."""
srv_module = _get_module(service_type)
client = node.create_client(srv_module, service_name)
request = srv_module.Request()
try:
set_message_fields(request, service_args)
except Exception as e:
raise e
return 'Failed to populate field: {0}'.format(e)
if not client.service_is_ready():
client.wait_for_service()
client.call(request)
def __init__(self, name: str, config: typing.Dict[str, typing.Any], node: Node) -> None:
super().__init__(name, config, 'buttons', 'axes')
self.name = name
action_type = get_interface_type(config['interface_type'], 'action')
self.goal = action_type.Goal()
if 'action_goal' in config:
# Set the message fields for the goal in the constructor. This goal will be used
# during runtime, and has hte side benefit of giving the user early feedback if the
# config can't work.
set_message_fields(self.goal, config['action_goal'])
action_name = config['action_name']
self.action_client = ActionClient(node, action_type, action_name)
self.client_ready = False
def __init__(self, name: str, config: typing.Dict[str, typing.Any], node: Node) -> None:
super().__init__(name, config, 'buttons', 'axes')
self.name = name
service_name = config['service_name']
service_type = get_interface_type(config['interface_type'], 'srv')
self.request = service_type.Request()
if 'service_request' in config:
# Set the message fields in the request in the constructor. This request will be used
# during runtime, and has the side benefit of giving the user early feedback if the
# config can't work.
set_message_fields(self.request, config['service_request'])
self.service_client = node.create_client(service_type, service_name)
self.client_ready = False
def publisher(
node: Node,
message_type: MsgType,
topic_name: str,
values: dict,
period: float,
print_nth: int,
times: int,
qos_profile: QoSProfile,
keep_alive: float,
) -> Optional[str]:
"""Initialize a node with a single publisher and run its publish loop (maybe only once)."""
msg_module = get_message(message_type)
values_dictionary = yaml.safe_load(values)
if not isinstance(values_dictionary, dict):
return 'The passed value needs to be a dictionary in YAML format'
pub = node.create_publisher(msg_module, topic_name, qos_profile)
msg = msg_module()
try:
set_message_fields(msg, values_dictionary)
except Exception as e:
return 'Failed to populate field: {0}'.format(e)
print('publisher: beginning loop')
count = 0
def timer_callback():
nonlocal count
count += 1
if print_nth and count % print_nth == 0:
print('publishing #%d: %r\n' % (count, msg))
pub.publish(msg)
timer = node.create_timer(period, timer_callback)
while times == 0 or count < times:
rclpy.spin_once(node)
# give some time for the messages to reach the wire before exiting
time.sleep(keep_alive)
node.destroy_timer(timer)
def execute(self):
"""Publish a message to a topic."""
msg = ('Publishing message to topic {} '
'for a duration of {} with a rate of {}')
self.node.get_logger().info(
msg.format(self.topic, self.duration, self.rate))
once = self.duration == 1 and self.rate == 1
qos_profile = \
qos_profile_from_short_keys(self.qos_profile_str,
reliability=self.qos_reliability_str,
durability=self.qos_durability_str)
msg_module = get_message(self.msg_type)
pub = self.node.create_publisher(msg_module, self.topic, qos_profile)
msg = msg_module()
if self.use_substitution:
# deep copy so we don't lose the variables for future executions
parameters_copy = copy.deepcopy(self.parameters)
substitude_values(parameters_copy, self.parameter_values)
set_message_fields(msg, parameters_copy)
else:
set_message_fields(msg, self.parameters)
if not once:
clock = self.node.get_clock()
sched_time = clock.now()
end_time = sched_time + Duration(nanoseconds=self.duration * 10**9)
while clock.now() < end_time:
while clock.now() < sched_time:
time.sleep((sched_time - clock.now()).nanoseconds / 10**9)
if clock.now() > end_time:
break
pub.publish(msg)
sched_time = sched_time + Duration(
nanoseconds=(1. / self.rate) * 10**9)
else:
pub.publish(msg)
self.node.destroy_publisher(pub)
return True
def test_set_message_fields_partial():
original_msg = message_fixtures.get_msg_basic_types()[0]
original_msg.bool_value = False
original_msg.char_value = 3
original_msg.int32_value = 42
modified_msg = copy.copy(original_msg)
values = {}
values['bool_value'] = True
values['char_value'] = 1
values['int32_value'] = 24
set_message_fields(modified_msg, values)
for _attr in original_msg.__slots__:
# Remove underscore prefix
attr = _attr[1:]
if attr in values:
assert getattr(modified_msg, attr) == values[attr]
else:
assert getattr(modified_msg, attr) == getattr(original_msg, attr)
def test_set_message_fields_none():
# Smoke-test 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:
original_m = copy.copy(m)
set_message_fields(m, {})
# Assert message is not modified when setting no fields
assert original_m == m
def __sub_callback(self, actual_msg):
# Test the equality:
expected_msg = get_message(self.__message_type)()
equal = True
try:
set_message_fields(expected_msg, self.__expected_values)
except Exception as e:
self.__node.get_logger().error(
'Failed to populate field: {0}'.format(e))
raise e
for field in expected_msg.get_fields_and_field_types():
expected = str(getattr(expected_msg, field))
actual = str(getattr(actual_msg, field))
if expected != actual:
equal = False
# Call the callback with the result
self.__callback(equal, actual_msg, expected_msg)
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 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()
