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()