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