Merge branch 'upstream/openpilot/master' into sync-20250908

# Conflicts:
#	.github/workflows/selfdrive_tests.yaml
#	README.md
#	docs/CARS.md
#	opendbc_repo
#	panda
#	selfdrive/car/tests/test_car_interfaces.py
#	selfdrive/modeld/modeld.py
#	selfdrive/selfdrived/selfdrived.py
#	selfdrive/ui/translations/main_ar.ts
#	selfdrive/ui/translations/main_de.ts
#	selfdrive/ui/translations/main_es.ts
#	selfdrive/ui/translations/main_fr.ts
#	selfdrive/ui/translations/main_ja.ts
#	selfdrive/ui/translations/main_ko.ts
#	selfdrive/ui/translations/main_pt-BR.ts
#	selfdrive/ui/translations/main_th.ts
#	selfdrive/ui/translations/main_tr.ts
#	selfdrive/ui/translations/main_zh-CHS.ts
#	selfdrive/ui/translations/main_zh-CHT.ts
#	system/hardware/hardwared.py
#	system/updated/updated.py
#	tinygrad_repo
#	uv.lock
This commit is contained in:
Jason Wen
2025-09-12 12:15:40 -04:00
136 changed files with 4744 additions and 6884 deletions

View File

@@ -1,6 +1,6 @@
#!/usr/bin/env python3
import sys
from openpilot.tools.lib.logreader import LogReader
from openpilot.tools.lib.logreader import LogReader, ReadMode
def main():
@@ -9,7 +9,7 @@ def main():
sys.exit(1)
log_path = sys.argv[1]
lr = LogReader(log_path, sort_by_time=True)
lr = LogReader(log_path, default_mode=ReadMode.AUTO, sort_by_time=True)
print("\n".join(lr.logreader_identifiers))

View File

@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:3ea96d8193eb9067a5efdc5d88a3099730ecafa40efcd09d7402bb3efd723603
size 2305

View File

@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:53097ac5403b725ff1841dfa186ea770b4bb3714205824bde36ec3c2a0fb5dba
size 2758

View File

@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:54dd035ff898d881509fa686c402a61af8ef5fb408b92414722da01f773b0d33
size 2900

View File

@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:adbd4e5df1f58694dca9dde46d1d95b4e7471684e42e6bca9f41ea5d346e67c5
size 3669

View File

@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:a6d9c90cb0dd906e0b15e1f7f3fd9f0dfad3c3b0b34eeed7a7882768dc5f3961
size 2053

352
tools/jotpluggler/data.py Normal file
View File

