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