def _extract_struct_fields(m): if isinstance(m, uavcan.transport.CompoundValue): out = CompactMessage(uavcan.get_uavcan_data_type(m).full_name) for field_name, field in uavcan.get_fields(m).items(): if uavcan.is_union( m) and uavcan.get_active_union_field(m) != field_name: continue val = _extract_struct_fields(field) if val is not None: out._add_field(field_name, val) return out elif isinstance(m, uavcan.transport.ArrayValue): # cannot say I'm breaking the rules container = bytes if uavcan.get_uavcan_data_type( m).is_string_like else list # if I can glue them back together return container( filter(lambda x: x is not None, (_extract_struct_fields(item) for item in m))) elif isinstance(m, uavcan.transport.PrimitiveValue): return m.value elif isinstance(m, (int, float, bool)): return m elif isinstance(m, uavcan.transport.VoidValue): pass else: raise ValueError(':(')
def request(self, payload, dest_node_id, timeout=None, priority=None): """ Synchronous request. Throws a TimeoutException on timeout. """ result = None def callback(event): nonlocal result if event is not None: logger.debug('Response %r from %d\n%s', uavcan.get_uavcan_data_type(event.response), event.transfer.source_node_id, uavcan.to_yaml(event.response)) assert event.transfer.source_node_id == dest_node_id result = event.response else: raise Node.TimeoutException( 'Request to node %d with payload %r has timed out in %.3f seconds' % (dest_node_id, payload, timeout or uavcan.node.DEFAULT_SERVICE_TIMEOUT)) logger.debug('Synchronously requesting %r from %d\n%s', uavcan.get_uavcan_data_type(payload), dest_node_id, uavcan.to_yaml(payload)) self.request_async(payload, dest_node_id, callback, timeout, priority) while result is None: self.spin_for(0.1) return result
def request(self, payload, dest_node_id, timeout=None, priority=None): """ Synchronous request. Throws a TimeoutException on timeout. """ result = None def callback(event): nonlocal result if event is not None: logger.debug('Response %r from %d\n%s', uavcan.get_uavcan_data_type(event.response), event.transfer.source_node_id, uavcan.to_yaml(event.response)) assert event.transfer.source_node_id == dest_node_id result = event.response else: raise Node.TimeoutException('Request to node %d with payload %r has timed out in %.3f seconds' % (dest_node_id, payload, timeout or uavcan.node.DEFAULT_SERVICE_TIMEOUT)) logger.debug('Synchronously requesting %r from %d\n%s', uavcan.get_uavcan_data_type(payload), dest_node_id, uavcan.to_yaml(payload)) self.request_async(payload, dest_node_id, callback, timeout, priority) while result is None: self.spin_for(0.1) return result
def broadcast(self, uavcan_msg, priority=None): #pylint: disable=W0613 uavcan_full_name = uavcan.get_uavcan_data_type(uavcan_msg).full_name if not uavcan_full_name in self.__publishers: self.__publishers[uavcan_full_name] = Message( uavcan_full_name).Publisher(queue_size=10) publisher = self.__publishers[uavcan_full_name] publisher.publish(copy_uavcan_ros(publisher.data_class(), uavcan_msg))
def _to_json_compatible_object_impl(obj): # Decomposing PrimitiveValue to value and type. This is ugly but it's by design... if isinstance(obj, PrimitiveValue): obj = obj.value # CompoundValue if isinstance(obj, CompoundValue): output = dict() for field_name, field in uavcan.get_fields(obj).items(): if uavcan.is_union( obj) and uavcan.get_active_union_field(obj) != field_name: continue if isinstance(field, VoidValue): continue output[field_name] = to_json_compatible_object(field) return output # ArrayValue elif isinstance(obj, ArrayValue): t = uavcan.get_uavcan_data_type(obj) if t.value_type.category == t.value_type.CATEGORY_PRIMITIVE: def is_nice_character(ch): if ch.is_printable() or ch.isspace(): return True if ch in b'\n\r\t': return True return False # Catch a string masquerading as an array if t.is_string_like and all(map(is_nice_character, obj)): return obj.decode() # Return the array! output = [] for x in obj: output.append(to_json_compatible_object(x)) return output # Primitive types elif isinstance(obj, float): return obj elif isinstance(obj, bool): return obj elif isinstance(obj, int): return obj # Non-printable types elif isinstance(obj, VoidValue): pass # Unknown types else: raise ValueError( 'Cannot generate JSON-compatible object representation for %r' % type(obj))
def __init__(self, type_or_struct, mode=None): # noinspection PyBroadException try: self.type = uavcan.get_uavcan_data_type(type_or_struct) except Exception: self.type = type_or_struct self.mode = mode if (self.type.kind == self.type.KIND_SERVICE) == (self.mode is None): raise ValueError('Mode must be set for service types, and for service types only')
def callback(event): nonlocal result if event is not None: logger.debug('Response %r from %d\n%s', uavcan.get_uavcan_data_type(event.response), event.transfer.source_node_id, uavcan.to_yaml(event.response)) assert event.transfer.source_node_id == dest_node_id result = event.response else: raise Node.TimeoutException('Request to node %d with payload %r has timed out in %.3f seconds' % (dest_node_id, payload, timeout or uavcan.node.DEFAULT_SERVICE_TIMEOUT))
def __init__(self, type_or_struct, mode=None): # noinspection PyBroadException try: self.type = uavcan.get_uavcan_data_type(type_or_struct) except Exception: self.type = type_or_struct self.mode = mode if (self.type.kind == self.type.KIND_SERVICE) == (self.mode is None): raise ValueError( 'Mode must be set for service types, and for service types only' )
def process_next(): nonlocal num_broadcasted try: do_broadcast() except Exception: logger.error('Automatic broadcast failed, job cancelled', exc_info=True) timer_handle.remove() else: num_broadcasted += 1 if (count is not None and num_broadcasted >= count) or (time.monotonic() >= deadline): logger.info('Background publisher for %r has stopped', uavcan.get_uavcan_data_type(payload).full_name) timer_handle.remove()
def _extract_struct_fields(m): if isinstance(m, uavcan.transport.CompoundValue): out = CompactMessage(uavcan.get_uavcan_data_type(m).full_name) for field_name, field in uavcan.get_fields(m).items(): if uavcan.is_union(m) and uavcan.get_active_union_field(m) != field_name: continue val = _extract_struct_fields(field) if val is not None: out._add_field(field_name, val) return out elif isinstance(m, uavcan.transport.ArrayValue): # cannot say I'm breaking the rules container = bytes if uavcan.get_uavcan_data_type(m).is_string_like else list # if I can glue them back together return container(filter(lambda x: x is not None, (_extract_struct_fields(item) for item in m))) elif isinstance(m, uavcan.transport.PrimitiveValue): return m.value elif isinstance(m, (int, float, bool)): return m elif isinstance(m, uavcan.transport.VoidValue): pass else: raise ValueError(':(')
def callback(event): nonlocal result if event is not None: logger.debug('Response %r from %d\n%s', uavcan.get_uavcan_data_type(event.response), event.transfer.source_node_id, uavcan.to_yaml(event.response)) assert event.transfer.source_node_id == dest_node_id result = event.response else: raise Node.TimeoutException( 'Request to node %d with payload %r has timed out in %.3f seconds' % (dest_node_id, payload, timeout or uavcan.node.DEFAULT_SERVICE_TIMEOUT))
def process_next(): nonlocal num_broadcasted try: do_broadcast() except Exception: logger.error("Automatic broadcast failed, job cancelled", exc_info=True) timer_handle.remove() else: num_broadcasted += 1 if (count is not None and num_broadcasted >= count) or (time.monotonic() >= deadline): logger.info( "Background publisher for %r has stopped", uavcan.get_uavcan_data_type(payload).full_name, ) timer_handle.remove()
def _read(self, e): logger.debug("[#{0:03d}:uavcan.protocol.file.Read] {1!r} @ offset {2:d}" .format(e.transfer.source_node_id, e.request.path.path.decode(), e.request.offset)) try: with open(self._resolve_path(e.request.path), "rb") as f: f.seek(e.request.offset) resp = uavcan.protocol.file.Read.Response() read_size = uavcan.get_uavcan_data_type(uavcan.get_fields(resp)['data']).max_size resp.data = bytearray(f.read(read_size)) resp.error.value = resp.error.OK except Exception: logger.exception("[#{0:03d}:uavcan.protocol.file.Read] error") resp = uavcan.protocol.file.Read.Response() resp.error.value = resp.error.UNKNOWN_ERROR return resp
def _read(self, e): logger.debug("[#{0:03d}:uavcan.protocol.file.Read] {1!r} @ offset {2:d}" .format(e.transfer.source_node_id, e.request.path.path.decode(), e.request.offset)) try: with open(self._resolve_path(e.request.path), "rb") as f: f.seek(e.request.offset) resp = uavcan.protocol.file.Read.Response() read_size = uavcan.get_uavcan_data_type(uavcan.get_fields(resp)['data']).max_size resp.data = f.read(read_size) resp.error.value = resp.error.OK except Exception: logger.exception("[#{0:03d}:uavcan.protocol.file.Read] error") resp = uavcan.protocol.file.Read.Response() resp.error.value = resp.error.UNKNOWN_ERROR return resp
def copy_uavcan_ros(ros_msg, uavcan_msg, uavcan_type=None, request=None): uavcan_type = uavcan_type or uavcan.get_uavcan_data_type(uavcan_msg) if uavcan_type.category == uavcan_type.CATEGORY_COMPOUND: try: fields = uavcan_type.fields union = uavcan_type.union except AttributeError: fields = uavcan_type.request_fields if request else uavcan_type.response_fields union = uavcan_type.request_union if request else uavcan_type.response_union for i, field in enumerate(fields): if field.type.category == field.type.CATEGORY_VOID: continue try: attr = getattr(uavcan_msg, field.name) except AttributeError: # expecting an AttributeError if a non union field is accessed pass else: val = copy_uavcan_ros(getattr(ros_msg, field.name), attr, uavcan_type=field.type, request=request) setattr(ros_msg, field.name, val) if union: setattr(ros_msg, union_tag_field_name, i) return ros_msg if uavcan_type.category == uavcan_type.CATEGORY_ARRAY: if isinstance(ros_msg, str): ros_msg = [] for uavcan_item in uavcan_msg: ros_msg.append( copy_uavcan_ros(ros_type_from_type(uavcan_type.value_type)(), uavcan_item, uavcan_type=uavcan_type.value_type, request=request)) return ros_msg if uavcan_type.category == uavcan_type.CATEGORY_PRIMITIVE: return uavcan_msg raise Exception("Could not copy UAVCAN message")
def _on_transfer(self, tr): try: dtname = uavcan.get_uavcan_data_type(tr.payload).full_name except Exception: try: kind = uavcan.dsdl.CompoundType.KIND_SERVICE if tr.service_not_message else \ uavcan.dsdl.CompoundType.KIND_MESSAGE dtname = uavcan.DATATYPES[(tr.data_type_id, kind)].full_name except Exception: logger.error('Could not detect data type name from transfer %r', tr, exc_info=True) return if tr.service_not_message: if dtname not in self._active_services: self._active_services.add(dtname) self.service_types_updated.emit() else: if dtname not in self._active_messages: self._active_messages.add(dtname) self.message_types_updated.emit()
def copy_ros_uavcan(uavcan_msg, ros_msg, uavcan_type=None, request=None): uavcan_type = uavcan_type or uavcan.get_uavcan_data_type(uavcan_msg) if uavcan_type.category == uavcan_type.CATEGORY_COMPOUND: try: fields = uavcan_type.fields union = uavcan_type.union except AttributeError: fields = uavcan_type.request_fields if request else uavcan_type.response_fields union = uavcan_type.request_union if request else uavcan_type.response_union for i, field in enumerate(fields): if field.type.category == field.type.CATEGORY_VOID: continue if (not union) or i == ros_msg.canros_union_tag: val = copy_ros_uavcan(getattr(uavcan_msg, field.name), getattr(ros_msg, field.name), uavcan_type=field.type, request=request) if val != getattr(uavcan_msg, field.name): setattr(uavcan_msg, field.name, val) return uavcan_msg if uavcan_type.category == uavcan_type.CATEGORY_ARRAY: # edge case for message fields of type uint8[] which get converted to strings if isinstance(ros_msg, str): return to_uint8(ros_msg) for ros_item in ros_msg: uavcan_msg.append( copy_ros_uavcan(uavcan_msg.new_item(), ros_item, uavcan_type=uavcan_type.value_type, request=request)) return uavcan_msg if uavcan_type.category == uavcan_type.CATEGORY_PRIMITIVE: return ros_msg raise Exception("Could not copy ros message")
def sender(self): pub = rospy.Publisher( "~wibotic_info", msg.WiBoticInfo, queue_size=10, ) while not rospy.is_shutdown(): incoming_data = _uav_incoming_info.get() uavcan_dsdl_type = uavcan.get_uavcan_data_type( incoming_data ) unpacked_data = {} for field in uavcan_dsdl_type.fields: unpacked_data[field.name] = getattr( incoming_data, field.name ) packaged_data = msg.WiBoticInfo( **unpacked_data ) rospy.loginfo(packaged_data) pub.publish(packaged_data)
def request(self, uavcan_msg, node_id, callback, priority=None, timeout=None): #pylint: disable=W0613 uavcan_type = uavcan.get_uavcan_data_type(uavcan_msg) if not uavcan_type.full_name in self.__service_proxies: self.__service_proxies[uavcan_type.full_name] = Service( uavcan_type.full_name).ServiceProxy() service_proxy = self.__service_proxies[uavcan_type.full_name] ros_req = copy_uavcan_ros(service_proxy.request_class(), uavcan_msg, request=True) setattr(ros_req, uavcan_id_field_name, node_id) def service_proxy_call(): try: return service_proxy(ros_req) except rospy.ServiceException: return None def request_finished(fut): ros_resp = fut.result() if ros_resp is None: uavcan_resp = None else: uavcan_resp = uavcan_node._Event(self, uavcan_type, ros_resp, uavcan_id=node_id, request=False) callback(uavcan_resp) self.__request_executor.submit(service_proxy_call).add_done_callback( request_finished)
def _on_transfer(self, tr): try: dtname = uavcan.get_uavcan_data_type(tr.payload).full_name except Exception: try: kind = uavcan.dsdl.CompoundType.KIND_SERVICE if tr.service_not_message else \ uavcan.dsdl.CompoundType.KIND_MESSAGE dtname = uavcan.DATATYPES[(tr.data_type_id, kind)].full_name except Exception: logger.error( 'Could not detect data type name from transfer %r', tr, exc_info=True) return if tr.service_not_message: if dtname not in self._active_services: self._active_services.add(dtname) self.service_types_updated.emit() else: if dtname not in self._active_messages: self._active_messages.add(dtname) self.message_types_updated.emit()
def _to_yaml_impl(obj, indent_level=0, parent=None, name=None, uavcan_type=None): buf = StringIO() def write(fmt, *args): buf.write((fmt % args) if len(args) else fmt) def indent_newline(): buf.write(os.linesep + ' ' * 2 * indent_level) # Decomposing PrimitiveValue to value and type. This is ugly but it's by design... if isinstance(obj, PrimitiveValue): uavcan_type = uavcan.get_uavcan_data_type(obj) obj = obj.value # CompoundValue if isinstance(obj, CompoundValue): first_field = True # Rendering all fields than can be rendered for field_name, field in uavcan.get_fields(obj).items(): if uavcan.is_union(obj) and uavcan.get_active_union_field(obj) != field_name: continue if isinstance(field, VoidValue): continue if (first_field and indent_level > 0) or not first_field: indent_newline() first_field = False rendered_field = _to_yaml_impl(field, indent_level=indent_level + 1, parent=obj, name=field_name) write('%s: %s', field_name, rendered_field) # Special case - empty non-union struct is rendered as empty map if first_field and not uavcan.is_union(obj): if indent_level > 0: indent_newline() write('{}') # ArrayValue elif isinstance(obj, ArrayValue): t = uavcan.get_uavcan_data_type(obj) if t.value_type.category == t.value_type.CATEGORY_PRIMITIVE: def is_nice_character(ch): if 32 <= ch <= 126: return True if ch in b'\n\r\t': return True return False as_bytes = '[%s]' % ', '.join([_to_yaml_impl(x, indent_level=indent_level + 1, uavcan_type=t.value_type) for x in obj]) if t.is_string_like and all(map(is_nice_character, obj)): write('%r # ', obj.decode()) write(as_bytes) else: if len(obj) == 0: write('[]') else: for x in obj: indent_newline() write('- %s', _to_yaml_impl(x, indent_level=indent_level + 1, uavcan_type=t.value_type)) # Primitive types elif isinstance(obj, float): assert uavcan_type is not None float_fmt = { 16: '%.4f', 32: '%.6f', 64: '%.9f', }[uavcan_type.bitlen] write(float_fmt, obj) elif isinstance(obj, bool): write('%s', 'true' if obj else 'false') elif isinstance(obj, int): write('%s', obj) if parent is not None and name is not None: resolved_name = value_to_constant_name(parent, name) if isinstance(resolved_name, str): write(' # %s', resolved_name) # Non-printable types elif isinstance(obj, VoidValue): pass # Unknown types else: raise ValueError('Cannot generate YAML representation for %r' % type(obj)) return buf.getvalue()
def value_to_constant_name(struct, field_name, keep_literal=False): """ This function accepts a UAVCAN struct (message, request, or response), and a field name; and returns the name of constant or bit mask that match the value. If no match could be established, the literal value will be returned as is. Args: struct: UAVCAN struct to work with field_name: Name of the field to work with keep_literal: Whether to include the input integer value in the output string Returns: Name of the constant or flags if match could be detected, otherwise integer as is. """ # Extracting constants uavcan_type = uavcan.get_uavcan_data_type(struct) if uavcan.is_request(struct): consts = uavcan_type.request_constants fields = uavcan_type.request_fields elif uavcan.is_response(struct): consts = uavcan_type.response_constants fields = uavcan_type.response_fields else: consts = uavcan_type.constants fields = uavcan_type.fields assert len(fields) > 0 # noinspection PyShadowingNames def format_output(name, value, remove_common_prefix): if remove_common_prefix: num_seps = len(field_name.split('_')) parts = name.split('_')[num_seps:] name = '_'.join(parts) return ('%s (%r)' % (name, value)) if keep_literal else name # noinspection PyShadowingNames def match_one_prefix(prefix, value): matches = [] for cname, cval in [(x.name, x.value) for x in consts if x.name.lower().startswith(prefix.lower())]: if cval == value: matches.append(cname) # Making sure we found exactly one match, otherwise it's not a correct result if len(matches) == 1: return matches[0] # noinspection PyShadowingNames def match_value(value): # Trying direct match match = match_one_prefix(field_name + '_', value) if match: return format_output(match, value, True) # Trying direct match without the terminal letter if it is 's' (plural). This works for 'flags'. # TODO: this is sketchy. if field_name[-1] == 's': match = match_one_prefix(field_name[:-1] + '_', value) if match: return format_output(match, value, True) # Trying match without prefix, only if there's just one field if len(fields) == 1: match = match_one_prefix('', value) if match: return format_output(match, value, False) # Trying single value first value = getattr(struct, field_name) match = match_value(value) if match: return match # Trying bit masks def extract_powers_of_2(x): i = 1 while i <= x: if i & x: yield i i <<= 1 matches = [] for pow2 in extract_powers_of_2(value): match = match_value(pow2) if match: matches.append(match) else: matches = [] break # If at least one couldn't be matched, we're on a wrong track, stop if len(matches) > 0: return ' | '.join(matches) # No match could be found, returning the value as is return value
def __init__(self, tr): self.source_node_id = tr.source_node_id self.ts_mono = tr.ts_monotonic self.data_type_name = uavcan.get_uavcan_data_type(tr.payload).full_name self.message = _extract_struct_fields(tr.payload)
class ESCPanel(QDialog): DEFAULT_INTERVAL = 0.1 CMD_BIT_LENGTH = uavcan.get_uavcan_data_type(uavcan.equipment.esc.RawCommand().cmd).value_type.bitlen CMD_MAX = 2 ** (CMD_BIT_LENGTH - 1) - 1 CMD_MIN = -(2 ** (CMD_BIT_LENGTH - 1)) def __init__(self, parent, node): super(ESCPanel, self).__init__(parent) self.setWindowTitle('ESC Management Panel') self.setAttribute(Qt.WA_DeleteOnClose) # This is required to stop background timers! self._node = node self._sliders = [PercentSlider(self) for _ in range(5)] self._num_sliders = QSpinBox(self) self._num_sliders.setMinimum(len(self._sliders)) self._num_sliders.setMaximum(20) self._num_sliders.setValue(len(self._sliders)) self._num_sliders.valueChanged.connect(self._update_number_of_sliders) self._bcast_interval = QDoubleSpinBox(self) self._bcast_interval.setMinimum(0.01) self._bcast_interval.setMaximum(1.0) self._bcast_interval.setSingleStep(0.1) self._bcast_interval.setValue(self.DEFAULT_INTERVAL) self._bcast_interval.valueChanged.connect( lambda: self._bcast_timer.setInterval(self._bcast_interval.value() * 1e3)) self._stop_all = make_icon_button('hand-stop-o', 'Zero all channels', self, text='Stop All', on_clicked=self._do_stop_all) self._pause = make_icon_button('pause', 'Pause publishing', self, checkable=True, text='Pause') self._msg_viewer = QPlainTextEdit(self) self._msg_viewer.setReadOnly(True) self._msg_viewer.setLineWrapMode(QPlainTextEdit.NoWrap) self._msg_viewer.setFont(get_monospace_font()) self._msg_viewer.setVerticalScrollBarPolicy(Qt.ScrollBarAsNeeded) self._msg_viewer.setHorizontalScrollBarPolicy(Qt.ScrollBarAsNeeded) self._bcast_timer = QTimer(self) self._bcast_timer.start(self.DEFAULT_INTERVAL * 1e3) self._bcast_timer.timeout.connect(self._do_broadcast) layout = QVBoxLayout(self) self._slider_layout = QHBoxLayout(self) for sl in self._sliders: self._slider_layout.addWidget(sl) layout.addLayout(self._slider_layout) layout.addWidget(self._stop_all) controls_layout = QHBoxLayout(self) controls_layout.addWidget(QLabel('Channels:', self)) controls_layout.addWidget(self._num_sliders) controls_layout.addWidget(QLabel('Broadcast interval:', self)) controls_layout.addWidget(self._bcast_interval) controls_layout.addWidget(QLabel('sec', self)) controls_layout.addStretch() controls_layout.addWidget(self._pause) layout.addLayout(controls_layout) layout.addWidget(QLabel('Generated message:', self)) layout.addWidget(self._msg_viewer) self.setLayout(layout) self.resize(self.minimumWidth(), self.minimumHeight()) def _do_broadcast(self): try: if not self._pause.isChecked(): msg = uavcan.equipment.esc.RPMCommand() for sl in self._sliders: # raw_value = sl.get_value() / 100100 raw_value = sl.get_value() # value = (-self.CMD_MIN if raw_value < 0 else self.CMD_MAX) * raw_value value = (-30 if raw_value < 0 else 30) * raw_value msg.rpm.append(int(value)) self._node.broadcast(msg) self._msg_viewer.setPlainText(uavcan.to_yaml(msg)) else: self._msg_viewer.setPlainText('Paused') except Exception as ex: self._msg_viewer.setPlainText('Publishing failed:\n' + str(ex)) def _do_stop_all(self): for sl in self._sliders: sl.zero() def _update_number_of_sliders(self): num_sliders = self._num_sliders.value() while len(self._sliders) > num_sliders: removee = self._sliders[-1] self._sliders = self._sliders[:-1] self._slider_layout.removeWidget(removee) removee.close() removee.deleteLater() while len(self._sliders) < num_sliders: new = PercentSlider(self) self._slider_layout.addWidget(new) self._sliders.append(new) def deferred_resize(): self.resize(self.minimumWidth(), self.height()) deferred_resize() # noinspection PyCallByClass,PyTypeChecker QTimer.singleShot(200, deferred_resize) def __del__(self): global _singleton _singleton = None def closeEvent(self, event): global _singleton _singleton = None super(ESCPanel, self).closeEvent(event)
def test_uavcan(): node_info = uavcan.protocol.GetNodeInfo.Response() node_info.name = 'com.zubax.drwatson.sapog' iface = init_can_iface() with closing(uavcan.make_node(iface, bitrate=CAN_BITRATE, node_id=127, mode=uavcan.protocol.NodeStatus().MODE_OPERATIONAL, node_info=node_info)) as n: def safe_spin(timeout): try: n.spin(timeout) except uavcan.UAVCANException: logger.error('Node spin failure', exc_info=True) @contextmanager def time_limit(timeout, error_fmt, *args): aborter = n.defer(timeout, partial(abort, error_fmt, *args)) yield aborter.remove() try: # Dynamic node ID allocation nsmon = uavcan.app.node_monitor.NodeMonitor(n) alloc = uavcan.app.dynamic_node_id.CentralizedServer(n, nsmon) info('Waiting for the node to show up on the CAN bus...') with time_limit(10, 'The node did not show up in time. Check CAN interface and crystal oscillator.'): while True: safe_spin(1) target_nodes = list(nsmon.find_all(lambda e: e.info and e.info.name.decode() == PRODUCT_NAME)) if len(target_nodes) == 1: break if len(target_nodes) > 1: abort('Expected to find exactly one target node, found more: %r', target_nodes) node_id = target_nodes[0].node_id info('Node %r initialized', node_id) for nd in target_nodes: logger.info('Discovered node %r', nd) def request(what): response_event = None def cb(e): nonlocal response_event if not e: abort('Request has timed out: %r', what) response_event = e n.request(what, node_id, cb) while response_event is None: safe_spin(0.1) return response_event.response # Starting the node and checking its self-reported diag outputs def wait_for_init(): with time_limit(10, 'The node did not complete initialization in time'): while True: safe_spin(1) if nsmon.exists(node_id) and nsmon.get(node_id).status.mode == \ uavcan.protocol.NodeStatus().MODE_OPERATIONAL: break def check_status(): status = nsmon.get(node_id).status enforce(status.mode == uavcan.protocol.NodeStatus().MODE_OPERATIONAL, 'Unexpected operating mode') enforce(status.health == uavcan.protocol.NodeStatus().HEALTH_OK, 'Bad node health') info('Waiting for the node to complete initialization...') wait_for_init() check_status() info('Resetting the configuration to factory defaults...') enforce(request(uavcan.protocol.param.ExecuteOpcode.Request( opcode=uavcan.protocol.param.ExecuteOpcode.Request().OPCODE_ERASE)).ok, 'The node refused to reset configuration to factory defaults') col_esc_status = uavcan.app.message_collector.MessageCollector(n, uavcan.equipment.esc.Status, timeout=10) def check_everything(check_rotation=False): check_status() try: m = col_esc_status[node_id].message except KeyError: abort('Rock is dead.') else: if check_rotation: enforce(m.rpm > 100 and m.power_rating_pct > 0, 'Could not start the motor') enforce(m.current > 0, 'Current is not positive') enforce(m.error_count < ESC_ERROR_LIMIT, 'High error count: %r', m.error_count) temp_degc = convert_units_from_to(m.temperature, 'Kelvin', 'Celsius') enforce(TEMPERATURE_RANGE_DEGC[0] <= temp_degc <= TEMPERATURE_RANGE_DEGC[1], 'Invalid temperature: %r degC', temp_degc) # Testing before the motor is started imperative('CAUTION: THE MOTOR WILL START IN 2 SECONDS, KEEP CLEAR') safe_spin(2) check_everything() # Starting the motor esc_raw_command_bitlen = \ uavcan.get_uavcan_data_type(uavcan.get_fields(uavcan.equipment.esc.RawCommand())['cmd'])\ .value_type.bitlen # SO EASY TO USE def do_publish(duty_cycle, check_rotation): command_value = int(duty_cycle * (2 ** (esc_raw_command_bitlen - 1))) n.broadcast(uavcan.equipment.esc.RawCommand(cmd=[command_value])) check_everything(check_rotation) info('Starting the motor') publisher = n.periodic(0.01, partial(do_publish, STARTUP_DUTY_CYCLE, False)) safe_spin(5) publisher.remove() info('Checking stability...') for dc in STABILITY_TEST_DUTY_CYCLES: info('Setting duty cycle %d%%...', int(dc * 100)) publisher = n.periodic(0.01, partial(do_publish, dc, True)) safe_spin(5) publisher.remove() info('Stopping...') latest_status = col_esc_status[node_id].message safe_spin(1) check_everything() # Final results info('Validate the latest ESC status variables (units are SI):\n%s', uavcan.to_yaml(latest_status)) # Testing CAN2 with BackgroundSpinner(safe_spin, 0.1): input('1. Disconnect CAN1 and connect to CAN2\n' '2. Terminate CAN2\n' '3. Press ENTER') safe_spin(1) try: check_status() except Exception as ex: logger.info('CAN2 test failed', exc_info=True) abort('CAN2 test failed [%r]', ex) # Testing LED info('Testing LED') def set_led(): rgb = uavcan.equipment.indication.RGB565(red=0b11111, green=0b111111, blue=0b11111) slc = uavcan.equipment.indication.SingleLightCommand(light_id=0, color=rgb) n.broadcast(uavcan.equipment.indication.LightsCommand(commands=[slc])) check_everything() publisher = n.periodic(0.1, set_led) with BackgroundSpinner(safe_spin, 0.1): if not input('Is the LED glowing bright white?', yes_no=True, default_answer=True): abort('LED is not working properly') publisher.remove() except Exception: for nid in nsmon.get_all_node_id(): logger.info('UAVCAN test failed; last known state of the device node: %r' % nsmon.get(nid)) raise
class AnalysisMainWindow(QMainWindow): DEFAULT_INTERVAL = 0.1 CMD_BIT_LENGTH = uavcan.get_uavcan_data_type(uavcan.equipment.esc.RawCommand().cmd).value_type.bitlen CMD_MAX = 2 ** (CMD_BIT_LENGTH - 1) - 1 CMD_MIN = -(2 ** (CMD_BIT_LENGTH - 1)) def __init__(self, parent, node): super(AnalysisMainWindow, self).__init__(parent) self.setAttribute(Qt.WA_DeleteOnClose) # This is required to stop background timers self._inferiors = None self._hook_handle = None # self.getTransfer=get_transfer_callback self._node = node self._initUI() node.add_handler(uavcan.equipment.esc.Status, self._updateEscInfo) self._bcast_timer = QTimer(self) self._bcast_timer.start(self.DEFAULT_INTERVAL * 1e3) self._bcast_timer.timeout.connect(self._do_broadcast) # self._transfer_timer = QTimer(self) # self._transfer_timer.setSingleShot(False) # self._transfer_timer.start(self.DEFAULT_INTERVAL * 1e3) # self._transfer_timer.timeout.connect(self._update) self._thrust_timer = QTimer(self) self._thrust_timer.start(self.DEFAULT_INTERVAL * 1e3) self._thrust_timer.timeout.connect(self._updateThrust) self._thrust_port = None self._thrust_queue = None self._refreshSerialPortsList() def _initUI(self): # # init layout # self.setGeometry(560, 240, 1000, 500) # self.setFixedSize(500, 500 self.setWindowTitle('Motor Analysis') self._mainWidget = QWidget(self) # init box layout mainHbox = QHBoxLayout(self._mainWidget) vboxLeft = QVBoxLayout(self._mainWidget) vboxRight = QVBoxLayout(self._mainWidget) mainHbox.addLayout(vboxLeft) mainHbox.addLayout(vboxRight) self._statusGroup = QGroupBox("Sensor Status") vboxStatus = QVBoxLayout(self._statusGroup) self._statusGroup.setLayout(vboxStatus) vboxLeft.addWidget(self._statusGroup) self._statusLabel = QLabel('Load info failed.') vboxStatus.addWidget(self._statusLabel) # fill right column self._controlTypeTabs = QTabWidget(self._mainWidget) self._manualControlPanel = ManualControlPanel(self._controlTypeTabs) self._manualControlPanel._recordWidget._getEscInfoCallback = self._getEscInfo self._manualControlPanel._recordWidget._getThrustCallback = self._getThrust self._automaticControlPanel = AutomaticControlPanel(self._controlTypeTabs, self._manualControlPanel._escSlider.set_value) self._automaticControlPanel._recordWidget._getEscInfoCallback = self._getEscInfo self._automaticControlPanel._recordWidget._getThrustCallback = self._getThrust self._controlTypeTabs.addTab(self._manualControlPanel, "Manual Control") self._controlTypeTabs.addTab(self._automaticControlPanel, "Automatic Control") vboxRight.addWidget(self._controlTypeTabs) self._plotWidget = PlotsWidget(self, self._getTransfer) vboxRight.addWidget(self._plotWidget) self._automaticControlPanel._recordWidget._setPlotStyle = self._plotWidget.setPlotStyle self.setCentralWidget(self._mainWidget) # # Control menu # control_menu = self.menuBar().addMenu('Plot &control') self._stop_action = QAction(get_icon('stop'), '&Stop Updates', self) self._stop_action.setStatusTip('While stopped, all new data will be discarded') self._stop_action.setShortcut(QKeySequence('Ctrl+Shift+S')) self._stop_action.setCheckable(True) self._stop_action.toggled.connect(self._on_stop_toggled) control_menu.addAction(self._stop_action) self._pause_action = QAction(get_icon('pause'), '&Pause Updates', self) self._pause_action.setStatusTip('While paused, new data will be accumulated in memory ' 'to be processed once un-paused') self._pause_action.setShortcut(QKeySequence('Ctrl+Shift+P')) self._pause_action.setCheckable(True) self._pause_action.toggled.connect(self._on_pause_toggled) control_menu.addAction(self._pause_action) control_menu.addSeparator() self._reset_time_action = QAction(get_icon('history'), '&Reset', self) self._reset_time_action.setStatusTip('Base time will be reset; all plots will be reset') self._reset_time_action.setShortcut(QKeySequence('Ctrl+Shift+R')) self._reset_time_action.triggered.connect(self._plotWidget._do_reset) control_menu.addAction(self._reset_time_action) self.setWindowTitle("Motor Status Plots") # # Config Menu # config_menu = self.menuBar().addMenu('Con&fig') self._thrust_ports_menu = QMenu("Thrust Sensor Ports") self._thrust_port_lists = QActionGroup(self) config_menu.addMenu(self._thrust_ports_menu) def _on_stop_toggled(self, checked): self._pause_action.setChecked(False) self.statusBar().showMessage('Stopped' if checked else 'Un-stopped') def _on_pause_toggled(self, checked): self.statusBar().showMessage('Paused' if checked else 'Un-paused') def _do_broadcast(self): try: msg = uavcan.equipment.esc.RPMCommand() raw_value = self._manualControlPanel._escSlider.get_value() value = raw_value msg.rpm.append(int(value)) self._node.broadcast(msg) except Exception as ex: logger.info("RPM Message publishing failed:" + str(ex)) return def _updateEscInfo(self, tr): self._tr=tr self._plotWidget._update() self._statusLabel.setText(''' Voltage :{} Current :{} Thrust :{} Torque :{} Weight :{} MotorSpeed:{} '''.format(str(tr.message.voltage)[0:5], str(tr.message.current)[0:5], self._getThrust(), "N/A", "N/A", tr.message.rpm)) def _getThrust(self): return self._plotWidget._plc_thrust._value def _getEscInfo(self): return self._tr.message def _getTransfer(self): return self._tr def _refreshSerialPortsList(self): print("refresh serial ports list") ports = list(serial.tools.list_ports.comports()) self._thrust_ports_menu.clear() if(len(ports)==0): action=QAction("No Serial Device Found.",self) self._thrust_ports_menu.addAction(action) return for p in ports: print("{}:{}".format(p[0],p[1])) action=QAction("{}:{}".format(p[0],p[1]),self) action.setCheckable(True) action.triggered.connect(partial(self._setThrustSerialPort,port=p[0])) self._thrust_port_lists.addAction(action) self._thrust_ports_menu.addAction(action) def _updateThrust(self): if (self._thrust_queue != None): try: thrust = self._thrust_queue.get_nowait() except Exception: return self._plotWidget._plc_thrust.setValue(thrust) def _setThrustSerialPort(self,port): print(port) self._thrust_port=port self._plotWidget._plc_thrust.setHowToLabel("Thrust data source successfully set to {}".format(port) , "success") self._startThrustAcquireLoop() def _startThrustAcquireLoop(self): def handle_data(data,q): if(data!=b'' and data!=b'-' and data!=b'\n'): q.put(data.decode().rstrip()) def read_from_port(ser,q): while ser.isOpen(): msg = ser.read(ser.inWaiting()) handle_data(msg,q) time.sleep(0.1) if(self._thrust_port!=None): print("set thrust port:{}".format(self._thrust_port)) arduino = serial.Serial(self._thrust_port, 9600, timeout=5) self._thrust_queue=Queue() self._thrust_read_thread=Thread(target=read_from_port,args=(arduino,self._thrust_queue)) self._thrust_read_thread.start() else: msgbox = QMessageBox(self) msgbox.setText("ERROR: Can not configure the thrust device automatically." + "Please select it manually in <Thrust Sensor Ports> menu.") msgbox.exec()
def _to_yaml_impl(obj, indent_level=0, parent=None, name=None, uavcan_type=None): buf = StringIO() def write(fmt, *args): buf.write((fmt % args) if len(args) else fmt) def indent_newline(): buf.write(os.linesep + ' ' * 2 * indent_level) # Decomposing PrimitiveValue to value and type. This is ugly but it's by design... if isinstance(obj, PrimitiveValue): uavcan_type = uavcan.get_uavcan_data_type(obj) obj = obj.value # CompoundValue if isinstance(obj, CompoundValue): first_field = True # Rendering all fields than can be rendered for field_name, field in uavcan.get_fields(obj).items(): if uavcan.is_union( obj) and uavcan.get_active_union_field(obj) != field_name: continue if isinstance(field, VoidValue): continue if (first_field and indent_level > 0) or not first_field: indent_newline() first_field = False rendered_field = _to_yaml_impl(field, indent_level=indent_level + 1, parent=obj, name=field_name) write('%s: %s', field_name, rendered_field) # Special case - empty non-union struct is rendered as empty map if first_field and not uavcan.is_union(obj): if indent_level > 0: indent_newline() write('{}') # ArrayValue elif isinstance(obj, ArrayValue): t = uavcan.get_uavcan_data_type(obj) if t.value_type.category == t.value_type.CATEGORY_PRIMITIVE: def is_nice_character(ch): if 32 <= ch <= 126: return True if ch in b'\n\r\t': return True return False as_bytes = '[%s]' % ', '.join([ _to_yaml_impl( x, indent_level=indent_level + 1, uavcan_type=t.value_type) for x in obj ]) if t.is_string_like and all(map(is_nice_character, obj)): write('%r # ', obj.decode()) write(as_bytes) else: if len(obj) == 0: write('[]') else: for x in obj: indent_newline() write( '- %s', _to_yaml_impl(x, indent_level=indent_level + 1, uavcan_type=t.value_type)) # Primitive types elif isinstance(obj, float): assert uavcan_type is not None float_fmt = { 16: '%.4f', 32: '%.6f', 64: '%.9f', }[uavcan_type.bitlen] write(float_fmt, obj) elif isinstance(obj, bool): write('%s', 'true' if obj else 'false') elif isinstance(obj, int): write('%s', obj) if parent is not None and name is not None: resolved_name = value_to_constant_name(parent, name) if isinstance(resolved_name, str): write(' # %s', resolved_name) # Non-printable types elif isinstance(obj, VoidValue): pass # Unknown types else: raise ValueError('Cannot generate YAML representation for %r' % type(obj)) return buf.getvalue()