@@ -0,0 +1,352 @@
import numpy as np
import threading
import multiprocessing
import bisect
from collections import defaultdict
from tqdm import tqdm
from openpilot.common.swaglog import cloudlog
from openpilot.tools.lib.logreader import _LogFileReader, LogReader
def flatten_dict(d: dict, sep: str = "/", prefix: str = None) -> dict:
result = {}
stack: list[tuple] = [(d, prefix)]
while stack:
obj, current_prefix = stack.pop()
if isinstance(obj, dict):
for key, val in obj.items():
new_prefix = key if current_prefix is None else f"{current_prefix}{sep}{key}"
if isinstance(val, (dict, list)):
stack.append((val, new_prefix))
else:
result[new_prefix] = val
elif isinstance(obj, list):
for i, item in enumerate(obj):
new_prefix = f"{current_prefix}{sep}{i}"
if isinstance(item, (dict, list)):
stack.append((item, new_prefix))
else:
result[new_prefix] = item
else:
if current_prefix is not None:
result[current_prefix] = obj
return result
def extract_field_types(schema, prefix, field_types_dict):
stack = [(schema, prefix)]
while stack:
current_schema, current_prefix = stack.pop()
for field in current_schema.fields_list:
field_name = field.proto.name
field_path = f"{current_prefix}/{field_name}"
field_proto = field.proto
field_which = field_proto.which()
field_type = field_proto.slot.type.which() if field_which == 'slot' else field_which
field_types_dict[field_path] = field_type
if field_which == 'slot':
slot_type = field_proto.slot.type
type_which = slot_type.which()
if type_which == 'list':
element_type = slot_type.list.elementType.which()
list_path = f"{field_path}/*"
field_types_dict[list_path] = element_type
if element_type == 'struct':
stack.append((field.schema.elementType, list_path))
elif type_which == 'struct':
stack.append((field.schema, field_path))
elif field_which == 'group':
stack.append((field.schema, field_path))
def _convert_to_optimal_dtype(values_list, capnp_type):
dtype_mapping = {
'bool': np.bool_, 'int8': np.int8, 'int16': np.int16, 'int32': np.int32, 'int64': np.int64,
'uint8': np.uint8, 'uint16': np.uint16, 'uint32': np.uint32, 'uint64': np.uint64,
'float32': np.float32, 'float64': np.float64, 'text': object, 'data': object,
'enum': object, 'anyPointer': object,
}
target_dtype = dtype_mapping.get(capnp_type, object)
return np.array(values_list, dtype=target_dtype)
def _match_field_type(field_path, field_types):
if field_path in field_types:
return field_types[field_path]
path_parts = field_path.split('/')
template_parts = [p if not p.isdigit() else '*' for p in path_parts]
template_path = '/'.join(template_parts)
return field_types.get(template_path)
def _get_field_times_values(segment, field_name):
if field_name not in segment:
return None, None
field_data = segment[field_name]
segment_times = segment['t']
if field_data['sparse']:
if len(field_data['t_index']) == 0:
return None, None
return segment_times[field_data['t_index']], field_data['values']
else:
return segment_times, field_data['values']
def msgs_to_time_series(msgs):
"""Extract scalar fields and return (time_series_data, start_time, end_time)."""
collected_data = defaultdict(lambda: {'timestamps': [], 'columns': defaultdict(list), 'sparse_fields': set()})
field_types = {}
extracted_schemas = set()
min_time = max_time = None
for msg in msgs:
typ = msg.which()
timestamp = msg.logMonoTime * 1e-9
if typ != 'initData':
if min_time is None:
min_time = timestamp
max_time = timestamp
sub_msg = getattr(msg, typ)
if not hasattr(sub_msg, 'to_dict'):
continue
if hasattr(sub_msg, 'schema') and typ not in extracted_schemas:
extract_field_types(sub_msg.schema, typ, field_types)
extracted_schemas.add(typ)
try:
msg_dict = sub_msg.to_dict(verbose=True)
except Exception as e:
cloudlog.warning(f"Failed to convert sub_msg.to_dict() for message of type: {typ}: {e}")
continue
flat_dict = flatten_dict(msg_dict)
flat_dict['_valid'] = msg.valid
field_types[f"{typ}/_valid"] = 'bool'
type_data = collected_data[typ]
columns, sparse_fields = type_data['columns'], type_data['sparse_fields']
known_fields = set(columns.keys())
missing_fields = known_fields - flat_dict.keys()
for field, value in flat_dict.items():
if field not in known_fields and type_data['timestamps']:
sparse_fields.add(field)
columns[field].append(value)
if value is None:
sparse_fields.add(field)
for field in missing_fields:
columns[field].append(None)
sparse_fields.add(field)
type_data['timestamps'].append(timestamp)
final_result = {}
for typ, data in collected_data.items():
if not data['timestamps']:
continue
typ_result = {'t': np.array(data['timestamps'], dtype=np.float64)}
sparse_fields = data['sparse_fields']
for field_name, values in data['columns'].items():
if len(values) < len(data['timestamps']):
values = [None] * (len(data['timestamps']) - len(values)) + values
sparse_fields.add(field_name)
capnp_type = _match_field_type(f"{typ}/{field_name}", field_types)
if field_name in sparse_fields: # extract non-None values and their indices
non_none_indices = []
non_none_values = []
for i, value in enumerate(values):
if value is not None:
non_none_indices.append(i)
non_none_values.append(value)
if non_none_values: # check if indices > uint16 max, currently would require a 1000+ Hz signal since indices are within segments
assert max(non_none_indices) <= 65535, f"Sparse field {typ}/{field_name} has timestamp indices exceeding uint16 max. Max: {max(non_none_indices)}"
typ_result[field_name] = {
'values': _convert_to_optimal_dtype(non_none_values, capnp_type),
'sparse': True,
't_index': np.array(non_none_indices, dtype=np.uint16),
}
else: # dense representation
typ_result[field_name] = {'values': _convert_to_optimal_dtype(values, capnp_type), 'sparse': False}
final_result[typ] = typ_result
return final_result, min_time or 0.0, max_time or 0.0
def _process_segment(segment_identifier: str):
try:
lr = _LogFileReader(segment_identifier, sort_by_time=True)
return msgs_to_time_series(lr)
except Exception as e:
cloudlog.warning(f"Warning: Failed to process segment {segment_identifier}: {e}")
return {}, 0.0, 0.0
class DataManager:
def __init__(self):
self._segments = []
self._segment_starts = []
self._start_time = 0.0
self._duration = 0.0
self._paths = set()
self._observers = []
self._loading = False
self._lock = threading.RLock()
def load_route(self, route: str) -> None:
if self._loading:
return
self._reset()
threading.Thread(target=self._load_async, args=(route,), daemon=True).start()
def get_timeseries(self, path: str):
with self._lock:
msg_type, field = path.split('/', 1)
times, values = [], []
for segment in self._segments:
if msg_type in segment:
field_times, field_values = _get_field_times_values(segment[msg_type], field)
if field_times is not None:
times.append(field_times)
values.append(field_values)
if not times:
return np.array([]), np.array([])
combined_times = np.concatenate(times) - self._start_time
if len(values) > 1:
first_dtype = values[0].dtype
if all(arr.dtype == first_dtype for arr in values): # check if all arrays have compatible dtypes
combined_values = np.concatenate(values)
else:
combined_values = np.concatenate([arr.astype(object) for arr in values])
else:
combined_values = values[0] if values else np.array([])
return combined_times, combined_values
def get_value_at(self, path: str, time: float):
with self._lock:
MAX_LOOKBACK = 5.0 # seconds
absolute_time = self._start_time + time
message_type, field = path.split('/', 1)
current_index = bisect.bisect_right(self._segment_starts, absolute_time) - 1
for index in (current_index, current_index - 1):
if not 0 <= index < len(self._segments):
continue
segment = self._segments[index].get(message_type)
if not segment:
continue
times, values = _get_field_times_values(segment, field)
if times is None or len(times) == 0 or (index != current_index and absolute_time - times[-1] > MAX_LOOKBACK):
continue
position = np.searchsorted(times, absolute_time, 'right') - 1
if position >= 0 and absolute_time - times[position] <= MAX_LOOKBACK:
return values[position]
return None
def get_all_paths(self):
with self._lock:
return sorted(self._paths)
def get_duration(self):
with self._lock:
return self._duration
def is_plottable(self, path: str):
_, values = self.get_timeseries(path)
if len(values) == 0:
return False
return np.issubdtype(values.dtype, np.number) or np.issubdtype(values.dtype, np.bool_)
def add_observer(self, callback):
with self._lock:
self._observers.append(callback)
def remove_observer(self, callback):
with self._lock:
if callback in self._observers:
self._observers.remove(callback)
def _reset(self):
with self._lock:
self._loading = True
self._segments.clear()
self._segment_starts.clear()
self._paths.clear()
self._start_time = self._duration = 0.0
observers = self._observers.copy()
for callback in observers:
callback({'reset': True})
def _load_async(self, route: str):
try:
lr = LogReader(route, sort_by_time=True)
if not lr.logreader_identifiers:
cloudlog.warning(f"Warning: No log segments found for route: {route}")
return
num_processes = max(1, multiprocessing.cpu_count() // 2)
with multiprocessing.Pool(processes=num_processes) as pool, tqdm(total=len(lr.logreader_identifiers), desc="Processing Segments") as pbar:
for segment_result, start_time, end_time in pool.imap(_process_segment, lr.logreader_identifiers):
pbar.update(1)
if segment_result:
self._add_segment(segment_result, start_time, end_time)
except Exception:
cloudlog.exception(f"Error loading route {route}:")
finally:
self._finalize_loading()
def _add_segment(self, segment_data: dict, start_time: float, end_time: float):
with self._lock:
self._segments.append(segment_data)
self._segment_starts.append(start_time)
if len(self._segments) == 1:
self._start_time = start_time
self._duration = end_time - self._start_time
for msg_type, data in segment_data.items():
for field_name in data.keys():
if field_name != 't':
self._paths.add(f"{msg_type}/{field_name}")
observers = self._observers.copy()
for callback in observers:
callback({'segment_added': True, 'duration': self._duration, 'segment_count': len(self._segments)})
def _finalize_loading(self):
with self._lock:
self._loading = False
observers = self._observers.copy()
duration = self._duration
for callback in observers:
callback({'loading_complete': True, 'duration': duration})

View File

@@ -0,0 +1,315 @@
import os
import re
import threading
import numpy as np
import dearpygui.dearpygui as dpg
class DataTreeNode:
def __init__(self, name: str, full_path: str = "", parent=None):
self.name = name
self.full_path = full_path
self.parent = parent
self.children: dict[str, DataTreeNode] = {}
self.filtered_children: dict[str, DataTreeNode] = {}
self.created_children: dict[str, DataTreeNode] = {}
self.is_leaf = False
self.is_plottable: bool | None = None
self.ui_created = False
self.children_ui_created = False
self.ui_tag: str | None = None
class DataTree:
MAX_NODES_PER_FRAME = 50
def __init__(self, data_manager, playback_manager):
self.data_manager = data_manager
self.playback_manager = playback_manager
self.current_search = ""
self.data_tree = DataTreeNode(name="root")
self._build_queue: dict[str, tuple[DataTreeNode, DataTreeNode, str | int]] = {} # full_path -> (node, parent, before_tag)
self._current_created_paths: set[str] = set()
self._current_filtered_paths: set[str] = set()
self._path_to_node: dict[str, DataTreeNode] = {} # full_path -> node
self._expanded_tags: set[str] = set()
self._item_handlers: dict[str, str] = {} # ui_tag -> handler_tag
self._char_width = None
self._queued_search = None
self._new_data = False
self._ui_lock = threading.RLock()
self._handlers_to_delete = []
self.data_manager.add_observer(self._on_data_loaded)
def create_ui(self, parent_tag: str):
with dpg.child_window(parent=parent_tag, border=False, width=-1, height=-1):
dpg.add_text("Timeseries List")
dpg.add_separator()
dpg.add_input_text(tag="search_input", width=-1, hint="Search fields...", callback=self.search_data)
dpg.add_separator()
with dpg.child_window(border=False, width=-1, height=-1):
with dpg.group(tag="data_tree_container"):
pass
def _on_data_loaded(self, data: dict):
with self._ui_lock:
if data.get('segment_added') or data.get('reset'):
self._new_data = True
def update_frame(self, font):
if self._handlers_to_delete: # we need to do everything in main thread, frame callbacks are flaky
dpg.render_dearpygui_frame() # wait a frame to ensure queued callbacks are done
with self._ui_lock:
for handler in self._handlers_to_delete:
dpg.delete_item(handler)
self._handlers_to_delete.clear()
with self._ui_lock:
if self._char_width is None:
if size := dpg.get_text_size(" ", font=font):
self._char_width = size[0]
if self._new_data:
self._process_path_change()
self._new_data = False
return
if self._queued_search is not None:
self.current_search = self._queued_search
self._process_path_change()
self._queued_search = None
return
nodes_processed = 0
while self._build_queue and nodes_processed < self.MAX_NODES_PER_FRAME:
child_node, parent, before_tag = self._build_queue.pop(next(iter(self._build_queue)))
parent_tag = "data_tree_container" if parent.name == "root" else parent.ui_tag
if not child_node.ui_created:
if child_node.is_leaf:
self._create_leaf_ui(child_node, parent_tag, before_tag)
else:
self._create_tree_node_ui(child_node, parent_tag, before_tag)
parent.created_children[child_node.name] = parent.children[child_node.name]
self._current_created_paths.add(child_node.full_path)
nodes_processed += 1
def _process_path_change(self):
self._build_queue.clear()
search_term = self.current_search.strip().lower()
all_paths = set(self.data_manager.get_all_paths())
new_filtered_leafs = {path for path in all_paths if self._should_show_path(path, search_term)}
new_filtered_paths = set(new_filtered_leafs)
for path in new_filtered_leafs:
parts = path.split('/')
for i in range(1, len(parts)):
prefix = '/'.join(parts[:i])
new_filtered_paths.add(prefix)
created_paths_to_remove = self._current_created_paths - new_filtered_paths
filtered_paths_to_remove = self._current_filtered_paths - new_filtered_leafs
if created_paths_to_remove or filtered_paths_to_remove:
self._remove_paths_from_tree(created_paths_to_remove, filtered_paths_to_remove)
self._apply_expansion_to_tree(self.data_tree, search_term)
paths_to_add = new_filtered_leafs - self._current_created_paths
if paths_to_add:
self._add_paths_to_tree(paths_to_add)
self._apply_expansion_to_tree(self.data_tree, search_term)
self._current_filtered_paths = new_filtered_paths
def _remove_paths_from_tree(self, created_paths_to_remove, filtered_paths_to_remove):
for path in sorted(created_paths_to_remove, reverse=True):
current_node = self._path_to_node[path]
if len(current_node.created_children) == 0:
self._current_created_paths.remove(current_node.full_path)
if item_handler_tag := self._item_handlers.get(current_node.ui_tag):
dpg.configure_item(item_handler_tag, show=False)
self._handlers_to_delete.append(item_handler_tag)
del self._item_handlers[current_node.ui_tag]
dpg.delete_item(current_node.ui_tag)
current_node.ui_created = False
current_node.ui_tag = None
current_node.children_ui_created = False
del current_node.parent.created_children[current_node.name]
del current_node.parent.filtered_children[current_node.name]
for path in filtered_paths_to_remove:
parts = path.split('/')
current_node = self._path_to_node[path]
part_array_index = -1
while len(current_node.filtered_children) == 0 and part_array_index >= -len(parts):
current_node = current_node.parent
if parts[part_array_index] in current_node.filtered_children:
del current_node.filtered_children[parts[part_array_index]]
part_array_index -= 1
def _add_paths_to_tree(self, paths):
parent_nodes_to_recheck = set()
for path in sorted(paths):
parts = path.split('/')
current_node = self.data_tree
current_path_prefix = ""
for i, part in enumerate(parts):
current_path_prefix = f"{current_path_prefix}/{part}" if current_path_prefix else part
if i < len(parts) - 1:
parent_nodes_to_recheck.add(current_node) # for incremental changes from new data
if part not in current_node.children:
current_node.children[part] = DataTreeNode(name=part, full_path=current_path_prefix, parent=current_node)
self._path_to_node[current_path_prefix] = current_node.children[part]
current_node.filtered_children[part] = current_node.children[part]
current_node = current_node.children[part]
if not current_node.is_leaf:
current_node.is_leaf = True
for p_node in parent_nodes_to_recheck:
p_node.children_ui_created = False
self._request_children_build(p_node)
def _get_node_label_and_expand(self, node: DataTreeNode, search_term: str):
label = f"{node.name} ({len(node.filtered_children)} fields)"
expand = len(search_term) > 0 and any(search_term in path for path in self._get_descendant_paths(node))
if expand and node.parent and len(node.parent.filtered_children) > 100 and len(node.filtered_children) > 2:
label += " (+)" # symbol for large lists which aren't fully expanded for performance (only affects procLog rn)
expand = False
return label, expand
def _apply_expansion_to_tree(self, node: DataTreeNode, search_term: str):
if node.ui_created and not node.is_leaf and node.ui_tag and dpg.does_item_exist(node.ui_tag):
label, expand = self._get_node_label_and_expand(node, search_term)
if expand:
self._expanded_tags.add(node.ui_tag)
dpg.set_value(node.ui_tag, expand)
elif node.ui_tag in self._expanded_tags: # not expanded and was expanded
self._expanded_tags.remove(node.ui_tag)
dpg.set_value(node.ui_tag, expand)
dpg.delete_item(node.ui_tag, children_only=True) # delete children (not visible since collapsed)
self._reset_ui_state_recursive(node)
node.children_ui_created = False
dpg.set_item_label(node.ui_tag, label)
for child in node.created_children.values():
self._apply_expansion_to_tree(child, search_term)
def _reset_ui_state_recursive(self, node: DataTreeNode):
for child in node.created_children.values():
if child.ui_tag is not None:
if item_handler_tag := self._item_handlers.get(child.ui_tag):
self._handlers_to_delete.append(item_handler_tag)
dpg.configure_item(item_handler_tag, show=False)
del self._item_handlers[child.ui_tag]
self._reset_ui_state_recursive(child)
child.ui_created = False
child.ui_tag = None
child.children_ui_created = False
self._current_created_paths.remove(child.full_path)
node.created_children.clear()
def search_data(self):
with self._ui_lock:
self._queued_search = dpg.get_value("search_input")
def _create_tree_node_ui(self, node: DataTreeNode, parent_tag: str, before: str | int):
node.ui_tag = f"tree_{node.full_path}"
search_term = self.current_search.strip().lower()
label, expand = self._get_node_label_and_expand(node, search_term)
if expand:
self._expanded_tags.add(node.ui_tag)
elif node.ui_tag in self._expanded_tags:
self._expanded_tags.remove(node.ui_tag)
with dpg.tree_node(
label=label, parent=parent_tag, tag=node.ui_tag, default_open=expand, open_on_arrow=True, open_on_double_click=True, before=before, delay_search=True
):
with dpg.item_handler_registry() as handler_tag:
dpg.add_item_toggled_open_handler(callback=lambda s, a, u: self._request_children_build(node))
dpg.add_item_visible_handler(callback=lambda s, a, u: self._request_children_build(node))
dpg.bind_item_handler_registry(node.ui_tag, handler_tag)
self._item_handlers[node.ui_tag] = handler_tag
node.ui_created = True
def _create_leaf_ui(self, node: DataTreeNode, parent_tag: str, before: str | int):
node.ui_tag = f"leaf_{node.full_path}"
with dpg.group(parent=parent_tag, tag=node.ui_tag, before=before, delay_search=True):
with dpg.table(header_row=False, policy=dpg.mvTable_SizingStretchProp, delay_search=True):
dpg.add_table_column(init_width_or_weight=0.5)
dpg.add_table_column(init_width_or_weight=0.5)
with dpg.table_row():
dpg.add_text(node.name)
dpg.add_text("N/A", tag=f"value_{node.full_path}")
if node.is_plottable is None:
node.is_plottable = self.data_manager.is_plottable(node.full_path)
if node.is_plottable:
with dpg.drag_payload(parent=node.ui_tag, drag_data=node.full_path, payload_type="TIMESERIES_PAYLOAD"):
dpg.add_text(f"Plot: {node.full_path}")
with dpg.item_handler_registry() as handler_tag:
dpg.add_item_visible_handler(callback=self._on_item_visible, user_data=node.full_path)
dpg.bind_item_handler_registry(node.ui_tag, handler_tag)
self._item_handlers[node.ui_tag] = handler_tag
node.ui_created = True
def _on_item_visible(self, sender, app_data, user_data):
with self._ui_lock:
path = user_data
value_tag = f"value_{path}"
if not dpg.does_item_exist(value_tag):
return
value_column_width = dpg.get_item_rect_size(f"leaf_{path}")[0] // 2
value = self.data_manager.get_value_at(path, self.playback_manager.current_time_s)
if value is not None:
formatted_value = self.format_and_truncate(value, value_column_width, self._char_width)
dpg.set_value(value_tag, formatted_value)
else:
dpg.set_value(value_tag, "N/A")
def _request_children_build(self, node: DataTreeNode):
with self._ui_lock:
if not node.children_ui_created and (node.name == "root" or (node.ui_tag is not None and dpg.get_value(node.ui_tag))): # check root or node expanded
sorted_children = sorted(node.filtered_children.values(), key=self._natural_sort_key)
next_existing: list[int | str] = [0] * len(sorted_children)
current_before_tag: int | str = 0
for i in range(len(sorted_children) - 1, -1, -1): # calculate "before_tag" for correct ordering when incrementally building tree
child = sorted_children[i]
next_existing[i] = current_before_tag
if child.ui_created:
candidate_tag = f"leaf_{child.full_path}" if child.is_leaf else f"tree_{child.full_path}"
if dpg.does_item_exist(candidate_tag):
current_before_tag = candidate_tag
for i, child_node in enumerate(sorted_children):
if not child_node.ui_created:
before_tag = next_existing[i]
self._build_queue[child_node.full_path] = (child_node, node, before_tag)
node.children_ui_created = True
def _should_show_path(self, path: str, search_term: str) -> bool:
if 'DEPRECATED' in path and not os.environ.get('SHOW_DEPRECATED'):
return False
return not search_term or search_term in path.lower()
def _natural_sort_key(self, node: DataTreeNode):
node_type_key = node.is_leaf
parts = [int(p) if p.isdigit() else p.lower() for p in re.split(r'(\d+)', node.name) if p]
return (node_type_key, parts)
def _get_descendant_paths(self, node: DataTreeNode):
for child_name, child_node in node.filtered_children.items():
child_name_lower = child_name.lower()
if child_node.is_leaf:
yield child_name_lower
else:
for path in self._get_descendant_paths(child_node):
yield f"{child_name_lower}/{path}"
@staticmethod
def format_and_truncate(value, available_width: float, char_width: float) -> str:
s = f"{value:.5f}" if np.issubdtype(type(value), np.floating) else str(value)
max_chars = int(available_width / char_width)
if len(s) > max_chars:
return s[: max(0, max_chars - 3)] + "..."
return s

272
tools/jotpluggler/layout.py Normal file
View File

@@ -0,0 +1,272 @@
import dearpygui.dearpygui as dpg
from openpilot.tools.jotpluggler.data import DataManager
from openpilot.tools.jotpluggler.views import TimeSeriesPanel
GRIP_SIZE = 4
MIN_PANE_SIZE = 60
class PlotLayoutManager:
def __init__(self, data_manager: DataManager, playback_manager, worker_manager, scale: float = 1.0):
self.data_manager = data_manager
self.playback_manager = playback_manager
self.worker_manager = worker_manager
self.scale = scale
self.container_tag = "plot_layout_container"
self.active_panels: list = []
self.grip_size = int(GRIP_SIZE * self.scale)
self.min_pane_size = int(MIN_PANE_SIZE * self.scale)
initial_panel = TimeSeriesPanel(data_manager, playback_manager, worker_manager)
self.layout: dict = {"type": "panel", "panel": initial_panel}
def create_ui(self, parent_tag: str):
if dpg.does_item_exist(self.container_tag):
dpg.delete_item(self.container_tag)
with dpg.child_window(tag=self.container_tag, parent=parent_tag, border=False, width=-1, height=-1, no_scrollbar=True, no_scroll_with_mouse=True):
container_width, container_height = dpg.get_item_rect_size(self.container_tag)
self._create_ui_recursive(self.layout, self.container_tag, [], container_width, container_height)
def _create_ui_recursive(self, layout: dict, parent_tag: str, path: list[int], width: int, height: int):
if layout["type"] == "panel":
self._create_panel_ui(layout, parent_tag, path, width, height)
else:
self._create_split_ui(layout, parent_tag, path, width, height)
def _create_panel_ui(self, layout: dict, parent_tag: str, path: list[int], width: int, height:int):
panel_tag = self._path_to_tag(path, "panel")
panel = layout["panel"]
self.active_panels.append(panel)
text_size = int(13 * self.scale)
bar_height = (text_size+24) if width < int(279 * self.scale + 80) else (text_size+8) # adjust height to allow for scrollbar
with dpg.child_window(parent=parent_tag, border=True, width=-1, height=-1, no_scrollbar=True):
with dpg.group(horizontal=True):
with dpg.child_window(tag=panel_tag, width=-(text_size + 16), height=bar_height, horizontal_scrollbar=True, no_scroll_with_mouse=True, border=False):
with dpg.group(horizontal=True):
dpg.add_input_text(default_value=panel.title, width=int(100 * self.scale), callback=lambda s, v: setattr(panel, "title", v))
dpg.add_combo(items=["Time Series"], default_value="Time Series", width=int(100 * self.scale))
dpg.add_button(label="Clear", callback=lambda: self.clear_panel(panel), width=int(40 * self.scale))
dpg.add_image_button(texture_tag="split_h_texture", callback=lambda: self.split_panel(path, 0), width=text_size, height=text_size)
dpg.add_image_button(texture_tag="split_v_texture", callback=lambda: self.split_panel(path, 1), width=text_size, height=text_size)
dpg.add_image_button(texture_tag="x_texture", callback=lambda: self.delete_panel(path), width=text_size, height=text_size)
dpg.add_separator()
content_tag = self._path_to_tag(path, "content")
with dpg.child_window(tag=content_tag, border=False, height=-1, width=-1, no_scrollbar=True):
panel.create_ui(content_tag)
def _create_split_ui(self, layout: dict, parent_tag: str, path: list[int], width: int, height: int):
split_tag = self._path_to_tag(path, "split")
orientation, _, pane_sizes = self._get_split_geometry(layout, (width, height))
with dpg.group(tag=split_tag, parent=parent_tag, horizontal=orientation == 0):
for i, child_layout in enumerate(layout["children"]):
child_path = path + [i]
container_tag = self._path_to_tag(child_path, "container")
pane_width, pane_height = [(pane_sizes[i], -1), (-1, pane_sizes[i])][orientation] # fill 2nd dim up to the border
with dpg.child_window(tag=container_tag, width=pane_width, height=pane_height, border=False, no_scrollbar=True):
child_width, child_height = [(pane_sizes[i], height), (width, pane_sizes[i])][orientation]
self._create_ui_recursive(child_layout, container_tag, child_path, child_width, child_height)
if i < len(layout["children"]) - 1:
self._create_grip(split_tag, path, i, orientation)
def clear_panel(self, panel):
panel.clear()
def delete_panel(self, panel_path: list[int]):
if not panel_path: # Root deletion
old_panel = self.layout["panel"]
old_panel.destroy_ui()
self.active_panels.remove(old_panel)
new_panel = TimeSeriesPanel(self.data_manager, self.playback_manager, self.worker_manager)
self.layout = {"type": "panel", "panel": new_panel}
self._rebuild_ui_at_path([])
return
parent, child_index = self._get_parent_and_index(panel_path)
layout_to_delete = parent["children"][child_index]
self._cleanup_ui_recursive(layout_to_delete, panel_path)
parent["children"].pop(child_index)
parent["proportions"].pop(child_index)
if len(parent["children"]) == 1: # remove parent and collapse
remaining_child = parent["children"][0]
if len(panel_path) == 1: # parent is at root level - promote remaining child to root
self.layout = remaining_child
self._rebuild_ui_at_path([])
else: # replace parent with remaining child in grandparent
grandparent_path = panel_path[:-2]
parent_index = panel_path[-2]
self._replace_layout_at_path(grandparent_path + [parent_index], remaining_child)
self._rebuild_ui_at_path(grandparent_path + [parent_index])
else: # redistribute proportions
equal_prop = 1.0 / len(parent["children"])
parent["proportions"] = [equal_prop] * len(parent["children"])
self._rebuild_ui_at_path(panel_path[:-1])
def split_panel(self, panel_path: list[int], orientation: int):
current_layout = self._get_layout_at_path(panel_path)
existing_panel = current_layout["panel"]
new_panel = TimeSeriesPanel(self.data_manager, self.playback_manager, self.worker_manager)
parent, child_index = self._get_parent_and_index(panel_path)
if parent is None: # Root split
self.layout = {
"type": "split",
"orientation": orientation,
"children": [{"type": "panel", "panel": existing_panel}, {"type": "panel", "panel": new_panel}],
"proportions": [0.5, 0.5],
}
self._rebuild_ui_at_path([])
elif parent["type"] == "split" and parent["orientation"] == orientation: # Same orientation - insert into existing split
parent["children"].insert(child_index + 1, {"type": "panel", "panel": new_panel})
parent["proportions"] = [1.0 / len(parent["children"])] * len(parent["children"])
self._rebuild_ui_at_path(panel_path[:-1])
else: # Different orientation - create new split level
new_split = {"type": "split", "orientation": orientation, "children": [current_layout, {"type": "panel", "panel": new_panel}], "proportions": [0.5, 0.5]}
self._replace_layout_at_path(panel_path, new_split)
self._rebuild_ui_at_path(panel_path)
def _rebuild_ui_at_path(self, path: list[int]):
layout = self._get_layout_at_path(path)
if path:
container_tag = self._path_to_tag(path, "container")
else: # Root update
container_tag = self.container_tag
self._cleanup_ui_recursive(layout, path)
dpg.delete_item(container_tag, children_only=True)
width, height = dpg.get_item_rect_size(container_tag)
self._create_ui_recursive(layout, container_tag, path, width, height)
def _cleanup_ui_recursive(self, layout: dict, path: list[int]):
if layout["type"] == "panel":
panel = layout["panel"]
panel.destroy_ui()
if panel in self.active_panels:
self.active_panels.remove(panel)
else:
for i in range(len(layout["children"]) - 1):
handler_tag = f"{self._path_to_tag(path, f'grip_{i}')}_handler"
if dpg.does_item_exist(handler_tag):
dpg.delete_item(handler_tag)
for i, child in enumerate(layout["children"]):
self._cleanup_ui_recursive(child, path + [i])
def update_all_panels(self):
for panel in self.active_panels:
panel.update()
def on_viewport_resize(self):
self._resize_splits_recursive(self.layout, [])
def _resize_splits_recursive(self, layout: dict, path: list[int], width: int | None = None, height: int | None = None):
if layout["type"] == "split":
split_tag = self._path_to_tag(path, "split")
if dpg.does_item_exist(split_tag):
available_sizes = (width, height) if width and height else dpg.get_item_rect_size(dpg.get_item_parent(split_tag))
orientation, _, pane_sizes = self._get_split_geometry(layout, available_sizes)
size_properties = ("width", "height")
for i, child_layout in enumerate(layout["children"]):
child_path = path + [i]
container_tag = self._path_to_tag(child_path, "container")
if dpg.does_item_exist(container_tag):
dpg.configure_item(container_tag, **{size_properties[orientation]: pane_sizes[i]})
child_width, child_height = [(pane_sizes[i], available_sizes[1]), (available_sizes[0], pane_sizes[i])][orientation]
self._resize_splits_recursive(child_layout, child_path, child_width, child_height)
else: # leaf node/panel - adjust bar height to allow for scrollbar
panel_tag = self._path_to_tag(path, "panel")
if width is not None and width < int(279 * self.scale + 80): # scaled widths of the elements in top bar + fixed 8 padding on left and right of each item
dpg.configure_item(panel_tag, height=(int(13*self.scale) + 24))
else:
dpg.configure_item(panel_tag, height=(int(13*self.scale) + 8))
def _get_split_geometry(self, layout: dict, available_size: tuple[int, int]) -> tuple[int, int, list[int]]:
orientation = layout["orientation"]
num_grips = len(layout["children"]) - 1
usable_size = max(self.min_pane_size, available_size[orientation] - (num_grips * (self.grip_size + 8 * (2-orientation)))) # approximate, scaling is weird
pane_sizes = [max(self.min_pane_size, int(usable_size * prop)) for prop in layout["proportions"]]
return orientation, usable_size, pane_sizes
def _get_layout_at_path(self, path: list[int]) -> dict:
current = self.layout
for index in path:
current = current["children"][index]
return current
def _get_parent_and_index(self, path: list[int]) -> tuple:
return (None, -1) if not path else (self._get_layout_at_path(path[:-1]), path[-1])
def _replace_layout_at_path(self, path: list[int], new_layout: dict):
if not path:
self.layout = new_layout
else:
parent, index = self._get_parent_and_index(path)
parent["children"][index] = new_layout
def _path_to_tag(self, path: list[int], prefix: str = "") -> str:
path_str = "_".join(map(str, path)) if path else "root"
return f"{prefix}_{path_str}" if prefix else path_str
def _create_grip(self, parent_tag: str, path: list[int], grip_index: int, orientation: int):
grip_tag = self._path_to_tag(path, f"grip_{grip_index}")
width, height = [(self.grip_size, -1), (-1, self.grip_size)][orientation]
with dpg.child_window(tag=grip_tag, parent=parent_tag, width=width, height=height, no_scrollbar=True, border=False):
button_tag = dpg.add_button(label="", width=-1, height=-1)
with dpg.item_handler_registry(tag=f"{grip_tag}_handler"):
user_data = (path, grip_index, orientation)
dpg.add_item_active_handler(callback=self._on_grip_drag, user_data=user_data)
dpg.add_item_deactivated_handler(callback=self._on_grip_end, user_data=user_data)
dpg.bind_item_handler_registry(button_tag, f"{grip_tag}_handler")
def _on_grip_drag(self, sender, app_data, user_data):
path, grip_index, orientation = user_data
layout = self._get_layout_at_path(path)
if "_drag_data" not in layout:
layout["_drag_data"] = {"initial_proportions": layout["proportions"][:], "start_mouse": dpg.get_mouse_pos(local=False)[orientation]}
return
drag_data = layout["_drag_data"]
split_tag = self._path_to_tag(path, "split")
if not dpg.does_item_exist(split_tag):
return
_, usable_size, _ = self._get_split_geometry(layout, dpg.get_item_rect_size(split_tag))
current_coord = dpg.get_mouse_pos(local=False)[orientation]
delta = current_coord - drag_data["start_mouse"]
delta_prop = delta / usable_size
left_idx = grip_index
right_idx = left_idx + 1
initial = drag_data["initial_proportions"]
min_prop = self.min_pane_size / usable_size
new_left = max(min_prop, initial[left_idx] + delta_prop)
new_right = max(min_prop, initial[right_idx] - delta_prop)
total_available = initial[left_idx] + initial[right_idx]
if new_left + new_right > total_available:
if new_left > new_right:
new_left = total_available - new_right
else:
new_right = total_available - new_left
layout["proportions"] = initial[:]
layout["proportions"][left_idx] = new_left
layout["proportions"][right_idx] = new_right
self._resize_splits_recursive(layout, path)
def _on_grip_end(self, sender, app_data, user_data):
path, _, _ = user_data
self._get_layout_at_path(path).pop("_drag_data", None)

252
tools/jotpluggler/pluggle.py Executable file
View File

@@ -0,0 +1,252 @@
#!/usr/bin/env python3
import argparse
import os
import pyautogui
import subprocess
import dearpygui.dearpygui as dpg
import multiprocessing
import uuid
import signal
from openpilot.common.basedir import BASEDIR
from openpilot.tools.jotpluggler.data import DataManager
from openpilot.tools.jotpluggler.datatree import DataTree
from openpilot.tools.jotpluggler.layout import PlotLayoutManager
DEMO_ROUTE = "a2a0ccea32023010|2023-07-27--13-01-19"
class WorkerManager:
def __init__(self, max_workers=None):
self.pool = multiprocessing.Pool(max_workers or min(4, multiprocessing.cpu_count()), initializer=WorkerManager.worker_initializer)
self.active_tasks = {}
def submit_task(self, func, args_list, callback=None, task_id=None):
task_id = task_id or str(uuid.uuid4())
if task_id in self.active_tasks:
try:
self.active_tasks[task_id].terminate()
except Exception:
pass
def handle_success(result):
self.active_tasks.pop(task_id, None)
if callback:
try:
callback(result)
except Exception as e:
print(f"Callback for task {task_id} failed: {e}")
def handle_error(error):
self.active_tasks.pop(task_id, None)
print(f"Task {task_id} failed: {error}")
async_result = self.pool.starmap_async(func, args_list, callback=handle_success, error_callback=handle_error)
self.active_tasks[task_id] = async_result
return task_id
@staticmethod
def worker_initializer():
signal.signal(signal.SIGINT, signal.SIG_IGN)
def shutdown(self):
for task in self.active_tasks.values():
try:
task.terminate()
except Exception:
pass
self.pool.terminate()
self.pool.join()
class PlaybackManager:
def __init__(self):
self.is_playing = False
self.current_time_s = 0.0
self.duration_s = 0.0
def set_route_duration(self, duration: float):
self.duration_s = duration
self.seek(min(self.current_time_s, duration))
def toggle_play_pause(self):
if not self.is_playing and self.current_time_s >= self.duration_s:
self.seek(0.0)
self.is_playing = not self.is_playing
texture_tag = "pause_texture" if self.is_playing else "play_texture"
dpg.configure_item("play_pause_button", texture_tag=texture_tag)
def seek(self, time_s: float):
self.current_time_s = max(0.0, min(time_s, self.duration_s))
def update_time(self, delta_t: float):
if self.is_playing:
self.current_time_s = min(self.current_time_s + delta_t, self.duration_s)
if self.current_time_s >= self.duration_s:
self.is_playing = False
dpg.configure_item("play_pause_button", texture_tag="play_texture")
return self.current_time_s
class MainController:
def __init__(self, scale: float = 1.0):
self.scale = scale
self.data_manager = DataManager()
self.playback_manager = PlaybackManager()
self.worker_manager = WorkerManager()
self._create_global_themes()
self.data_tree = DataTree(self.data_manager, self.playback_manager)
self.plot_layout_manager = PlotLayoutManager(self.data_manager, self.playback_manager, self.worker_manager, scale=self.scale)
self.data_manager.add_observer(self.on_data_loaded)
def _create_global_themes(self):
with dpg.theme(tag="global_line_theme"):
with dpg.theme_component(dpg.mvLineSeries):
scaled_thickness = max(1.0, self.scale)
dpg.add_theme_style(dpg.mvPlotStyleVar_LineWeight, scaled_thickness, category=dpg.mvThemeCat_Plots)
with dpg.theme(tag="global_timeline_theme"):
with dpg.theme_component(dpg.mvInfLineSeries):
scaled_thickness = max(1.0, self.scale)
dpg.add_theme_style(dpg.mvPlotStyleVar_LineWeight, scaled_thickness, category=dpg.mvThemeCat_Plots)
dpg.add_theme_color(dpg.mvPlotCol_Line, (255, 0, 0, 128), category=dpg.mvThemeCat_Plots)
def on_data_loaded(self, data: dict):
duration = data.get('duration', 0.0)
self.playback_manager.set_route_duration(duration)
if data.get('reset'):
self.playback_manager.current_time_s = 0.0
self.playback_manager.duration_s = 0.0
self.playback_manager.is_playing = False
dpg.set_value("load_status", "Loading...")
dpg.set_value("timeline_slider", 0.0)
dpg.configure_item("timeline_slider", max_value=0.0)
dpg.configure_item("play_pause_button", texture_tag="play_texture")
dpg.configure_item("load_button", enabled=True)
elif data.get('loading_complete'):
num_paths = len(self.data_manager.get_all_paths())
dpg.set_value("load_status", f"Loaded {num_paths} data paths")
dpg.configure_item("load_button", enabled=True)
elif data.get('segment_added'):
segment_count = data.get('segment_count', 0)
dpg.set_value("load_status", f"Loading... {segment_count} segments processed")
dpg.configure_item("timeline_slider", max_value=duration)
def setup_ui(self):
with dpg.texture_registry():
script_dir = os.path.dirname(os.path.realpath(__file__))
for image in ["play", "pause", "x", "split_h", "split_v"]:
texture = dpg.load_image(os.path.join(script_dir, "assets", f"{image}.png"))
dpg.add_static_texture(width=texture[0], height=texture[1], default_value=texture[3], tag=f"{image}_texture")
with dpg.window(tag="Primary Window"):
with dpg.group(horizontal=True):
# Left panel - Data tree
with dpg.child_window(label="Sidebar", width=300 * self.scale, tag="sidebar_window", border=True, resizable_x=True):
with dpg.group(horizontal=True):
dpg.add_input_text(tag="route_input", width=-75 * self.scale, hint="Enter route name...")
dpg.add_button(label="Load", callback=self.load_route, tag="load_button", width=-1)
dpg.add_text("Ready to load route", tag="load_status")
dpg.add_separator()
self.data_tree.create_ui("sidebar_window")
# Right panel - Plots and timeline
with dpg.group(tag="right_panel"):
with dpg.child_window(label="Plot Window", border=True, height=-(32 + 13 * self.scale), tag="main_plot_area"):
self.plot_layout_manager.create_ui("main_plot_area")
with dpg.child_window(label="Timeline", border=True):
with dpg.table(header_row=False, borders_innerH=False, borders_innerV=False, borders_outerH=False, borders_outerV=False):
btn_size = int(13 * self.scale)
dpg.add_table_column(width_fixed=True, init_width_or_weight=(btn_size + 8)) # Play button
dpg.add_table_column(width_stretch=True) # Timeline slider
dpg.add_table_column(width_fixed=True, init_width_or_weight=int(50 * self.scale)) # FPS counter
with dpg.table_row():
dpg.add_image_button(texture_tag="play_texture", tag="play_pause_button", callback=self.toggle_play_pause, width=btn_size, height=btn_size)
dpg.add_slider_float(tag="timeline_slider", default_value=0.0, label="", width=-1, callback=self.timeline_drag)
dpg.add_text("", tag="fps_counter")
with dpg.item_handler_registry(tag="plot_resize_handler"):
dpg.add_item_resize_handler(callback=self.on_plot_resize)
dpg.bind_item_handler_registry("right_panel", "plot_resize_handler")
dpg.set_primary_window("Primary Window", True)
def on_plot_resize(self, sender, app_data, user_data):
self.plot_layout_manager.on_viewport_resize()
def load_route(self):
route_name = dpg.get_value("route_input").strip()
if route_name:
dpg.set_value("load_status", "Loading route...")
dpg.configure_item("load_button", enabled=False)
self.data_manager.load_route(route_name)
def toggle_play_pause(self, sender):
self.playback_manager.toggle_play_pause()
def timeline_drag(self, sender, app_data):
self.playback_manager.seek(app_data)
def update_frame(self, font):
self.data_tree.update_frame(font)
new_time = self.playback_manager.update_time(dpg.get_delta_time())
if not dpg.is_item_active("timeline_slider"):
dpg.set_value("timeline_slider", new_time)
self.plot_layout_manager.update_all_panels()
dpg.set_value("fps_counter", f"{dpg.get_frame_rate():.1f} FPS")
def shutdown(self):
self.worker_manager.shutdown()
def main(route_to_load=None):
dpg.create_context()
# TODO: find better way of calculating display scaling
try:
w, h = next(tuple(map(int, l.split()[0].split('x'))) for l in subprocess.check_output(['xrandr']).decode().split('\n') if '*' in l) # actual resolution
scale = pyautogui.size()[0] / w # scaled resolution
except Exception:
scale = 1
with dpg.font_registry():
default_font = dpg.add_font(os.path.join(BASEDIR, "selfdrive/assets/fonts/JetBrainsMono-Medium.ttf"), int(13 * scale))
dpg.bind_font(default_font)
viewport_width, viewport_height = int(1200 * scale), int(800 * scale)
mouse_x, mouse_y = pyautogui.position() # TODO: find better way of creating the window where the user is (default dpg behavior annoying on multiple displays)
dpg.create_viewport(
title='JotPluggler', width=viewport_width, height=viewport_height, x_pos=mouse_x - viewport_width // 2, y_pos=mouse_y - viewport_height // 2
)
dpg.setup_dearpygui()
controller = MainController(scale=scale)
controller.setup_ui()
if route_to_load:
dpg.set_value("route_input", route_to_load)
controller.load_route()
dpg.show_viewport()
# Main loop
try:
while dpg.is_dearpygui_running():
controller.update_frame(default_font)
dpg.render_dearpygui_frame()
finally:
controller.shutdown()
dpg.destroy_context()
if __name__ == "__main__":
parser = argparse.ArgumentParser(description="A tool for visualizing openpilot logs.")
parser.add_argument("--demo", action="store_true", help="Use the demo route instead of providing one")
parser.add_argument("route", nargs='?', default=None, help="Optional route name to load on startup.")
args = parser.parse_args()
route = DEMO_ROUTE if args.demo else args.route
main(route_to_load=route)

195
tools/jotpluggler/views.py Normal file
View File

@@ -0,0 +1,195 @@
import uuid
import threading
import numpy as np
from collections import deque
import dearpygui.dearpygui as dpg
from abc import ABC, abstractmethod
class ViewPanel(ABC):
"""Abstract base class for all view panels that can be displayed in a plot container"""
def __init__(self, panel_id: str = None):
self.panel_id = panel_id or str(uuid.uuid4())
self.title = "Untitled Panel"
@abstractmethod
def clear(self):
pass
@abstractmethod
def create_ui(self, parent_tag: str):
pass
@abstractmethod
def destroy_ui(self):
pass
@abstractmethod
def get_panel_type(self) -> str:
pass
@abstractmethod
def update(self):
pass
class TimeSeriesPanel(ViewPanel):
def __init__(self, data_manager, playback_manager, worker_manager, panel_id: str | None = None):
super().__init__(panel_id)
self.data_manager = data_manager
self.playback_manager = playback_manager
self.worker_manager = worker_manager
self.title = "Time Series Plot"
self.plot_tag = f"plot_{self.panel_id}"
self.x_axis_tag = f"{self.plot_tag}_x_axis"
self.y_axis_tag = f"{self.plot_tag}_y_axis"
self.timeline_indicator_tag = f"{self.plot_tag}_timeline"
self._ui_created = False
self._series_data: dict[str, tuple[np.ndarray, np.ndarray]] = {}
self._last_plot_duration = 0
self._update_lock = threading.RLock()
self._results_deque: deque[tuple[str, list, list]] = deque()
self._new_data = False
def create_ui(self, parent_tag: str):
self.data_manager.add_observer(self.on_data_loaded)
with dpg.plot(height=-1, width=-1, tag=self.plot_tag, parent=parent_tag, drop_callback=self._on_series_drop, payload_type="TIMESERIES_PAYLOAD"):
dpg.add_plot_legend()
dpg.add_plot_axis(dpg.mvXAxis, no_label=True, tag=self.x_axis_tag)
dpg.add_plot_axis(dpg.mvYAxis, no_label=True, tag=self.y_axis_tag)
timeline_series_tag = dpg.add_inf_line_series(x=[0], label="Timeline", parent=self.y_axis_tag, tag=self.timeline_indicator_tag)
dpg.bind_item_theme(timeline_series_tag, "global_timeline_theme")
for series_path in list(self._series_data.keys()):
self.add_series(series_path)
self._ui_created = True
def update(self):
with self._update_lock:
if not self._ui_created:
return
if self._new_data: # handle new data in main thread
self._new_data = False
for series_path in list(self._series_data.keys()):
self.add_series(series_path, update=True)
while self._results_deque: # handle downsampled results in main thread
results = self._results_deque.popleft()
for series_path, downsampled_time, downsampled_values in results:
series_tag = f"series_{self.panel_id}_{series_path}"
if dpg.does_item_exist(series_tag):
dpg.set_value(series_tag, (downsampled_time, downsampled_values.astype(float)))
# update timeline
current_time_s = self.playback_manager.current_time_s
dpg.set_value(self.timeline_indicator_tag, [[current_time_s], [0]])
# update timeseries legend label
for series_path, (time_array, value_array) in self._series_data.items():
position = np.searchsorted(time_array, current_time_s, side='right') - 1
if position >= 0 and (current_time_s - time_array[position]) <= 1.0:
value = value_array[position]
formatted_value = f"{value:.5f}" if np.issubdtype(type(value), np.floating) else str(value)
series_tag = f"series_{self.panel_id}_{series_path}"
if dpg.does_item_exist(series_tag):
dpg.configure_item(series_tag, label=f"{series_path}: {formatted_value}")
# downsample if plot zoom changed significantly
plot_duration = dpg.get_axis_limits(self.x_axis_tag)[1] - dpg.get_axis_limits(self.x_axis_tag)[0]
if plot_duration > self._last_plot_duration * 2 or plot_duration < self._last_plot_duration * 0.5:
self._downsample_all_series(plot_duration)
def _downsample_all_series(self, plot_duration):
plot_width = dpg.get_item_rect_size(self.plot_tag)[0]
if plot_width <= 0 or plot_duration <= 0:
return
self._last_plot_duration = plot_duration
target_points_per_second = plot_width / plot_duration
work_items = []
for series_path, (time_array, value_array) in self._series_data.items():
if len(time_array) == 0:
continue
series_duration = time_array[-1] - time_array[0] if len(time_array) > 1 else 1
points_per_second = len(time_array) / series_duration
if points_per_second > target_points_per_second * 2:
target_points = max(int(target_points_per_second * series_duration), plot_width)
work_items.append((series_path, time_array, value_array, target_points))
elif dpg.does_item_exist(f"series_{self.panel_id}_{series_path}"):
dpg.set_value(f"series_{self.panel_id}_{series_path}", (time_array, value_array.astype(float)))
if work_items:
self.worker_manager.submit_task(
TimeSeriesPanel._downsample_worker, work_items, callback=lambda results: self._results_deque.append(results), task_id=f"downsample_{self.panel_id}"
)
def add_series(self, series_path: str, update: bool = False):
with self._update_lock:
if update or series_path not in self._series_data:
self._series_data[series_path] = self.data_manager.get_timeseries(series_path)
time_array, value_array = self._series_data[series_path]
series_tag = f"series_{self.panel_id}_{series_path}"
if dpg.does_item_exist(series_tag):
dpg.set_value(series_tag, (time_array, value_array.astype(float)))
else:
line_series_tag = dpg.add_line_series(x=time_array, y=value_array.astype(float), label=series_path, parent=self.y_axis_tag, tag=series_tag)
dpg.bind_item_theme(line_series_tag, "global_line_theme")
dpg.fit_axis_data(self.x_axis_tag)
dpg.fit_axis_data(self.y_axis_tag)
plot_duration = dpg.get_axis_limits(self.x_axis_tag)[1] - dpg.get_axis_limits(self.x_axis_tag)[0]
self._downsample_all_series(plot_duration)
def destroy_ui(self):
with self._update_lock:
self.data_manager.remove_observer(self.on_data_loaded)
if dpg.does_item_exist(self.plot_tag):
dpg.delete_item(self.plot_tag)
self._ui_created = False
def get_panel_type(self) -> str:
return "timeseries"
def clear(self):
with self._update_lock:
for series_path in list(self._series_data.keys()):
self.remove_series(series_path)
def remove_series(self, series_path: str):
with self._update_lock:
if series_path in self._series_data:
if dpg.does_item_exist(f"series_{self.panel_id}_{series_path}"):
dpg.delete_item(f"series_{self.panel_id}_{series_path}")
del self._series_data[series_path]
def on_data_loaded(self, data: dict):
self._new_data = True
def _on_series_drop(self, sender, app_data, user_data):
self.add_series(app_data)
@staticmethod
def _downsample_worker(series_path, time_array, value_array, target_points):
if len(time_array) <= target_points:
return series_path, time_array, value_array
step = len(time_array) / target_points
indices = []
for i in range(target_points):
start_idx = int(i * step)
end_idx = int((i + 1) * step)
if start_idx == end_idx:
indices.append(start_idx)
else:
bucket_values = value_array[start_idx:end_idx]
min_idx = start_idx + np.argmin(bucket_values)
max_idx = start_idx + np.argmax(bucket_values)
if min_idx != max_idx:
indices.extend([min(min_idx, max_idx), max(min_idx, max_idx)])
else:
indices.append(min_idx)
indices = sorted(set(indices))
return series_path, time_array[indices], value_array[indices]

View File

@@ -365,6 +365,7 @@ function op_switch() {
fi
BRANCH="$1"
git config --replace-all remote.origin.fetch "+refs/heads/*:refs/remotes/origin/*"
git fetch "$REMOTE" "$BRANCH"
git checkout -f FETCH_HEAD
git checkout -B "$BRANCH" --track "$REMOTE"/"$BRANCH"

View File

@@ -1,77 +1,45 @@
<?xml version='1.0' encoding='UTF-8'?>
<root>
<tabbed_widget parent="main_window" name="Main Window">
<tabbed_widget name="Main Window" parent="main_window">
<Tab containers="1" tab_name="Lateral Plan Conformance">
<Container>
<DockSplitter count="4" sizes="0.25;0.25;0.25;0.25" orientation="-">
<DockSplitter orientation="-" count="4" sizes="0.250949;0.249051;0.250949;0.249051">
<DockArea name="desired vs actual lateral acceleration (closer means better conformance to plan)">
<plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
<range top="3.586831" right="269.643117" left="0.000140" bottom="-2.354077"/>
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="1.858161" bottom="-1.823407" right="1138.891674" left="0.000194"/>
<limitY/>
<curve color="#1f77b4" name="/controlsState/lateralControlState/torqueState/actualLateralAccel"/>
<curve color="#d62728" name="/controlsState/lateralControlState/torqueState/desiredLateralAccel"/>
<curve name="/controlsState/lateralControlState/torqueState/actualLateralAccel" color="#1f77b4"/>
<curve name="/controlsState/lateralControlState/torqueState/desiredLateralAccel" color="#d62728"/>
</plot>
</DockArea>
<DockArea name="desired vs actual lateral acceleration, road-roll factored out (closer means better conformance to plan)">
<plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
<range top="3.445087" right="269.643117" left="0.000140" bottom="-2.654874"/>
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="2.749816" bottom="-3.723091" right="1138.891674" left="0.000194"/>
<limitY/>
<curve color="#1ac938" name="Actual lateral accel (roll compensated)"/>
<curve color="#ff7f0e" name="Desired lateral accel (roll compensated)"/>
<curve name="Actual lateral accel (roll compensated)" color="#1ac938"/>
<curve name="Desired lateral accel (roll compensated)" color="#ff7f0e"/>
</plot>
</DockArea>
<DockArea name="controller feed-forward vs actuator output (closer means controller prediction is more accurate)">
<plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
<range top="1.082031" right="269.643117" left="0.000140" bottom="-1.050781"/>
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="1.978032" bottom="-1.570956" right="1138.891674" left="0.000194"/>
<limitY/>
<curve color="#9467bd" name="/carOutput/actuatorsOutput/torque">
<curve name="/carOutput/actuatorsOutput/torque" color="#9467bd">
<transform alias="/carOutput/actuatorsOutput/torque[Scale/Offset]" name="Scale/Offset">
<options value_scale="-1" time_offset="0" value_offset="0"/>
<options value_offset="0" value_scale="-1" time_offset="0"/>
</transform>
</curve>
<curve color="#1f77b4" name="/controlsState/lateralControlState/torqueState/f"/>
<curve color="#ff000f" name="/carState/steeringPressed"/>
<curve name="/controlsState/lateralControlState/torqueState/f" color="#1f77b4"/>
<curve name="/carState/steeringPressed" color="#ff000f"/>
</plot>
</DockArea>
<DockArea name="vehicle speed">
<plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
<range top="101.506277" right="269.643117" left="0.000140" bottom="-2.475763"/>
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="105.981304" bottom="-2.709314" right="1138.891674" left="0.000194"/>
<limitY/>
<curve color="#d62728" name="carState.vEgo mph"/>
<curve color="#1ac938" name="carState.vEgo kmh"/>
<curve color="#ff7f0e" name="/carState/vEgo"/>
</plot>
</DockArea>
</DockSplitter>
</Container>
</Tab>
<Tab containers="1" tab_name="Actuator Performance">
<Container>
<DockSplitter count="3" sizes="0.33361;0.33278;0.33361" orientation="-">
<DockArea name="offline-calculated vs online-learned lateral accel scaling factor, accel obtained from 100% actuator output">
<plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
<range top="4.186453" right="269.490213" left="0.000000" bottom="3.175940"/>
<limitY/>
<curve color="#1f77b4" name="/liveTorqueParameters/latAccelFactorFiltered"/>
<curve color="#d62728" name="/liveTorqueParameters/latAccelFactorRaw"/>
<curve color="#1c9222" name="/carParams/lateralTuning/torque/latAccelFactor"/>
</plot>
</DockArea>
<DockArea name="learned lateral accel offset, vehicle-specific compensation to obtain true zero lateral accel">
<plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
<range top="0.003035" right="269.490213" left="0.000000" bottom="-0.124417"/>
<limitY/>
<curve color="#1ac938" name="/liveTorqueParameters/latAccelOffsetFiltered"/>
<curve color="#ff7f0e" name="/liveTorqueParameters/latAccelOffsetRaw"/>
</plot>
</DockArea>
<DockArea name="offline-calculated vs online-learned EPS friction factor, necessary to start moving the steering wheel">
<plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
<range top="0.121143" right="269.490213" left="0.000000" bottom="-0.002955"/>
<limitY/>
<curve color="#f14cc1" name="/liveTorqueParameters/frictionCoefficientFiltered"/>
<curve color="#9467bd" name="/liveTorqueParameters/frictionCoefficientRaw"/>
<curve color="#1c9222" name="/carParams/lateralTuning/torque/friction"/>
<curve name="carState.vEgo mph" color="#d62728"/>
<curve name="carState.vEgo kmh" color="#1ac938"/>
<curve name="/carState/vEgo" color="#ff7f0e"/>
</plot>
</DockArea>
</DockSplitter>
@@ -79,115 +47,134 @@
</Tab>
<Tab containers="1" tab_name="Vehicle Dynamics">
<Container>
<DockSplitter count="3" sizes="0.33361;0.33278;0.33361" orientation="-">
<DockSplitter orientation="-" count="3" sizes="0.334282;0.331437;0.334282">
<DockArea name="configured-initial vs online-learned steerRatio, set configured value to match learned">
<plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
<range top="12.903705" right="269.638801" left="0.000000" bottom="12.748092"/>
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="19.665784" bottom="19.359553" right="1138.816328" left="0.000000"/>
<limitY/>
<curve color="#1f77b4" name="/carParams/steerRatio"/>
<curve color="#1ac938" name="/liveParameters/steerRatio"/>
<curve name="/carParams/steerRatio" color="#1f77b4"/>
<curve name="/liveParameters/steerRatio" color="#1ac938"/>
</plot>
</DockArea>
<DockArea name="configured-initial vs online-learned tireStiffnessRatio, set configured value to match learned">
<plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
<range top="1.000520" right="269.638801" left="0.000000" bottom="0.999718"/>
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="1.112210" bottom="0.995631" right="1138.816328" left="0.000000"/>
<limitY/>
<curve color="#d62728" name="/carParams/tireStiffnessFactor"/>
<curve color="#ff7f0e" name="/liveParameters/stiffnessFactor"/>
<curve name="/carParams/tireStiffnessFactor" color="#d62728"/>
<curve name="/liveParameters/stiffnessFactor" color="#ff7f0e"/>
</plot>
</DockArea>
<DockArea name="live steering angle offsets for straight-ahead driving, large values here may indicate alignment problems">
<plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
<range top="-0.332067" right="269.638801" left="0.000000" bottom="-3.149970"/>
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="-1.081041" bottom="-4.494133" right="1138.816328" left="0.000000"/>
<limitY/>
<curve color="#f14cc1" name="/liveParameters/angleOffsetAverageDeg"/>
<curve color="#9467bd" name="/liveParameters/angleOffsetDeg"/>
<curve name="/liveParameters/angleOffsetAverageDeg" color="#f14cc1"/>
<curve name="/liveParameters/angleOffsetDeg" color="#9467bd"/>
</plot>
</DockArea>
</DockSplitter>
</Container>
</Tab>
<Tab containers="1" tab_name="Controller PIF Terms">
<Tab containers="1" tab_name="Actuator Performance">
<Container>
<DockSplitter count="3" sizes="0.33361;0.33278;0.33361" orientation="-">
<DockArea name="controller feed-forward vs actuator output (closer means controller prediction is more accurate)">
<plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
<range top="1.082031" right="269.643117" left="0.000140" bottom="-1.050781"/>
<DockSplitter orientation="-" count="3" sizes="0.333333;0.333333;0.333333">
<DockArea name="offline-calculated vs online-learned lateral accel scaling factor, accel obtained from 100% actuator output">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="1.216110" bottom="0.539474" right="1138.920072" left="0.000000"/>
<limitY/>
<curve color="#9467bd" name="/carOutput/actuatorsOutput/torque">
<curve name="/liveTorqueParameters/latAccelFactorFiltered" color="#1f77b4"/>
<curve name="/liveTorqueParameters/latAccelFactorRaw" color="#d62728"/>
<curve name="/carParams/lateralTuning/torque/latAccelFactor" color="#1c9222"/>
</plot>
</DockArea>
<DockArea name="learned lateral accel offset, vehicle-specific compensation to obtain true zero lateral accel">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="-0.304367" bottom="-0.418688" right="1138.920072" left="0.000000"/>
<limitY/>
<curve name="/liveTorqueParameters/latAccelOffsetFiltered" color="#1ac938"/>
<curve name="/liveTorqueParameters/latAccelOffsetRaw" color="#ff7f0e"/>
</plot>
</DockArea>
<DockArea name="offline-calculated vs online-learned EPS friction factor, necessary to start moving the steering wheel">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="0.226389" bottom="0.158050" right="1138.920072" left="0.000000"/>
<limitY/>
<curve name="/liveTorqueParameters/frictionCoefficientFiltered" color="#f14cc1"/>
<curve name="/liveTorqueParameters/frictionCoefficientRaw" color="#9467bd"/>
<curve name="/carParams/lateralTuning/torque/friction" color="#1c9222"/>
</plot>
</DockArea>
</DockSplitter>
</Container>
</Tab>
<Tab containers="1" tab_name="Actuator Delay">
<Container>
<DockSplitter orientation="-" count="3" sizes="0.30441;0.358464;0.337127">
<DockArea name="actuator lag learning state, 0 = learning, 1 = learned/applying, 2 = invalid">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="1.025000" bottom="-0.025000" right="1138.749979" left="0.000000"/>
<limitY/>
<curve name="/liveDelay/status" color="#ff7f0e"/>
</plot>
</DockArea>
<DockArea name="offline default vs online estimated steering actuator lag">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="0.419648" bottom="0.318362" right="1138.749979" left="0.000000"/>
<limitY/>
<curve name="/liveDelay/lateralDelay" color="#1f77b4"/>
<curve name="/liveDelay/lateralDelayEstimate" color="#d62728"/>
<curve name="opendbc default steering lag" color="#1ac938"/>
</plot>
</DockArea>
<DockArea name="online estimated steering actuator lag, standard deviation">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="0.067320" bottom="-0.001642" right="1138.749979" left="0.000000"/>
<limitY/>
<curve name="/liveDelay/lateralDelayEstimateStd" color="#f14cc1"/>
</plot>
</DockArea>
</DockSplitter>
</Container>
</Tab>
<Tab containers="1" tab_name="Controls Performance">
<Container>
<DockSplitter orientation="-" count="4" sizes="0.265655;0.251898;0.245731;0.236717">
<DockArea name="rate-of-change limits on steering actuator (blue = original, green = rate-limited before CAN output)">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="1.050000" bottom="-1.050000" right="1138.891921" left="0.000194"/>
<limitY/>
<curve name="/carControl/actuators/torque" color="#0c00f2"/>
<curve name="/carOutput/actuatorsOutput/torque" color="#2cd63a"/>
</plot>
</DockArea>
<DockArea name="controller feed-forward vs actuator output (closer means controller prediction is more accurate)">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="1.978032" bottom="-1.570956" right="1138.891921" left="0.000194"/>
<limitY/>
<curve name="/carOutput/actuatorsOutput/torque" color="#9467bd">
<transform alias="/carOutput/actuatorsOutput/torque[Scale/Offset]" name="Scale/Offset">
<options value_scale="-1.0" time_offset="0" value_offset="0"/>
<options value_offset="0" value_scale="-1.0" time_offset="0"/>
</transform>
</curve>
<curve color="#1f77b4" name="/controlsState/lateralControlState/torqueState/f"/>
<curve color="#ff000f" name="/carState/steeringPressed"/>
<curve name="/controlsState/lateralControlState/torqueState/f" color="#1f77b4"/>
<curve name="/carState/steeringPressed" color="#ff000f"/>
</plot>
</DockArea>
<DockArea name="proportional, integral, and feed-forward terms (actuator output = sum of PIF terms)">
<plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
<range top="1.572946" right="269.643117" left="0.000140" bottom="-3.822608"/>
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="2.099784" bottom="-4.027542" right="1138.891921" left="0.000194"/>
<limitY/>
<curve color="#0ab027" name="/controlsState/lateralControlState/torqueState/f"/>
<curve color="#d62728" name="/controlsState/lateralControlState/torqueState/p"/>
<curve color="#ffaf00" name="/controlsState/lateralControlState/torqueState/i"/>
<curve color="#756a6a" name="Zero"/>
<curve name="/controlsState/lateralControlState/torqueState/f" color="#0ab027"/>
<curve name="/controlsState/lateralControlState/torqueState/p" color="#d62728"/>
<curve name="/controlsState/lateralControlState/torqueState/i" color="#ffaf00"/>
<curve name="Zero" color="#756a6a"/>
</plot>
</DockArea>
<DockArea name="road roll angle, from openpilot localizer">
<plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
<range top="0.059127" right="269.643117" left="0.000140" bottom="-0.031841"/>
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="0.109446" bottom="-0.045525" right="1138.891921" left="0.000194"/>
<limitY/>
<curve color="#f14cc1" name="/liveParameters/roll"/>
</plot>
</DockArea>
</DockSplitter>
</Container>
</Tab>
<Tab containers="1" tab_name="Actuator Delay Estimation">
<Container>
<DockSplitter count="4" sizes="0.25;0.25;0.25;0.25" orientation="-">
<DockArea name="desired vs actual lateral acceleration (baseline)">
<plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
<range top="3.586831" right="269.943117" left="0.134774" bottom="-2.354077"/>
<limitY/>
<curve color="#ff7f0e" name="/controlsState/lateralControlState/torqueState/desiredLateralAccel"/>
<curve color="#1ac938" name="/controlsState/lateralControlState/torqueState/actualLateralAccel"/>
</plot>
</DockArea>
<DockArea name="desired vs actual lateral acceleration (desired shifted by +0.1s)">
<plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
<range top="3.586831" right="269.943117" left="0.134774" bottom="-2.354077"/>
<limitY/>
<curve color="#ff7f0e" name="/controlsState/lateralControlState/torqueState/desiredLateralAccel">
<transform alias="/controlsState/lateralControlState/torqueState/desiredLateralAccel[Scale/Offset]" name="Scale/Offset">
<options value_scale="1.0" time_offset="0.1" value_offset="0"/>
</transform>
</curve>
<curve color="#1ac938" name="/controlsState/lateralControlState/torqueState/actualLateralAccel"/>
</plot>
</DockArea>
<DockArea name="desired vs actual lateral acceleration (desired shifted by +0.2s)">
<plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
<range top="3.586831" right="269.943117" left="0.134774" bottom="-2.354077"/>
<limitY/>
<curve color="#ff7f0e" name="/controlsState/lateralControlState/torqueState/desiredLateralAccel">
<transform alias="/controlsState/lateralControlState/torqueState/desiredLateralAccel[Scale/Offset]" name="Scale/Offset">
<options value_scale="1.0" time_offset="0.2" value_offset="0"/>
</transform>
</curve>
<curve color="#1ac938" name="/controlsState/lateralControlState/torqueState/actualLateralAccel"/>
</plot>
</DockArea>
<DockArea name="desired vs actual lateral acceleration (desired shifted by +0.3s)">
<plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
<range top="3.586831" right="269.943117" left="0.134774" bottom="-2.354077"/>
<limitY/>
<curve color="#ff7f0e" name="/controlsState/lateralControlState/torqueState/desiredLateralAccel">
<transform alias="/controlsState/lateralControlState/torqueState/desiredLateralAccel[Scale/Offset]" name="Scale/Offset">
<options value_scale="1.0" time_offset="0.3" value_offset="0"/>
</transform>
</curve>
<curve color="#1ac938" name="/controlsState/lateralControlState/torqueState/actualLateralAccel"/>
<curve name="/liveParameters/roll" color="#f14cc1"/>
</plot>
</DockArea>
</DockSplitter>
@@ -199,25 +186,34 @@
<!-- - - - - - - - - - - - - - - -->
<!-- - - - - - - - - - - - - - - -->
<Plugins>
<plugin ID="DataLoad CSV">
<default delimiter="0" time_axis=""/>
</plugin>
<plugin ID="DataLoad Rlog"/>
<plugin ID="DataLoad ULog"/>
<plugin ID="Cereal Subscriber"/>
<plugin ID="UDP Server"/>
<plugin ID="ZMQ Subscriber"/>
<plugin ID="Fast Fourier Transform"/>
<plugin ID="Quaternion to RPY"/>
<plugin ID="Reactive Script Editor">
<library code="--[[ Helper function to create a series from arrays&#xa;&#xa; new_series: a series previously created with ScatterXY.new(name)&#xa; prefix: prefix of the timeseries, before the index of the array&#xa; suffix_X: suffix to complete the name of the series containing the X value. If [nil], use the index of the array.&#xa; suffix_Y: suffix to complete the name of the series containing the Y value&#xa; timestamp: usually the tracker_time variable&#xa; &#xa; Example:&#xa; &#xa; Assuming we have multiple series in the form:&#xa; &#xa; /trajectory/node.{X}/position/x&#xa; /trajectory/node.{X}/position/y&#xa; &#xa; where {N} is the index of the array (integer). We can create a reactive series from the array with:&#xa; &#xa; new_series = ScatterXY.new(&quot;my_trajectory&quot;) &#xa; CreateSeriesFromArray( new_series, &quot;/trajectory/node&quot;, &quot;position/x&quot;, &quot;position/y&quot;, tracker_time );&#xa;--]]&#xa;&#xa;function CreateSeriesFromArray( new_series, prefix, suffix_X, suffix_Y, timestamp )&#xa; &#xa; --- clear previous values&#xa; new_series:clear()&#xa; &#xa; --- Append points to new_series&#xa; index = 0&#xa; while(true) do&#xa;&#xa; x = index;&#xa; -- if not nil, get the X coordinate from a series&#xa; if suffix_X ~= nil then &#xa; series_x = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_X) )&#xa; if series_x == nil then break end&#xa; x = series_x:atTime(timestamp)&#x9; &#xa; end&#xa; &#xa; series_y = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_Y) )&#xa; if series_y == nil then break end &#xa; y = series_y:atTime(timestamp)&#xa; &#xa; new_series:push_back(x,y)&#xa; index = index+1&#xa; end&#xa;end&#xa;&#xa;--[[ Similar to the built-in function GetSeriesNames(), but select only the names with a give prefix. --]]&#xa;&#xa;function GetSeriesNamesByPrefix(prefix)&#xa; -- GetSeriesNames(9 is a built-in function&#xa; all_names = GetSeriesNames()&#xa; filtered_names = {}&#xa; for i, name in ipairs(all_names) do&#xa; -- check the prefix&#xa; if name:find(prefix, 1, #prefix) then&#xa; table.insert(filtered_names, name);&#xa; end&#xa; end&#xa; return filtered_names&#xa;end&#xa;&#xa;--[[ Modify an existing series, applying offsets to all their X and Y values&#xa;&#xa; series: an existing timeseries, obtained with TimeseriesView.find(name)&#xa; delta_x: offset to apply to each x value&#xa; delta_y: offset to apply to each y value &#xa; &#xa;--]]&#xa;&#xa;function ApplyOffsetInPlace(series, delta_x, delta_y)&#xa; -- use C++ indeces, not Lua indeces&#xa; for index=0, series:size()-1 do&#xa; x,y = series:at(index)&#xa; series:set(index, x + delta_x, y + delta_y)&#xa; end&#xa;end&#xa;"/>
<scripts/>
</plugin>
<plugin ID="CSV Exporter"/>
</Plugins>
<!-- - - - - - - - - - - - - - - -->
<!-- - - - - - - - - - - - - - - -->
<customMathEquations>
<snippet name="Zero">
<snippet name="carState.vEgo kmh">
<global></global>
<function>return (0)</function>
<linked_source>/carState/canValid</linked_source>
<function>return value * 3.6</function>
<linked_source>/carState/vEgo</linked_source>
</snippet>
<snippet name="Actual lateral accel (roll compensated)">
<snippet name="carState.vEgo mph">
<global></global>
<function>return (value * v1 ^ 2) - (v2 * 9.81)</function>
<linked_source>/controlsState/curvature</linked_source>
<additional_sources>
<v1>/carState/vEgo</v1>
<v2>/liveParameters/roll</v2>
</additional_sources>
<function>return value * 2.23694</function>
<linked_source>/carState/vEgo</linked_source>
</snippet>
<snippet name="Desired lateral accel (roll compensated)">
<global></global>
@@ -228,15 +224,24 @@
<v2>/liveParameters/roll</v2>
</additional_sources>
</snippet>
<snippet name="carState.vEgo mph">
<snippet name="Actual lateral accel (roll compensated)">
<global></global>
<function>return value * 2.23694</function>
<linked_source>/carState/vEgo</linked_source>
<function>return (value * v1 ^ 2) - (v2 * 9.81)</function>
<linked_source>/controlsState/curvature</linked_source>
<additional_sources>
<v1>/carState/vEgo</v1>
<v2>/liveParameters/roll</v2>
</additional_sources>
</snippet>
<snippet name="carState.vEgo kmh">
<snippet name="opendbc default steering lag">
<global></global>
<function>return value * 3.6</function>
<linked_source>/carState/vEgo</linked_source>
<function>return value + 0.2</function>
<linked_source>/carParams/steerActuatorDelay</linked_source>
</snippet>
<snippet name="Zero">
<global></global>
<function>return (0)</function>
<linked_source>/carState/canValid</linked_source>
</snippet>
</customMathEquations>
<snippets/>

View File

@@ -13,6 +13,8 @@ else:
replay_lib_src = ["replay.cc", "consoleui.cc", "camera.cc", "filereader.cc", "logreader.cc", "framereader.cc",
"route.cc", "util.cc", "seg_mgr.cc", "timeline.cc", "api.cc"]
if arch != "Darwin":
replay_lib_src.append("qcom_decoder.cc")
replay_lib = replay_env.Library("replay", replay_lib_src, LIBS=base_libs, FRAMEWORKS=base_frameworks)
Export('replay_lib')
replay_libs = [replay_lib, 'avutil', 'avcodec', 'avformat', 'bz2', 'zstd', 'curl', 'yuv', 'ncurses'] + base_libs

View File

@@ -8,6 +8,7 @@
#include "common/util.h"
#include "third_party/libyuv/include/libyuv.h"
#include "tools/replay/util.h"
#include "system/hardware/hw.h"
#ifdef __APPLE__
#define HW_DEVICE_TYPE AV_HWDEVICE_TYPE_VIDEOTOOLBOX
@@ -37,7 +38,16 @@ struct DecoderManager {
return it->second.get();
}
auto decoder = std::make_unique<VideoDecoder>();
std::unique_ptr<VideoDecoder> decoder;
#ifndef __APPLE__
if (Hardware::TICI() && hw_decoder) {
decoder = std::make_unique<QcomVideoDecoder>();
} else
#endif
{
decoder = std::make_unique<FFmpegVideoDecoder>();
}
if (!decoder->open(codecpar, hw_decoder)) {
decoder.reset(nullptr);
}
@@ -114,19 +124,19 @@ bool FrameReader::get(int idx, VisionBuf *buf) {
// class VideoDecoder
VideoDecoder::VideoDecoder() {
FFmpegVideoDecoder::FFmpegVideoDecoder() {
av_frame_ = av_frame_alloc();
hw_frame_ = av_frame_alloc();
}
VideoDecoder::~VideoDecoder() {
FFmpegVideoDecoder::~FFmpegVideoDecoder() {
if (hw_device_ctx) av_buffer_unref(&hw_device_ctx);
if (decoder_ctx) avcodec_free_context(&decoder_ctx);
av_frame_free(&av_frame_);
av_frame_free(&hw_frame_);
}
bool VideoDecoder::open(AVCodecParameters *codecpar, bool hw_decoder) {
bool FFmpegVideoDecoder::open(AVCodecParameters *codecpar, bool hw_decoder) {
const AVCodec *decoder = avcodec_find_decoder(codecpar->codec_id);
if (!decoder) return false;
@@ -149,7 +159,7 @@ bool VideoDecoder::open(AVCodecParameters *codecpar, bool hw_decoder) {
return true;
}
bool VideoDecoder::initHardwareDecoder(AVHWDeviceType hw_device_type) {
bool FFmpegVideoDecoder::initHardwareDecoder(AVHWDeviceType hw_device_type) {
const AVCodecHWConfig *config = nullptr;
for (int i = 0; (config = avcodec_get_hw_config(decoder_ctx->codec, i)) != nullptr; i++) {
if (config->methods & AV_CODEC_HW_CONFIG_METHOD_HW_DEVICE_CTX && config->device_type == hw_device_type) {
@@ -175,7 +185,7 @@ bool VideoDecoder::initHardwareDecoder(AVHWDeviceType hw_device_type) {
return true;
}
bool VideoDecoder::decode(FrameReader *reader, int idx, VisionBuf *buf) {
bool FFmpegVideoDecoder::decode(FrameReader *reader, int idx, VisionBuf *buf) {
int current_idx = idx;
if (idx != reader->prev_idx + 1) {
// seeking to the nearest key frame
@@ -219,7 +229,7 @@ bool VideoDecoder::decode(FrameReader *reader, int idx, VisionBuf *buf) {
return false;
}
AVFrame *VideoDecoder::decodeFrame(AVPacket *pkt) {
AVFrame *FFmpegVideoDecoder::decodeFrame(AVPacket *pkt) {
int ret = avcodec_send_packet(decoder_ctx, pkt);
if (ret < 0) {
rError("Error sending a packet for decoding: %d", ret);
@@ -239,7 +249,7 @@ AVFrame *VideoDecoder::decodeFrame(AVPacket *pkt) {
return (av_frame_->format == hw_pix_fmt) ? hw_frame_ : av_frame_;
}
bool VideoDecoder::copyBuffer(AVFrame *f, VisionBuf *buf) {
bool FFmpegVideoDecoder::copyBuffer(AVFrame *f, VisionBuf *buf) {
if (hw_pix_fmt == HW_PIX_FMT) {
for (int i = 0; i < height/2; i++) {
memcpy(buf->y + (i*2 + 0)*buf->stride, f->data[0] + (i*2 + 0)*f->linesize[0], width);
@@ -256,3 +266,47 @@ bool VideoDecoder::copyBuffer(AVFrame *f, VisionBuf *buf) {
}
return true;
}
#ifndef __APPLE__
bool QcomVideoDecoder::open(AVCodecParameters *codecpar, bool hw_decoder) {
if (codecpar->codec_id != AV_CODEC_ID_HEVC) {
rError("Hardware decoder only supports HEVC codec");
return false;
}
width = codecpar->width;
height = codecpar->height;
msm_vidc.init(VIDEO_DEVICE, width, height, V4L2_PIX_FMT_HEVC);
return true;
}
bool QcomVideoDecoder::decode(FrameReader *reader, int idx, VisionBuf *buf) {
int from_idx = idx;
if (idx != reader->prev_idx + 1) {
// seeking to the nearest key frame
for (int i = idx; i >= 0; --i) {
if (reader->packets_info[i].flags & AV_PKT_FLAG_KEY) {
from_idx = i;
break;
}
}
auto pos = reader->packets_info[from_idx].pos;
int ret = avformat_seek_file(reader->input_ctx, 0, pos, pos, pos, AVSEEK_FLAG_BYTE);
if (ret < 0) {
rError("Failed to seek to byte position %lld: %d", pos, AVERROR(ret));
return false;
}
}
reader->prev_idx = idx;
bool result = false;
AVPacket pkt;
msm_vidc.avctx = reader->input_ctx;
for (int i = from_idx; i <= idx; ++i) {
if (av_read_frame(reader->input_ctx, &pkt) == 0) {
result = msm_vidc.decodeFrame(&pkt, buf) && (i == idx);
av_packet_unref(&pkt);
}
}
return result;
}
#endif

View File

@@ -7,6 +7,10 @@
#include "tools/replay/filereader.h"
#include "tools/replay/util.h"
#ifndef __APPLE__
#include "tools/replay/qcom_decoder.h"
#endif
extern "C" {
#include <libavcodec/avcodec.h>
#include <libavformat/avformat.h>
@@ -40,11 +44,18 @@ public:
class VideoDecoder {
public:
VideoDecoder();
~VideoDecoder();
bool open(AVCodecParameters *codecpar, bool hw_decoder);
bool decode(FrameReader *reader, int idx, VisionBuf *buf);
virtual ~VideoDecoder() = default;
virtual bool open(AVCodecParameters *codecpar, bool hw_decoder) = 0;
virtual bool decode(FrameReader *reader, int idx, VisionBuf *buf) = 0;
int width = 0, height = 0;
};
class FFmpegVideoDecoder : public VideoDecoder {
public:
FFmpegVideoDecoder();
~FFmpegVideoDecoder() override;
bool open(AVCodecParameters *codecpar, bool hw_decoder) override;
bool decode(FrameReader *reader, int idx, VisionBuf *buf) override;
private:
bool initHardwareDecoder(AVHWDeviceType hw_device_type);
@@ -56,3 +67,16 @@ private:
AVPixelFormat hw_pix_fmt = AV_PIX_FMT_NONE;
AVBufferRef *hw_device_ctx = nullptr;
};
#ifndef __APPLE__
class QcomVideoDecoder : public VideoDecoder {
public:
QcomVideoDecoder() {};
~QcomVideoDecoder() override {};
bool open(AVCodecParameters *codecpar, bool hw_decoder) override;
bool decode(FrameReader *reader, int idx, VisionBuf *buf) override;
private:
MsmVidc msm_vidc = MsmVidc();
};
#endif

View File

@@ -0,0 +1,346 @@
#include "qcom_decoder.h"
#include <assert.h>
#include "third_party/linux/include/v4l2-controls.h"
#include <linux/videodev2.h>
#include "common/swaglog.h"
#include "common/util.h"
// echo "0xFFFF" > /sys/kernel/debug/msm_vidc/debug_level
static void copyBuffer(VisionBuf *src_buf, VisionBuf *dst_buf) {
// Copy Y plane
memcpy(dst_buf->y, src_buf->y, src_buf->height * src_buf->stride);
// Copy UV plane
memcpy(dst_buf->uv, src_buf->uv, src_buf->height / 2 * src_buf->stride);
}
static void request_buffers(int fd, v4l2_buf_type buf_type, unsigned int count) {
struct v4l2_requestbuffers reqbuf = {
.count = count,
.type = buf_type,
.memory = V4L2_MEMORY_USERPTR
};
util::safe_ioctl(fd, VIDIOC_REQBUFS, &reqbuf, "VIDIOC_REQBUFS failed");
}
MsmVidc::~MsmVidc() {
if (fd > 0) {
close(fd);
}
}
bool MsmVidc::init(const char* dev, size_t width, size_t height, uint64_t codec) {
LOG("Initializing msm_vidc device %s", dev);
this->w = width;
this->h = height;
this->fd = open(dev, O_RDWR, 0);
if (fd < 0) {
LOGE("failed to open video device %s", dev);
return false;
}
subscribeEvents();
v4l2_buf_type out_type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
setPlaneFormat(out_type, V4L2_PIX_FMT_HEVC); // Also allocates the output buffer
setFPS(FPS);
request_buffers(fd, out_type, OUTPUT_BUFFER_COUNT);
util::safe_ioctl(fd, VIDIOC_STREAMON, &out_type, "VIDIOC_STREAMON OUTPUT failed");
restartCapture();
setupPolling();
this->initialized = true;
return true;
}
VisionBuf* MsmVidc::decodeFrame(AVPacket *pkt, VisionBuf *buf) {
assert(initialized && (pkt != nullptr) && (buf != nullptr));
this->frame_ready = false;
this->current_output_buf = buf;
bool sent_packet = false;
while (!this->frame_ready) {
if (!sent_packet) {
int buf_index = getBufferUnlocked();
if (buf_index >= 0) {
assert(buf_index < out_buf_cnt);
sendPacket(buf_index, pkt);
sent_packet = true;
}
}
if (poll(pfd, nfds, -1) < 0) {
LOGE("poll() error: %d", errno);
return nullptr;
}
if (VisionBuf* result = processEvents()) {
return result;
}
}
return buf;
}
VisionBuf* MsmVidc::processEvents() {
for (int idx = 0; idx < nfds; idx++) {
short revents = pfd[idx].revents;
if (!revents) continue;
if (idx == ev[EV_VIDEO]) {
if (revents & (POLLIN | POLLRDNORM)) {
VisionBuf *result = handleCapture();
if (result == this->current_output_buf) {
this->frame_ready = true;
}
}
if (revents & (POLLOUT | POLLWRNORM)) {
handleOutput();
}
if (revents & POLLPRI) {
handleEvent();
}
} else {
LOGE("Unexpected event on fd %d", pfd[idx].fd);
}
}
return nullptr;
}
VisionBuf* MsmVidc::handleCapture() {
struct v4l2_buffer buf = {0};
struct v4l2_plane planes[1] = {0};
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
buf.memory = V4L2_MEMORY_USERPTR;
buf.m.planes = planes;
buf.length = 1;
util::safe_ioctl(this->fd, VIDIOC_DQBUF, &buf, "VIDIOC_DQBUF CAPTURE failed");
if (this->reconfigure_pending || buf.m.planes[0].bytesused == 0) {
return nullptr;
}
copyBuffer(&cap_bufs[buf.index], this->current_output_buf);
queueCaptureBuffer(buf.index);
return this->current_output_buf;
}
bool MsmVidc::subscribeEvents() {
for (uint32_t event : subscriptions) {
struct v4l2_event_subscription sub = { .type = event};
util::safe_ioctl(fd, VIDIOC_SUBSCRIBE_EVENT, &sub, "VIDIOC_SUBSCRIBE_EVENT failed");
}
return true;
}
bool MsmVidc::setPlaneFormat(enum v4l2_buf_type type, uint32_t fourcc) {
struct v4l2_format fmt = {.type = type};
struct v4l2_pix_format_mplane *pix = &fmt.fmt.pix_mp;
*pix = {
.width = (__u32)this->w,
.height = (__u32)this->h,
.pixelformat = fourcc
};
util::safe_ioctl(fd, VIDIOC_S_FMT, &fmt, "VIDIOC_S_FMT failed");
if (type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
this->out_buf_size = pix->plane_fmt[0].sizeimage;
int ion_size = this->out_buf_size * OUTPUT_BUFFER_COUNT; // Output (input) buffers are ION buffer.
this->out_buf.allocate(ion_size); // mmap rw
for (int i = 0; i < OUTPUT_BUFFER_COUNT; i++) {
this->out_buf_off[i] = i * this->out_buf_size;
this->out_buf_addr[i] = (char *)this->out_buf.addr + this->out_buf_off[i];
this->out_buf_flag[i] = false;
}
LOGD("Set output buffer size to %d, count %d, addr %p", this->out_buf_size, OUTPUT_BUFFER_COUNT, this->out_buf.addr);
} else if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
request_buffers(this->fd, type, CAPTURE_BUFFER_COUNT);
util::safe_ioctl(fd, VIDIOC_G_FMT, &fmt, "VIDIOC_G_FMT failed");
const __u32 y_size = pix->plane_fmt[0].sizeimage;
const __u32 y_stride = pix->plane_fmt[0].bytesperline;
for (int i = 0; i < CAPTURE_BUFFER_COUNT; i++) {
size_t uv_offset = (size_t)y_stride * pix->height;
size_t required = uv_offset + (y_stride * pix->height / 2); // enough for Y + UV. For linear NV12, UV plane starts at y_stride * height.
size_t alloc_size = std::max<size_t>(y_size, required);
this->cap_bufs[i].allocate(alloc_size);
this->cap_bufs[i].init_yuv(pix->width, pix->height, y_stride, uv_offset);
}
LOGD("Set capture buffer size to %d, count %d, addr %p, extradata size %d",
pix->plane_fmt[0].sizeimage, CAPTURE_BUFFER_COUNT, this->cap_bufs[0].addr, pix->plane_fmt[1].sizeimage);
}
return true;
}
bool MsmVidc::setFPS(uint32_t fps) {
struct v4l2_streamparm streamparam = {
.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
.parm.output.timeperframe = {1, fps}
};
util::safe_ioctl(fd, VIDIOC_S_PARM, &streamparam, "VIDIOC_S_PARM failed");
return true;
}
bool MsmVidc::restartCapture() {
// stop if already initialized
enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
if (this->initialized) {
LOGD("Restarting capture, flushing buffers...");
util::safe_ioctl(this->fd, VIDIOC_STREAMOFF, &type, "VIDIOC_STREAMOFF CAPTURE failed");
struct v4l2_requestbuffers reqbuf = {.type = type, .memory = V4L2_MEMORY_USERPTR};
util::safe_ioctl(this->fd, VIDIOC_REQBUFS, &reqbuf, "VIDIOC_REQBUFS failed");
for (size_t i = 0; i < CAPTURE_BUFFER_COUNT; ++i) {
this->cap_bufs[i].free();
this->cap_buf_flag[i] = false; // mark as not queued
cap_bufs[i].~VisionBuf();
new (&cap_bufs[i]) VisionBuf();
}
}
// setup, start and queue capture buffers
setDBP();
setPlaneFormat(type, V4L2_PIX_FMT_NV12);
util::safe_ioctl(this->fd, VIDIOC_STREAMON, &type, "VIDIOC_STREAMON CAPTURE failed");
for (size_t i = 0; i < CAPTURE_BUFFER_COUNT; ++i) {
queueCaptureBuffer(i);
}
return true;
}
bool MsmVidc::queueCaptureBuffer(int i) {
struct v4l2_buffer buf = {0};
struct v4l2_plane planes[1] = {0};
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
buf.memory = V4L2_MEMORY_USERPTR;
buf.index = i;
buf.m.planes = planes;
buf.length = 1;
// decoded frame plane
planes[0].m.userptr = (unsigned long)this->cap_bufs[i].addr; // no security
planes[0].length = this->cap_bufs[i].len;
planes[0].reserved[0] = this->cap_bufs[i].fd; // ION fd
planes[0].reserved[1] = 0;
planes[0].bytesused = this->cap_bufs[i].len;
planes[0].data_offset = 0;
util::safe_ioctl(this->fd, VIDIOC_QBUF, &buf, "VIDIOC_QBUF failed");
this->cap_buf_flag[i] = true; // mark as queued
return true;
}
bool MsmVidc::queueOutputBuffer(int i, size_t size) {
struct v4l2_buffer buf = {0};
struct v4l2_plane planes[1] = {0};
buf.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
buf.memory = V4L2_MEMORY_USERPTR;
buf.index = i;
buf.m.planes = planes;
buf.length = 1;
// decoded frame plane
planes[0].m.userptr = (unsigned long)this->out_buf_off[i]; // check this
planes[0].length = this->out_buf_size;
planes[0].reserved[0] = this->out_buf.fd; // ION fd
planes[0].reserved[1] = 0;
planes[0].bytesused = size;
planes[0].data_offset = 0;
assert((this->out_buf_off[i] & 0xfff) == 0); // must be 4 KiB aligned
assert(this->out_buf_size % 4096 == 0); // ditto for size
util::safe_ioctl(this->fd, VIDIOC_QBUF, &buf, "VIDIOC_QBUF failed");
this->out_buf_flag[i] = true; // mark as queued
return true;
}
bool MsmVidc::setDBP() {
struct v4l2_ext_control control[2] = {0};
struct v4l2_ext_controls controls = {0};
control[0].id = V4L2_CID_MPEG_VIDC_VIDEO_STREAM_OUTPUT_MODE;
control[0].value = 1; // V4L2_CID_MPEG_VIDC_VIDEO_STREAM_OUTPUT_SECONDARY
control[1].id = V4L2_CID_MPEG_VIDC_VIDEO_DPB_COLOR_FORMAT;
control[1].value = 0; // V4L2_MPEG_VIDC_VIDEO_DPB_COLOR_FMT_NONE
controls.count = 2;
controls.ctrl_class = V4L2_CTRL_CLASS_MPEG;
controls.controls = control;
util::safe_ioctl(fd, VIDIOC_S_EXT_CTRLS, &controls, "VIDIOC_S_EXT_CTRLS failed");
return true;
}
bool MsmVidc::setupPolling() {
// Initialize poll array
pfd[EV_VIDEO] = {fd, POLLIN | POLLOUT | POLLWRNORM | POLLRDNORM | POLLPRI, 0};
ev[EV_VIDEO] = EV_VIDEO;
nfds = 1;
return true;
}
bool MsmVidc::sendPacket(int buf_index, AVPacket *pkt) {
assert(buf_index >= 0 && buf_index < out_buf_cnt);
assert(pkt != nullptr && pkt->data != nullptr && pkt->size > 0);
// Prepare output buffer
memset(this->out_buf_addr[buf_index], 0, this->out_buf_size);
uint8_t * data = (uint8_t *)this->out_buf_addr[buf_index];
memcpy(data, pkt->data, pkt->size);
queueOutputBuffer(buf_index, pkt->size);
return true;
}
int MsmVidc::getBufferUnlocked() {
for (int i = 0; i < this->out_buf_cnt; i++) {
if (!out_buf_flag[i]) {
return i;
}
}
return -1;
}
bool MsmVidc::handleOutput() {
struct v4l2_buffer buf = {0};
struct v4l2_plane planes[1];
buf.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
buf.memory = V4L2_MEMORY_USERPTR;
buf.m.planes = planes;
buf.length = 1;
util::safe_ioctl(this->fd, VIDIOC_DQBUF, &buf, "VIDIOC_DQBUF OUTPUT failed");
this->out_buf_flag[buf.index] = false; // mark as not queued
return true;
}
bool MsmVidc::handleEvent() {
// dequeue event
struct v4l2_event event = {0};
util::safe_ioctl(this->fd, VIDIOC_DQEVENT, &event, "VIDIOC_DQEVENT failed");
switch (event.type) {
case V4L2_EVENT_MSM_VIDC_PORT_SETTINGS_CHANGED_INSUFFICIENT: {
unsigned int *ptr = (unsigned int *)event.u.data;
unsigned int height = ptr[0];
unsigned int width = ptr[1];
this->w = width;
this->h = height;
LOGD("Port Reconfig received insufficient, new size %ux%u, flushing capture bufs...", width, height); // This is normal
struct v4l2_decoder_cmd dec;
dec.flags = V4L2_QCOM_CMD_FLUSH_CAPTURE;
dec.cmd = V4L2_QCOM_CMD_FLUSH;
util::safe_ioctl(this->fd, VIDIOC_DECODER_CMD, &dec, "VIDIOC_DECODER_CMD FLUSH_CAPTURE failed");
this->reconfigure_pending = true;
LOGD("Waiting for flush done event to reconfigure capture queue");
break;
}
case V4L2_EVENT_MSM_VIDC_FLUSH_DONE: {
unsigned int *ptr = (unsigned int *)event.u.data;
unsigned int flags = ptr[0];
if (flags & V4L2_QCOM_CMD_FLUSH_CAPTURE) {
if (this->reconfigure_pending) {
this->restartCapture();
this->reconfigure_pending = false;
}
}
break;
}
default:
break;
}
return true;
}

View File

@@ -0,0 +1,88 @@
#pragma once
#include <linux/videodev2.h>
#include <poll.h>
#include "msgq/visionipc/visionbuf.h"
extern "C" {
#include <libavcodec/avcodec.h>
#include <libavformat/avformat.h>
}
#define V4L2_EVENT_MSM_VIDC_START (V4L2_EVENT_PRIVATE_START + 0x00001000)
#define V4L2_EVENT_MSM_VIDC_FLUSH_DONE (V4L2_EVENT_MSM_VIDC_START + 1)
#define V4L2_EVENT_MSM_VIDC_PORT_SETTINGS_CHANGED_INSUFFICIENT (V4L2_EVENT_MSM_VIDC_START + 3)
#define V4L2_CID_MPEG_MSM_VIDC_BASE 0x00992000
#define V4L2_CID_MPEG_VIDC_VIDEO_DPB_COLOR_FORMAT (V4L2_CID_MPEG_MSM_VIDC_BASE + 44)
#define V4L2_CID_MPEG_VIDC_VIDEO_STREAM_OUTPUT_MODE (V4L2_CID_MPEG_MSM_VIDC_BASE + 22)
#define V4L2_QCOM_CMD_FLUSH_CAPTURE (1 << 1)
#define V4L2_QCOM_CMD_FLUSH (4)
#define VIDEO_DEVICE "/dev/video32"
#define OUTPUT_BUFFER_COUNT 8
#define CAPTURE_BUFFER_COUNT 8
#define FPS 20
class MsmVidc {
public:
MsmVidc() = default;
~MsmVidc();
bool init(const char* dev, size_t width, size_t height, uint64_t codec);
VisionBuf* decodeFrame(AVPacket* pkt, VisionBuf* buf);
AVFormatContext* avctx = nullptr;
int fd = 0;
private:
bool initialized = false;
bool reconfigure_pending = false;
bool frame_ready = false;
VisionBuf* current_output_buf = nullptr;
VisionBuf out_buf; // Single input buffer
VisionBuf cap_bufs[CAPTURE_BUFFER_COUNT]; // Capture (output) buffers
size_t w = 1928, h = 1208;
size_t cap_height = 0, cap_width = 0;
int cap_buf_size = 0;
int out_buf_size = 0;
size_t cap_plane_off[CAPTURE_BUFFER_COUNT] = {0};
size_t cap_plane_stride[CAPTURE_BUFFER_COUNT] = {0};
bool cap_buf_flag[CAPTURE_BUFFER_COUNT] = {false};
size_t out_buf_off[OUTPUT_BUFFER_COUNT] = {0};
void* out_buf_addr[OUTPUT_BUFFER_COUNT] = {0};
bool out_buf_flag[OUTPUT_BUFFER_COUNT] = {false};
const int out_buf_cnt = OUTPUT_BUFFER_COUNT;
const int subscriptions[2] = {
V4L2_EVENT_MSM_VIDC_FLUSH_DONE,
V4L2_EVENT_MSM_VIDC_PORT_SETTINGS_CHANGED_INSUFFICIENT
};
enum { EV_VIDEO, EV_COUNT };
struct pollfd pfd[EV_COUNT] = {0};
int ev[EV_COUNT] = {-1};
int nfds = 0;
VisionBuf* processEvents();
bool setupOutput();
bool subscribeEvents();
bool setPlaneFormat(v4l2_buf_type type, uint32_t fourcc);
bool setFPS(uint32_t fps);
bool restartCapture();
bool queueCaptureBuffer(int i);
bool queueOutputBuffer(int i, size_t size);
bool setDBP();
bool setupPolling();
bool sendPacket(int buf_index, AVPacket* pkt);
int getBufferUnlocked();
VisionBuf* handleCapture();
bool handleOutput();
bool handleEvent();
};

View File

@@ -11,7 +11,7 @@ from openpilot.tools.sim.lib.common import SimulatorState
class SimulatedCar:
"""Simulates a honda civic 2022 (panda state + can messages) to OpenPilot"""
packer = CANPacker("honda_civic_ex_2022_can_generated")
packer = CANPacker("honda_bosch_radarless_generated")
def __init__(self):
self.pm = messaging.PubMaster(['can', 'pandaStates'])
@@ -23,7 +23,7 @@ class SimulatedCar:
@staticmethod
def get_car_can_parser():
dbc_f = 'honda_civic_ex_2022_can_generated'
dbc_f = 'honda_bosch_radarless_generated'
checks = []
return CANParser(dbc_f, checks, 0)