forked from IQ.Lvbs/IQ.Pilot
IQ.Pilot Prebuilt Release @ ab07000
This commit is contained in:
IQ.Lvbs history cleanup
15
tools/Brewfile
Normal file
15
tools/Brewfile
Normal file
@@ -0,0 +1,15 @@
|
||||
brew "git-lfs"
|
||||
brew "capnp"
|
||||
brew "coreutils"
|
||||
brew "eigen"
|
||||
brew "ffmpeg"
|
||||
brew "glfw"
|
||||
brew "libusb"
|
||||
brew "libtool"
|
||||
brew "llvm"
|
||||
brew "openssl@3.0"
|
||||
brew "qt@5"
|
||||
brew "zeromq"
|
||||
cask "gcc-arm-embedded"
|
||||
brew "portaudio"
|
||||
brew "gcc@13"
|
||||
20
tools/CTF.md
Normal file
20
tools/CTF.md
Normal file
@@ -0,0 +1,20 @@
|
||||
## CTF
|
||||
Welcome to the first part of the comma CTF!
|
||||
|
||||
* all the flags are contained in this route: `0c7f0c7f0c7f0c7f|2021-10-13--13-00-00`
|
||||
* there's 2 flags in each segment, with roughly increasing difficulty
|
||||
* everything you'll need to find the flags is in the openpilot repo
|
||||
* grep is also your friend
|
||||
* first, [setup](https://github.com/commaai/openpilot/tree/master/tools#setup-your-pc) your PC
|
||||
* read the docs & checkout out the tools in tools/ and selfdrive/debug/
|
||||
* tip: once you get the replay and UI up, start by familiarizing yourself with seeking in replay
|
||||
|
||||
getting started
|
||||
```bash
|
||||
# start the route replay
|
||||
cd tools/replay
|
||||
./replay '0c7f0c7f0c7f0c7f|2021-10-13--13-00-00' --dcam --ecam
|
||||
|
||||
# start the UI in another terminal
|
||||
selfdrive/ui/ui
|
||||
```
|
||||
91
tools/README.md
Normal file
91
tools/README.md
Normal file
@@ -0,0 +1,91 @@
|
||||
# IQ.Pilot tools
|
||||
|
||||
## System Requirements
|
||||
|
||||
IQ.Pilot is developed and tested on **Apple macOS**, which is the primary development target aside from supported vehicle hardware (3, 3x, 4).
|
||||
|
||||
Most of IQ.Pilot should work natively on macOS. On Windows you can use WSL for a nearly native Ubuntu experience. Running natively on any other system is not currently recommended and will likely require modifications.
|
||||
|
||||
## Native setup on Ubuntu 24.04 and macOS
|
||||
|
||||
Follow these instructions for a fully managed setup experience. If you'd like to manage the dependencies yourself, just read the setup scripts in this directory.
|
||||
|
||||
**1. Clone IQ.Pilot**
|
||||
``` bash
|
||||
git clone https://gitlvb.teallvbs.xyz/IQ.Lvbs/IQ.Pilot.git
|
||||
```
|
||||
|
||||
**2. Run the setup script**
|
||||
``` bash
|
||||
cd IQ.Pilot
|
||||
tools/op.sh setup
|
||||
```
|
||||
|
||||
**3. Activate a Python shell**
|
||||
Activate a shell with the Python dependencies installed:
|
||||
``` bash
|
||||
source .venv/bin/activate
|
||||
```
|
||||
|
||||
**4. Build IQ.Pilot**
|
||||
``` bash
|
||||
scons -u -j$(nproc)
|
||||
```
|
||||
|
||||
# Using IQ.Pilot tools with Konn3kt:
|
||||
|
||||
This guide explains how to use IQ.Pilot tools to view and analyze routes from Konn3kt.
|
||||
|
||||
## Overview
|
||||
|
||||
All you need to do is authenticate with Konn3kt so you can access your routes.
|
||||
|
||||
## Quick Start
|
||||
|
||||
### 1. Authenticate with Konn3kt
|
||||
|
||||
Run the authentication helper:
|
||||
|
||||
```bash
|
||||
cd IQ.Pilot
|
||||
python3 tools/lib/auth.py
|
||||
```
|
||||
|
||||
This will:
|
||||
- Open your browser to log in via GitHub OAuth
|
||||
- Save your authentication token to `~/.comma/auth.json`
|
||||
- Allow access to your Konn3kt routes
|
||||
|
||||
If browser auto-open is unavailable (headless/WSL), copy the printed URL into any browser — the local callback listens on port 3000.
|
||||
|
||||
## How It Works
|
||||
|
||||
OP Tools reads your Konn3kt JWT token from `~/.comma/auth.json`.
|
||||
|
||||
You can always view public routes!
|
||||
|
||||
## WSL on Windows
|
||||
|
||||
[Windows Subsystem for Linux (WSL)](https://docs.microsoft.com/en-us/windows/wsl/about) should provide a similar experience to native Ubuntu. [WSL 2](https://docs.microsoft.com/en-us/windows/wsl/compare-versions) specifically has been reported by several users to be a seamless experience.
|
||||
|
||||
Follow [these instructions](https://docs.microsoft.com/en-us/windows/wsl/install) to setup the WSL and install the `Ubuntu-24.04` distribution. Once your Ubuntu WSL environment is setup, follow the Linux setup instructions to finish setting up your environment. See [these instructions](https://learn.microsoft.com/en-us/windows/wsl/tutorials/gui-apps) for running GUI apps.
|
||||
|
||||
**NOTE**: If you are running WSL and any GUIs are failing (segfaulting or other strange issues) even after following the steps above, you may need to enable software rendering with `LIBGL_ALWAYS_SOFTWARE=1`, e.g. `LIBGL_ALWAYS_SOFTWARE=1 selfdrive/ui/ui`.
|
||||
|
||||
## CTF
|
||||
Learn about the IQ.Pilot ecosystem and tools by playing our [CTF](/tools/CTF.md).
|
||||
|
||||
## Directory Structure
|
||||
|
||||
```
|
||||
├── cabana/ # View and plot CAN messages from drives or in realtime
|
||||
├── camerastream/ # Cameras stream over the network
|
||||
├── joystick/ # Control your car with a joystick
|
||||
├── lib/ # Libraries to support the tools and reading IQ.Pilot logs
|
||||
├── plotjuggler/ # A tool to plot IQ.Pilot logs
|
||||
├── replay/ # Replay drives and mock IQ.Pilot services
|
||||
├── scripts/ # Miscellaneous scripts
|
||||
├── serial/ # Tools for using the comma serial
|
||||
├── sim/ # Run IQ.Pilot in a simulator
|
||||
└── webcam/ # Run IQ.Pilot on a PC with webcams
|
||||
```
|
||||
0
tools/__init__.py
Normal file
0
tools/__init__.py
Normal file
17
tools/auto_source.py
Executable file
17
tools/auto_source.py
Executable file
@@ -0,0 +1,17 @@
|
||||
#!/usr/bin/env python3
|
||||
import sys
|
||||
from openpilot.tools.lib.logreader import LogReader, ReadMode
|
||||
|
||||
|
||||
def main():
|
||||
if len(sys.argv) != 2:
|
||||
print("Usage: python auto_source.py <log_path>")
|
||||
sys.exit(1)
|
||||
|
||||
log_path = sys.argv[1]
|
||||
lr = LogReader(log_path, default_mode=ReadMode.AUTO, sort_by_time=True)
|
||||
print("\n".join(lr.logreader_identifiers))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
4
tools/bodyteleop/.gitignore
vendored
Normal file
4
tools/bodyteleop/.gitignore
vendored
Normal file
@@ -0,0 +1,4 @@
|
||||
av
|
||||
av-10.0.0/*
|
||||
key.pem
|
||||
cert.pem
|
||||
103
tools/bodyteleop/static/index.html
Normal file
103
tools/bodyteleop/static/index.html
Normal file
@@ -0,0 +1,103 @@
|
||||
<html>
|
||||
<head>
|
||||
<meta charset="UTF-8"/>
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1.0" />
|
||||
<title>commabody</title>
|
||||
<link rel="stylesheet" href="/static/main.css">
|
||||
<link rel="stylesheet" href="https://cdnjs.cloudflare.com/ajax/libs/bootstrap/5.2.3/css/bootstrap.min.css" integrity="sha512-SbiR/eusphKoMVVXysTKG/7VseWii+Y3FdHrt0EpKgpToZeemhqHeZeLWLhJutz/2ut2Vw1uQEj2MbRF+TVBUA==" crossorigin="anonymous" referrerpolicy="no-referrer" /><script src="https://code.jquery.com/jquery-3.6.0.min.js"></script>
|
||||
<link rel="stylesheet" href="https://cdn.jsdelivr.net/npm/bootstrap-icons@1.10.5/font/bootstrap-icons.css">
|
||||
<script src="https://cdnjs.cloudflare.com/ajax/libs/bootstrap/5.2.3/js/bootstrap.min.js" integrity="sha512-1/RvZTcCDEUjY/CypiMz+iqqtaoQfAITmNSJY17Myp4Ms5mdxPS5UV7iOfdZoxcGhzFbOm6sntTKJppjvuhg4g==" crossorigin="anonymous" referrerpolicy="no-referrer"></script>
|
||||
<script src="https://cdnjs.cloudflare.com/ajax/libs/moment.js/2.29.1/moment.min.js"></script>
|
||||
<script src="https://cdn.jsdelivr.net/npm/chart.js@^3"></script>
|
||||
<script src="https://cdn.jsdelivr.net/npm/moment@^2"></script>
|
||||
<script src="https://cdn.jsdelivr.net/npm/chartjs-adapter-moment@^1"></script>
|
||||
</head>
|
||||
<body>
|
||||
<div id="main">
|
||||
<p class="jumbo">comma body</p>
|
||||
<audio id="audio" autoplay="true"></audio>
|
||||
<video id="video" playsinline autoplay muted loop poster="/static/poster.png"></video>
|
||||
<div id="icon-panel" class="row">
|
||||
<div class="col-sm-12 col-md-6 details">
|
||||
<div class="icon-sup-panel col-12">
|
||||
<div class="icon-sub-panel">
|
||||
<div class="icon-sub-sub-panel">
|
||||
<i class="bi bi-speaker-fill pre-blob"></i>
|
||||
<i class="bi bi-mic-fill pre-blob"></i>
|
||||
<i class="bi bi-camera-video-fill pre-blob"></i>
|
||||
</div>
|
||||
<p class="small">body</p>
|
||||
</div>
|
||||
<div class="icon-sub-panel">
|
||||
<div class="icon-sub-sub-panel">
|
||||
<i class="bi bi-speaker-fill pre-blob"></i>
|
||||
<i class="bi bi-mic-fill pre-blob"></i>
|
||||
</div>
|
||||
<p class="small">you</p>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
<div class="col-sm-12 col-md-6 details">
|
||||
<div class="icon-sup-panel col-12">
|
||||
<div class="icon-sub-panel">
|
||||
<div class="icon-sub-sub-panel">
|
||||
<i id="ping-time" class="pre-blob1">-</i>
|
||||
</div>
|
||||
<p class="bi bi-arrow-repeat small"> ping time</p>
|
||||
</div>
|
||||
<div class="icon-sub-panel">
|
||||
<div class="icon-sub-sub-panel">
|
||||
<i id="battery" class="pre-blob1">-</i>
|
||||
</div>
|
||||
<p class="bi bi-battery-half small"> battery</p>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
<!-- <div class="icon-sub-panel">
|
||||
<button type="button" id="start" class="btn btn-light btn-lg">Start</button>
|
||||
<button type="button" id="stop" class="btn btn-light btn-lg">Stop</button>
|
||||
</div> -->
|
||||
</div>
|
||||
<div class="row" style="width: 100%; padding: 0px 10px 0px 10px;">
|
||||
<div id="wasd" class="col-md-12 row">
|
||||
<div class="col-md-6 col-sm-12" style="justify-content: center; display: flex; flex-direction: column;">
|
||||
<div class="wasd-row">
|
||||
<div class="keys" id="key-w">W</div>
|
||||
<div id="key-val"><span id="pos-vals">0,0</span><span>x,y</span></div>
|
||||
</div>
|
||||
<div class="wasd-row">
|
||||
<div class="keys" id="key-a">A</div>
|
||||
<div class="keys" id="key-s">S</div>
|
||||
<div class="keys" id="key-d">D</div>
|
||||
</div>
|
||||
</div>
|
||||
<div class="col-md-6 col-sm-12 form-group plan-form">
|
||||
<label for="plan-text">Plan (w, a, s, d, t)</label>
|
||||
<label style="font-size: 15px;" for="plan-text">*Extremely Experimental*</label>
|
||||
<textarea class="form-control" id="plan-text" rows="7" placeholder="1,0,0,0,2"></textarea>
|
||||
<button type="button" id="plan-button" class="btn btn-light btn-lg">Execute</button>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
<div class="row" style="padding: 0px 10px 0px 10px; width: 100%;">
|
||||
<div class="panel row">
|
||||
<div class="col-sm-3" style="text-align: center;">
|
||||
<p>Play Sounds</p>
|
||||
</div>
|
||||
<div class="btn-group col-sm-8">
|
||||
<button type="button" id="sound-engage" class="btn btn-outline-success btn-lg sound">Engage</button>
|
||||
<button type="button" id="sound-disengage" class="btn btn-outline-warning btn-lg sound">Disengage</button>
|
||||
<button type="button" id="sound-error" class="btn btn-outline-danger btn-lg sound">Error</button>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
<div class="row" style="padding: 0px 10px 0px 10px; width: 100%;">
|
||||
<div class="panel row">
|
||||
<div class="col-sm-6"><canvas id="chart-ping"></canvas></div>
|
||||
<div class="col-sm-6"><canvas id="chart-battery"></canvas></div>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
<script src="/static/js/jsmain.js" type="module"></script>
|
||||
</body>
|
||||
</html>
|
||||
54
tools/bodyteleop/static/js/controls.js
vendored
Normal file
54
tools/bodyteleop/static/js/controls.js
vendored
Normal file
@@ -0,0 +1,54 @@
|
||||
const keyVals = {w: 0, a: 0, s: 0, d: 0}
|
||||
|
||||
export function getXY() {
|
||||
let x = -keyVals.w + keyVals.s
|
||||
let y = -keyVals.d + keyVals.a
|
||||
return {x, y}
|
||||
}
|
||||
|
||||
export const handleKeyX = (key, setValue) => {
|
||||
if (['w', 'a', 's', 'd'].includes(key)){
|
||||
keyVals[key] = setValue;
|
||||
let color = "#333";
|
||||
if (setValue === 1){
|
||||
color = "#e74c3c";
|
||||
}
|
||||
$("#key-"+key).css('background', color);
|
||||
const {x, y} = getXY();
|
||||
$("#pos-vals").text(x+","+y);
|
||||
}
|
||||
};
|
||||
|
||||
export async function executePlan() {
|
||||
let plan = $("#plan-text").val();
|
||||
const planList = [];
|
||||
plan.split("\n").forEach(function(e){
|
||||
let line = e.split(",").map(k=>parseInt(k));
|
||||
if (line.length != 5 || line.slice(0, 4).map(e=>[1, 0].includes(e)).includes(false) || line[4] < 0 || line[4] > 10){
|
||||
console.log("invalid plan");
|
||||
}
|
||||
else{
|
||||
planList.push(line)
|
||||
}
|
||||
});
|
||||
|
||||
async function execute() {
|
||||
for (var i = 0; i < planList.length; i++) {
|
||||
let [w, a, s, d, t] = planList[i];
|
||||
while(t > 0){
|
||||
console.log(w, a, s, d, t);
|
||||
if(w==1){$("#key-w").mousedown();}
|
||||
if(a==1){$("#key-a").mousedown();}
|
||||
if(s==1){$("#key-s").mousedown();}
|
||||
if(d==1){$("#key-d").mousedown();}
|
||||
await sleep(50);
|
||||
$("#key-w").mouseup();
|
||||
$("#key-a").mouseup();
|
||||
$("#key-s").mouseup();
|
||||
$("#key-d").mouseup();
|
||||
t = t - 0.05;
|
||||
}
|
||||
}
|
||||
}
|
||||
execute();
|
||||
}
|
||||
27
tools/bodyteleop/static/js/jsmain.js
Normal file
27
tools/bodyteleop/static/js/jsmain.js
Normal file
@@ -0,0 +1,27 @@
|
||||
import { handleKeyX, executePlan } from "./controls.js";
|
||||
import { start, stop, lastChannelMessageTime, playSoundRequest } from "./webrtc.js";
|
||||
|
||||
export var pc = null;
|
||||
export var dc = null;
|
||||
|
||||
document.addEventListener('keydown', (e)=>(handleKeyX(e.key.toLowerCase(), 1)));
|
||||
document.addEventListener('keyup', (e)=>(handleKeyX(e.key.toLowerCase(), 0)));
|
||||
$(".keys").bind("mousedown touchstart", (e)=>handleKeyX($(e.target).attr('id').replace('key-', ''), 1));
|
||||
$(".keys").bind("mouseup touchend", (e)=>handleKeyX($(e.target).attr('id').replace('key-', ''), 0));
|
||||
$("#plan-button").click(executePlan);
|
||||
$(".sound").click((e)=>{
|
||||
const sound = $(e.target).attr('id').replace('sound-', '')
|
||||
return playSoundRequest(sound);
|
||||
});
|
||||
|
||||
setInterval( () => {
|
||||
const dt = new Date().getTime();
|
||||
if ((dt - lastChannelMessageTime) > 1000) {
|
||||
$(".pre-blob").removeClass('blob');
|
||||
$("#battery").text("-");
|
||||
$("#ping-time").text('-');
|
||||
$("video")[0].load();
|
||||
}
|
||||
}, 5000);
|
||||
|
||||
start(pc, dc);
|
||||
53
tools/bodyteleop/static/js/plots.js
Normal file
53
tools/bodyteleop/static/js/plots.js
Normal file
@@ -0,0 +1,53 @@
|
||||
export const pingPoints = [];
|
||||
export const batteryPoints = [];
|
||||
|
||||
function getChartConfig(pts, color, title, ymax=100) {
|
||||
return {
|
||||
type: 'line',
|
||||
data: {
|
||||
datasets: [{
|
||||
label: title,
|
||||
data: pts,
|
||||
borderWidth: 1,
|
||||
borderColor: color,
|
||||
backgroundColor: color,
|
||||
fill: 'origin'
|
||||
}]
|
||||
},
|
||||
options: {
|
||||
scales: {
|
||||
x: {
|
||||
type: 'time',
|
||||
time: {
|
||||
unit: 'minute',
|
||||
displayFormats: {
|
||||
second: 'h:mm a'
|
||||
}
|
||||
},
|
||||
grid: {
|
||||
color: '#222', // Grid lines color
|
||||
},
|
||||
ticks: {
|
||||
source: 'data',
|
||||
fontColor: 'rgba(255, 255, 255, 1.0)', // Y-axis label color
|
||||
}
|
||||
},
|
||||
y: {
|
||||
beginAtZero: true,
|
||||
max: ymax,
|
||||
grid: {
|
||||
color: 'rgba(255, 255, 255, 0.1)', // Grid lines color
|
||||
},
|
||||
ticks: {
|
||||
fontColor: 'rgba(255, 255, 255, 0.7)', // Y-axis label color
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
const ctxPing = document.getElementById('chart-ping');
|
||||
const ctxBattery = document.getElementById('chart-battery');
|
||||
export const chartPing = new Chart(ctxPing, getChartConfig(pingPoints, 'rgba(192, 57, 43, 0.7)', 'Controls Ping Time (ms)', 250));
|
||||
export const chartBattery = new Chart(ctxBattery, getChartConfig(batteryPoints, 'rgba(41, 128, 185, 0.7)', 'Battery %', 100));
|
||||
209
tools/bodyteleop/static/js/webrtc.js
Normal file
209
tools/bodyteleop/static/js/webrtc.js
Normal file
@@ -0,0 +1,209 @@
|
||||
import { getXY } from "./controls.js";
|
||||
import { pingPoints, batteryPoints, chartPing, chartBattery } from "./plots.js";
|
||||
|
||||
export let controlCommandInterval = null;
|
||||
export let latencyInterval = null;
|
||||
export let lastChannelMessageTime = null;
|
||||
|
||||
|
||||
export function offerRtcRequest(sdp, type) {
|
||||
return fetch('/offer', {
|
||||
body: JSON.stringify({sdp: sdp, type: type}),
|
||||
headers: {'Content-Type': 'application/json'},
|
||||
method: 'POST'
|
||||
});
|
||||
}
|
||||
|
||||
|
||||
export function playSoundRequest(sound) {
|
||||
return fetch('/sound', {
|
||||
body: JSON.stringify({sound}),
|
||||
headers: {'Content-Type': 'application/json'},
|
||||
method: 'POST'
|
||||
});
|
||||
}
|
||||
|
||||
|
||||
export function pingHeadRequest() {
|
||||
return fetch('/', {
|
||||
method: 'HEAD'
|
||||
});
|
||||
}
|
||||
|
||||
|
||||
export function createPeerConnection(pc) {
|
||||
var config = {
|
||||
sdpSemantics: 'unified-plan'
|
||||
};
|
||||
|
||||
pc = new RTCPeerConnection(config);
|
||||
|
||||
// connect audio / video
|
||||
pc.addEventListener('track', function(evt) {
|
||||
console.log("Adding Tracks!")
|
||||
if (evt.track.kind == 'video')
|
||||
document.getElementById('video').srcObject = evt.streams[0];
|
||||
else
|
||||
document.getElementById('audio').srcObject = evt.streams[0];
|
||||
});
|
||||
return pc;
|
||||
}
|
||||
|
||||
|
||||
export function negotiate(pc) {
|
||||
return pc.createOffer({offerToReceiveAudio:true, offerToReceiveVideo:true}).then(function(offer) {
|
||||
return pc.setLocalDescription(offer);
|
||||
}).then(function() {
|
||||
return new Promise(function(resolve) {
|
||||
if (pc.iceGatheringState === 'complete') {
|
||||
resolve();
|
||||
}
|
||||
else {
|
||||
function checkState() {
|
||||
if (pc.iceGatheringState === 'complete') {
|
||||
pc.removeEventListener('icegatheringstatechange', checkState);
|
||||
resolve();
|
||||
}
|
||||
}
|
||||
pc.addEventListener('icegatheringstatechange', checkState);
|
||||
}
|
||||
});
|
||||
}).then(function() {
|
||||
var offer = pc.localDescription;
|
||||
return offerRtcRequest(offer.sdp, offer.type);
|
||||
}).then(function(response) {
|
||||
console.log(response);
|
||||
return response.json();
|
||||
}).then(function(answer) {
|
||||
return pc.setRemoteDescription(answer);
|
||||
}).catch(function(e) {
|
||||
alert(e);
|
||||
});
|
||||
}
|
||||
|
||||
|
||||
function isMobile() {
|
||||
let check = false;
|
||||
(function(a){if(/(android|bb\d+|meego).+mobile|avantgo|bada\/|blackberry|blazer|compal|elaine|fennec|hiptop|iemobile|ip(hone|od)|iris|kindle|lge |maemo|midp|mmp|mobile.+firefox|netfront|opera m(ob|in)i|palm( os)?|phone|p(ixi|re)\/|plucker|pocket|psp|series(4|6)0|symbian|treo|up\.(browser|link)|vodafone|wap|windows ce|xda|xiino/i.test(a)||/1207|6310|6590|3gso|4thp|50[1-6]i|770s|802s|a wa|abac|ac(er|oo|s\-)|ai(ko|rn)|al(av|ca|co)|amoi|an(ex|ny|yw)|aptu|ar(ch|go)|as(te|us)|attw|au(di|\-m|r |s )|avan|be(ck|ll|nq)|bi(lb|rd)|bl(ac|az)|br(e|v)w|bumb|bw\-(n|u)|c55\/|capi|ccwa|cdm\-|cell|chtm|cldc|cmd\-|co(mp|nd)|craw|da(it|ll|ng)|dbte|dc\-s|devi|dica|dmob|do(c|p)o|ds(12|\-d)|el(49|ai)|em(l2|ul)|er(ic|k0)|esl8|ez([4-7]0|os|wa|ze)|fetc|fly(\-|_)|g1 u|g560|gene|gf\-5|g\-mo|go(\.w|od)|gr(ad|un)|haie|hcit|hd\-(m|p|t)|hei\-|hi(pt|ta)|hp( i|ip)|hs\-c|ht(c(\-| |_|a|g|p|s|t)|tp)|hu(aw|tc)|i\-(20|go|ma)|i230|iac( |\-|\/)|ibro|idea|ig01|ikom|im1k|inno|ipaq|iris|ja(t|v)a|jbro|jemu|jigs|kddi|keji|kgt( |\/)|klon|kpt |kwc\-|kyo(c|k)|le(no|xi)|lg( g|\/(k|l|u)|50|54|\-[a-w])|libw|lynx|m1\-w|m3ga|m50\/|ma(te|ui|xo)|mc(01|21|ca)|m\-cr|me(rc|ri)|mi(o8|oa|ts)|mmef|mo(01|02|bi|de|do|t(\-| |o|v)|zz)|mt(50|p1|v )|mwbp|mywa|n10[0-2]|n20[2-3]|n30(0|2)|n50(0|2|5)|n7(0(0|1)|10)|ne((c|m)\-|on|tf|wf|wg|wt)|nok(6|i)|nzph|o2im|op(ti|wv)|oran|owg1|p800|pan(a|d|t)|pdxg|pg(13|\-([1-8]|c))|phil|pire|pl(ay|uc)|pn\-2|po(ck|rt|se)|prox|psio|pt\-g|qa\-a|qc(07|12|21|32|60|\-[2-7]|i\-)|qtek|r380|r600|raks|rim9|ro(ve|zo)|s55\/|sa(ge|ma|mm|ms|ny|va)|sc(01|h\-|oo|p\-)|sdk\/|se(c(\-|0|1)|47|mc|nd|ri)|sgh\-|shar|sie(\-|m)|sk\-0|sl(45|id)|sm(al|ar|b3|it|t5)|so(ft|ny)|sp(01|h\-|v\-|v )|sy(01|mb)|t2(18|50)|t6(00|10|18)|ta(gt|lk)|tcl\-|tdg\-|tel(i|m)|tim\-|t\-mo|to(pl|sh)|ts(70|m\-|m3|m5)|tx\-9|up(\.b|g1|si)|utst|v400|v750|veri|vi(rg|te)|vk(40|5[0-3]|\-v)|vm40|voda|vulc|vx(52|53|60|61|70|80|81|83|85|98)|w3c(\-| )|webc|whit|wi(g |nc|nw)|wmlb|wonu|x700|yas\-|your|zeto|zte\-/i.test(a.substr(0,4))) check = true;})(navigator.userAgent||navigator.vendor||window.opera);
|
||||
return check;
|
||||
};
|
||||
|
||||
|
||||
export const constraints = {
|
||||
audio: {
|
||||
autoGainControl: false,
|
||||
sampleRate: 48000,
|
||||
sampleSize: 16,
|
||||
echoCancellation: true,
|
||||
noiseSuppression: true,
|
||||
channelCount: 1
|
||||
},
|
||||
video: isMobile()
|
||||
};
|
||||
|
||||
|
||||
export function start(pc, dc) {
|
||||
pc = createPeerConnection(pc);
|
||||
|
||||
// add audio track
|
||||
navigator.mediaDevices.enumerateDevices()
|
||||
.then(function(devices) {
|
||||
const hasAudioInput = devices.find((device) => device.kind === "audioinput");
|
||||
var modifiedConstraints = {};
|
||||
modifiedConstraints.video = constraints.video;
|
||||
modifiedConstraints.audio = hasAudioInput ? constraints.audio : false;
|
||||
|
||||
return Promise.resolve(modifiedConstraints);
|
||||
})
|
||||
.then(function(constraints) {
|
||||
if (constraints.audio || constraints.video) {
|
||||
return navigator.mediaDevices.getUserMedia(constraints);
|
||||
} else{
|
||||
return Promise.resolve(null);
|
||||
}
|
||||
})
|
||||
.then(function(stream) {
|
||||
if (stream) {
|
||||
stream.getTracks().forEach(function(track) {
|
||||
pc.addTrack(track, stream);
|
||||
});
|
||||
}
|
||||
|
||||
return negotiate(pc);
|
||||
})
|
||||
.catch(function(err) {
|
||||
alert('Could not acquire media: ' + err);
|
||||
});
|
||||
|
||||
var parameters = {"ordered": true};
|
||||
dc = pc.createDataChannel('data', parameters);
|
||||
dc.onclose = function() {
|
||||
clearInterval(controlCommandInterval);
|
||||
clearInterval(latencyInterval);
|
||||
};
|
||||
|
||||
function sendJoystickOverDataChannel() {
|
||||
const {x, y} = getXY();
|
||||
var message = JSON.stringify({type: "testJoystick", data: {axes: [x, y], buttons: [false]}})
|
||||
dc.send(message);
|
||||
}
|
||||
function checkLatency() {
|
||||
const initialTime = new Date().getTime();
|
||||
pingHeadRequest().then(function() {
|
||||
const currentTime = new Date().getTime();
|
||||
if (Math.abs(currentTime - lastChannelMessageTime) < 1000) {
|
||||
const pingtime = currentTime - initialTime;
|
||||
pingPoints.push({'x': currentTime, 'y': pingtime});
|
||||
if (pingPoints.length > 1000) {
|
||||
pingPoints.shift();
|
||||
}
|
||||
chartPing.update();
|
||||
$("#ping-time").text((pingtime) + "ms");
|
||||
}
|
||||
})
|
||||
}
|
||||
dc.onopen = function() {
|
||||
controlCommandInterval = setInterval(sendJoystickOverDataChannel, 50);
|
||||
latencyInterval = setInterval(checkLatency, 1000);
|
||||
sendJoystickOverDataChannel();
|
||||
};
|
||||
|
||||
const textDecoder = new TextDecoder();
|
||||
var carStaterIndex = 0;
|
||||
dc.onmessage = function(evt) {
|
||||
const text = textDecoder.decode(evt.data);
|
||||
const msg = JSON.parse(text);
|
||||
if (carStaterIndex % 100 == 0 && msg.type === 'carState') {
|
||||
const batteryLevel = Math.round(msg.data.fuelGauge * 100);
|
||||
$("#battery").text(batteryLevel + "%");
|
||||
batteryPoints.push({'x': new Date().getTime(), 'y': batteryLevel});
|
||||
if (batteryPoints.length > 1000) {
|
||||
batteryPoints.shift();
|
||||
}
|
||||
chartBattery.update();
|
||||
}
|
||||
carStaterIndex += 1;
|
||||
lastChannelMessageTime = new Date().getTime();
|
||||
$(".pre-blob").addClass('blob');
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
export function stop(pc, dc) {
|
||||
if (dc) {
|
||||
dc.close();
|
||||
}
|
||||
if (pc.getTransceivers) {
|
||||
pc.getTransceivers().forEach(function(transceiver) {
|
||||
if (transceiver.stop) {
|
||||
transceiver.stop();
|
||||
}
|
||||
});
|
||||
}
|
||||
pc.getSenders().forEach(function(sender) {
|
||||
sender.track.stop();
|
||||
});
|
||||
setTimeout(function() {
|
||||
pc.close();
|
||||
}, 500);
|
||||
}
|
||||
185
tools/bodyteleop/static/main.css
Normal file
185
tools/bodyteleop/static/main.css
Normal file
@@ -0,0 +1,185 @@
|
||||
body {
|
||||
background: #333 !important;
|
||||
color: #fff !important;
|
||||
display: flex;
|
||||
justify-content: center;
|
||||
align-items: start;
|
||||
}
|
||||
|
||||
p {
|
||||
margin: 0px !important;
|
||||
}
|
||||
|
||||
i {
|
||||
font-style: normal;
|
||||
}
|
||||
|
||||
.small {
|
||||
font-size: 1em !important
|
||||
}
|
||||
|
||||
.jumbo {
|
||||
font-size: 8rem;
|
||||
}
|
||||
|
||||
|
||||
@media (max-width: 600px) {
|
||||
.small {
|
||||
font-size: 0.5em !important
|
||||
}
|
||||
.jumbo {
|
||||
display: none;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#main {
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
align-content: center;
|
||||
justify-content: center;
|
||||
align-items: center;
|
||||
font-size: 30px;
|
||||
width: 100%;
|
||||
max-width: 1200px;
|
||||
}
|
||||
|
||||
video {
|
||||
width: 95%;
|
||||
}
|
||||
|
||||
.pre-blob {
|
||||
display: flex;
|
||||
background: #333;
|
||||
border-radius: 50%;
|
||||
margin: 10px;
|
||||
height: 45px;
|
||||
width: 45px;
|
||||
justify-content: center;
|
||||
align-items: center;
|
||||
font-size: 1rem;
|
||||
}
|
||||
|
||||
.blob {
|
||||
background: rgba(231, 76, 60,1.0);
|
||||
box-shadow: 0 0 0 0 rgba(231, 76, 60,1.0);
|
||||
animation: pulse 2s infinite;
|
||||
}
|
||||
|
||||
@keyframes pulse {
|
||||
0% {
|
||||
box-shadow: 0 0 0 0px rgba(192, 57, 43, 1);
|
||||
}
|
||||
100% {
|
||||
box-shadow: 0 0 0 20px rgba(192, 57, 43, 0);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
.icon-sup-panel {
|
||||
display: flex;
|
||||
flex-direction: row;
|
||||
justify-content: space-around;
|
||||
align-items: center;
|
||||
background: #222;
|
||||
border-radius: 10px;
|
||||
padding: 5px;
|
||||
margin: 5px 0px 5px 0px;
|
||||
}
|
||||
|
||||
.icon-sub-panel {
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
justify-content: space-between;
|
||||
align-items: center;
|
||||
}
|
||||
|
||||
#icon-panel {
|
||||
display: flex;
|
||||
width: 100%;
|
||||
justify-content: space-between;
|
||||
margin-top: 5px;
|
||||
}
|
||||
|
||||
.icon-sub-sub-panel {
|
||||
display: flex;
|
||||
flex-direction: row;
|
||||
}
|
||||
|
||||
.keys, #key-val {
|
||||
background: #333;
|
||||
padding: 2rem;
|
||||
margin: 5px;
|
||||
color: #fff;
|
||||
display: flex;
|
||||
justify-content: center;
|
||||
align-items: center;
|
||||
border-radius: 10px;
|
||||
cursor: pointer;
|
||||
}
|
||||
|
||||
#key-val {
|
||||
pointer-events: none;
|
||||
background: #fff;
|
||||
color: #333;
|
||||
line-height: 1;
|
||||
font-size: 20px;
|
||||
flex-direction: column;
|
||||
}
|
||||
|
||||
.wasd-row {
|
||||
display: flex;
|
||||
flex-direction: row;
|
||||
justify-content: center;
|
||||
align-items: stretch;
|
||||
}
|
||||
|
||||
#wasd {
|
||||
margin: 5px 0px 5px 0px;
|
||||
background: #222;
|
||||
border-radius: 10px;
|
||||
width: 100%;
|
||||
padding: 20px;
|
||||
display: flex;
|
||||
flex-direction: row;
|
||||
justify-content: space-around;
|
||||
align-items: stretch;
|
||||
|
||||
user-select: none;
|
||||
-webkit-touch-callout: none;
|
||||
-webkit-user-select: none;
|
||||
-khtml-user-select: none;
|
||||
-moz-user-select: none;
|
||||
-ms-user-select: none;
|
||||
touch-action: manipulation;
|
||||
}
|
||||
|
||||
.panel {
|
||||
display: flex;
|
||||
justify-content: center;
|
||||
margin: 5px 0px 5px 0px !important;
|
||||
background: #222;
|
||||
border-radius: 10px;
|
||||
width: 100%;
|
||||
padding: 10px;
|
||||
}
|
||||
|
||||
#ping-time, #battery {
|
||||
font-size: 25px;
|
||||
}
|
||||
|
||||
#stop {
|
||||
display: none;
|
||||
}
|
||||
|
||||
.plan-form {
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
justify-content: space-between;
|
||||
align-items: center;
|
||||
}
|
||||
|
||||
.details {
|
||||
display: flex;
|
||||
padding: 0px 10px 0px 10px;
|
||||
}
|
||||
BIN
tools/bodyteleop/static/poster.png
Normal file
BIN
tools/bodyteleop/static/poster.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 8.1 KiB |
133
tools/bodyteleop/web.py
Normal file
133
tools/bodyteleop/web.py
Normal file
@@ -0,0 +1,133 @@
|
||||
import asyncio
|
||||
import dataclasses
|
||||
import json
|
||||
import logging
|
||||
import os
|
||||
import ssl
|
||||
import subprocess
|
||||
|
||||
import wave
|
||||
from aiohttp import web
|
||||
from aiohttp import ClientSession
|
||||
|
||||
from openpilot.common.basedir import BASEDIR
|
||||
from openpilot.system.webrtc.webrtcd import StreamRequestBody
|
||||
from openpilot.common.params import Params
|
||||
|
||||
logger = logging.getLogger("bodyteleop")
|
||||
logging.basicConfig(level=logging.INFO)
|
||||
|
||||
TELEOPDIR = f"{BASEDIR}/tools/bodyteleop"
|
||||
WEBRTCD_HOST, WEBRTCD_PORT = "localhost", 5001
|
||||
|
||||
|
||||
def _require_pyaudio():
|
||||
import pyaudio
|
||||
return pyaudio
|
||||
|
||||
|
||||
## UTILS
|
||||
async def play_sound(sound: str):
|
||||
SOUNDS = {
|
||||
"engage": "selfdrive/assets/sounds/engage.wav",
|
||||
"disengage": "selfdrive/assets/sounds/disengage.wav",
|
||||
"error": "selfdrive/assets/sounds/warning_immediate.wav",
|
||||
}
|
||||
assert sound in SOUNDS
|
||||
|
||||
chunk = 5120
|
||||
with wave.open(os.path.join(BASEDIR, SOUNDS[sound]), "rb") as wf:
|
||||
pyaudio = _require_pyaudio()
|
||||
|
||||
def callback(in_data, frame_count, time_info, status):
|
||||
data = wf.readframes(frame_count)
|
||||
return data, pyaudio.paContinue
|
||||
|
||||
p = pyaudio.PyAudio()
|
||||
stream = p.open(format=p.get_format_from_width(wf.getsampwidth()),
|
||||
channels=wf.getnchannels(),
|
||||
rate=wf.getframerate(),
|
||||
output=True,
|
||||
frames_per_buffer=chunk,
|
||||
stream_callback=callback)
|
||||
stream.start_stream()
|
||||
while stream.is_active():
|
||||
await asyncio.sleep(0)
|
||||
stream.stop_stream()
|
||||
stream.close()
|
||||
p.terminate()
|
||||
|
||||
## SSL
|
||||
def create_ssl_cert(cert_path: str, key_path: str):
|
||||
try:
|
||||
proc = subprocess.run(f'openssl req -x509 -newkey rsa:4096 -nodes -out {cert_path} -keyout {key_path} \
|
||||
-days 365 -subj "/C=US/ST=California/O=commaai/OU=comma body"',
|
||||
capture_output=True, shell=True)
|
||||
proc.check_returncode()
|
||||
except subprocess.CalledProcessError as ex:
|
||||
raise ValueError(f"Error creating SSL certificate:\n[stdout]\n{proc.stdout.decode()}\n[stderr]\n{proc.stderr.decode()}") from ex
|
||||
|
||||
|
||||
def create_ssl_context():
|
||||
cert_path = os.path.join(TELEOPDIR, "cert.pem")
|
||||
key_path = os.path.join(TELEOPDIR, "key.pem")
|
||||
if not os.path.exists(cert_path) or not os.path.exists(key_path):
|
||||
logger.info("Creating certificate...")
|
||||
create_ssl_cert(cert_path, key_path)
|
||||
else:
|
||||
logger.info("Certificate exists!")
|
||||
ssl_context = ssl.SSLContext(protocol=ssl.PROTOCOL_TLS_SERVER)
|
||||
ssl_context.load_cert_chain(cert_path, key_path)
|
||||
|
||||
return ssl_context
|
||||
|
||||
## ENDPOINTS
|
||||
async def index(request: 'web.Request'):
|
||||
with open(os.path.join(TELEOPDIR, "static", "index.html")) as f:
|
||||
content = f.read()
|
||||
return web.Response(content_type="text/html", text=content)
|
||||
|
||||
|
||||
async def ping(request: 'web.Request'):
|
||||
return web.Response(text="pong")
|
||||
|
||||
|
||||
async def sound(request: 'web.Request'):
|
||||
params = await request.json()
|
||||
sound_to_play = params["sound"]
|
||||
|
||||
await play_sound(sound_to_play)
|
||||
return web.json_response({"status": "ok"})
|
||||
|
||||
|
||||
async def offer(request: 'web.Request'):
|
||||
params = await request.json()
|
||||
body = StreamRequestBody(params["sdp"], ["driver"], ["testJoystick"], ["carState"])
|
||||
body_json = json.dumps(dataclasses.asdict(body))
|
||||
|
||||
logger.info("Sending offer to webrtcd...")
|
||||
webrtcd_url = f"http://{WEBRTCD_HOST}:{WEBRTCD_PORT}/stream"
|
||||
async with ClientSession() as session, session.post(webrtcd_url, data=body_json) as resp:
|
||||
assert resp.status == 200
|
||||
answer = await resp.json()
|
||||
return web.json_response(answer)
|
||||
|
||||
|
||||
def main():
|
||||
# Enable joystick debug mode
|
||||
Params().put_bool("JoystickDebugMode", True)
|
||||
|
||||
# App needs to be HTTPS for microphone and audio autoplay to work on the browser
|
||||
ssl_context = create_ssl_context()
|
||||
|
||||
app = web.Application()
|
||||
app.router.add_get("/", index)
|
||||
app.router.add_get("/ping", ping, allow_head=True)
|
||||
app.router.add_post("/offer", offer)
|
||||
app.router.add_post("/sound", sound)
|
||||
app.router.add_static('/static', os.path.join(TELEOPDIR, 'static'))
|
||||
web.run_app(app, access_log=None, host="0.0.0.0", port=5000, ssl_context=ssl_context)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
66
tools/camerastream/README.md
Normal file
66
tools/camerastream/README.md
Normal file
@@ -0,0 +1,66 @@
|
||||
# Camera stream
|
||||
|
||||
`compressed_vipc.py` connects to a remote device running openpilot, decodes the video streams, and republishes them over VisionIPC.
|
||||
|
||||
## Usage
|
||||
|
||||
### On the device
|
||||
SSH into the device and run following in separate terminals:
|
||||
|
||||
`cd /data/openpilot/cereal/messaging && ./bridge`
|
||||
|
||||
`cd /data/openpilot/system/loggerd && ./encoderd`
|
||||
|
||||
`cd /data/openpilot/system/camerad && ./camerad`
|
||||
|
||||
Note that both the device and your PC must be on the same openpilot commit.
|
||||
|
||||
Alternatively paste this as a single command:
|
||||
```
|
||||
(
|
||||
cd /data/openpilot/cereal/messaging/
|
||||
./bridge &
|
||||
|
||||
cd /data/openpilot/system/camerad/
|
||||
./camerad &
|
||||
|
||||
cd /data/openpilot/system/loggerd/
|
||||
./encoderd &
|
||||
|
||||
wait
|
||||
) ; trap 'kill $(jobs -p)' SIGINT
|
||||
```
|
||||
Ctrl+C will stop all three processes.
|
||||
|
||||
### On the PC
|
||||
Decode the stream with `compressed_vipc.py`:
|
||||
|
||||
```cd ~/openpilot/tools/camerastream && ./compressed_vipc.py <ip>```
|
||||
|
||||
To actually display the stream, run `watch3` in separate terminal:
|
||||
|
||||
```cd ~/openpilot/selfdrive/ui/ && ./watch3.py```
|
||||
|
||||
## compressed_vipc.py usage
|
||||
```
|
||||
$ python3 compressed_vipc.py -h
|
||||
usage: compressed_vipc.py [-h] [--nvidia] [--cams CAMS] [--silent] addr
|
||||
|
||||
Decode video streams and broadcast on VisionIPC
|
||||
|
||||
positional arguments:
|
||||
addr Address of comma three
|
||||
|
||||
options:
|
||||
-h, --help show this help message and exit
|
||||
--nvidia Use nvidia instead of ffmpeg
|
||||
--cams CAMS Cameras to decode
|
||||
--silent Suppress debug output
|
||||
```
|
||||
|
||||
|
||||
## Example:
|
||||
```
|
||||
cd ~/openpilot/tools/camerastream && ./compressed_vipc.py comma-ffffffff --cams 0
|
||||
cd ~/openpilot/selfdrive/ui/ && ./watch3.py
|
||||
```
|
||||
163
tools/camerastream/compressed_vipc.py
Executable file
163
tools/camerastream/compressed_vipc.py
Executable file
@@ -0,0 +1,163 @@
|
||||
#!/usr/bin/env python3
|
||||
import av
|
||||
import os
|
||||
import sys
|
||||
import argparse
|
||||
import numpy as np
|
||||
import multiprocessing
|
||||
import time
|
||||
import signal
|
||||
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from msgq.visionipc import VisionIpcServer, VisionStreamType
|
||||
|
||||
V4L2_BUF_FLAG_KEYFRAME = 8
|
||||
|
||||
# start encoderd
|
||||
# also start cereal messaging bridge
|
||||
# then run this "./compressed_vipc.py <ip>"
|
||||
|
||||
ENCODE_SOCKETS = {
|
||||
VisionStreamType.VISION_STREAM_ROAD: "roadEncodeData",
|
||||
VisionStreamType.VISION_STREAM_DRIVER: "driverEncodeData",
|
||||
VisionStreamType.VISION_STREAM_WIDE_ROAD: "wideRoadEncodeData",
|
||||
}
|
||||
|
||||
def decoder(addr, vipc_server, vst, nvidia, W, H, debug=False):
|
||||
sock_name = ENCODE_SOCKETS[vst]
|
||||
if debug:
|
||||
print(f"start decoder for {sock_name}, {W}x{H}")
|
||||
|
||||
if nvidia:
|
||||
os.environ["NV_LOW_LATENCY"] = "3" # both bLowLatency and CUVID_PKT_ENDOFPICTURE
|
||||
sys.path += os.environ["LD_LIBRARY_PATH"].split(":")
|
||||
import PyNvCodec as nvc
|
||||
|
||||
nvDec = nvc.PyNvDecoder(W, H, nvc.PixelFormat.NV12, nvc.CudaVideoCodec.HEVC, 0)
|
||||
cc1 = nvc.ColorspaceConversionContext(nvc.ColorSpace.BT_709, nvc.ColorRange.JPEG)
|
||||
conv_yuv = nvc.PySurfaceConverter(W, H, nvc.PixelFormat.NV12, nvc.PixelFormat.YUV420, 0)
|
||||
nvDwn_yuv = nvc.PySurfaceDownloader(W, H, nvc.PixelFormat.YUV420, 0)
|
||||
img_yuv = np.ndarray((H*W//2*3), dtype=np.uint8)
|
||||
else:
|
||||
codec = av.CodecContext.create("hevc", "r")
|
||||
|
||||
os.environ["ZMQ"] = "1"
|
||||
messaging.reset_context()
|
||||
sock = messaging.sub_sock(sock_name, None, addr=addr, conflate=False)
|
||||
cnt = 0
|
||||
last_idx = -1
|
||||
seen_iframe = False
|
||||
|
||||
time_q = []
|
||||
while 1:
|
||||
msgs = messaging.drain_sock(sock, wait_for_one=True)
|
||||
for evt in msgs:
|
||||
evta = getattr(evt, evt.which())
|
||||
if debug and evta.idx.encodeId != 0 and evta.idx.encodeId != (last_idx+1):
|
||||
print("DROP PACKET!")
|
||||
last_idx = evta.idx.encodeId
|
||||
if not seen_iframe and not (evta.idx.flags & V4L2_BUF_FLAG_KEYFRAME):
|
||||
if debug:
|
||||
print("waiting for iframe")
|
||||
continue
|
||||
time_q.append(time.monotonic())
|
||||
network_latency = (int(time.time()*1e9) - evta.unixTimestampNanos)/1e6 # noqa: TID251
|
||||
frame_latency = ((evta.idx.timestampEof/1e9) - (evta.idx.timestampSof/1e9))*1000
|
||||
process_latency = ((evt.logMonoTime/1e9) - (evta.idx.timestampEof/1e9))*1000
|
||||
|
||||
# put in header (first)
|
||||
if not seen_iframe:
|
||||
if nvidia:
|
||||
nvDec.DecodeSurfaceFromPacket(np.frombuffer(evta.header, dtype=np.uint8))
|
||||
else:
|
||||
codec.decode(av.packet.Packet(evta.header))
|
||||
seen_iframe = True
|
||||
|
||||
if nvidia:
|
||||
rawSurface = nvDec.DecodeSurfaceFromPacket(np.frombuffer(evta.data, dtype=np.uint8))
|
||||
if rawSurface.Empty():
|
||||
if debug:
|
||||
print("DROP SURFACE")
|
||||
continue
|
||||
convSurface = conv_yuv.Execute(rawSurface, cc1)
|
||||
nvDwn_yuv.DownloadSingleSurface(convSurface, img_yuv)
|
||||
else:
|
||||
frames = codec.decode(av.packet.Packet(evta.data))
|
||||
if len(frames) == 0:
|
||||
if debug:
|
||||
print("DROP SURFACE")
|
||||
continue
|
||||
assert len(frames) == 1
|
||||
img_yuv = frames[0].to_ndarray(format=av.video.format.VideoFormat('yuv420p')).flatten()
|
||||
uv_offset = H*W
|
||||
y = img_yuv[:uv_offset]
|
||||
uv = img_yuv[uv_offset:].reshape(2, -1).ravel('F')
|
||||
img_yuv = np.hstack((y, uv))
|
||||
|
||||
vipc_server.send(vst, img_yuv.data, cnt, int(time_q[0]*1e9), int(time.monotonic()*1e9))
|
||||
cnt += 1
|
||||
|
||||
pc_latency = (time.monotonic()-time_q[0])*1000
|
||||
time_q = time_q[1:]
|
||||
if debug:
|
||||
print(f"{len(msgs):2d} {evta.idx.encodeId:4d} {evt.logMonoTime/1e9:.3f} {evta.idx.timestampEof/1e6:.3f} \
|
||||
roll {frame_latency:6.2f} ms latency {process_latency:6.2f} ms + {network_latency:6.2f} ms + {pc_latency:6.2f} ms \
|
||||
= {process_latency+network_latency+pc_latency:6.2f} ms", len(evta.data), sock_name)
|
||||
|
||||
|
||||
class CompressedVipc:
|
||||
def __init__(self, addr, vision_streams, server_name, nvidia=False, debug=False):
|
||||
print("getting frame sizes")
|
||||
os.environ["ZMQ"] = "1"
|
||||
messaging.reset_context()
|
||||
sm = messaging.SubMaster([ENCODE_SOCKETS[s] for s in vision_streams], addr=addr)
|
||||
while min(sm.recv_frame.values()) == 0:
|
||||
sm.update(100)
|
||||
os.environ.pop("ZMQ")
|
||||
messaging.reset_context()
|
||||
|
||||
self.vipc_server = VisionIpcServer(server_name)
|
||||
for vst in vision_streams:
|
||||
ed = sm[ENCODE_SOCKETS[vst]]
|
||||
self.vipc_server.create_buffers(vst, 4, ed.width, ed.height)
|
||||
self.vipc_server.start_listener()
|
||||
|
||||
self.procs = []
|
||||
for vst in vision_streams:
|
||||
ed = sm[ENCODE_SOCKETS[vst]]
|
||||
p = multiprocessing.Process(target=decoder, args=(addr, self.vipc_server, vst, nvidia, ed.width, ed.height, debug))
|
||||
p.start()
|
||||
self.procs.append(p)
|
||||
|
||||
def join(self):
|
||||
for p in self.procs:
|
||||
p.join()
|
||||
|
||||
def kill(self):
|
||||
for p in self.procs:
|
||||
p.terminate()
|
||||
self.join()
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser(description="Decode video streams and broadcast on VisionIPC")
|
||||
parser.add_argument("addr", help="Address of comma three")
|
||||
parser.add_argument("--nvidia", action="store_true", help="Use nvidia instead of ffmpeg")
|
||||
parser.add_argument("--cams", default="0,1,2", help="Cameras to decode")
|
||||
parser.add_argument("--server", default="camerad", help="choose vipc server name")
|
||||
parser.add_argument("--silent", action="store_true", help="Suppress debug output")
|
||||
args = parser.parse_args()
|
||||
|
||||
vision_streams = [
|
||||
VisionStreamType.VISION_STREAM_ROAD,
|
||||
VisionStreamType.VISION_STREAM_DRIVER,
|
||||
VisionStreamType.VISION_STREAM_WIDE_ROAD,
|
||||
]
|
||||
|
||||
vsts = [vision_streams[int(x)] for x in args.cams.split(",")]
|
||||
cvipc = CompressedVipc(args.addr, vsts, args.server, args.nvidia, debug=(not args.silent))
|
||||
|
||||
# register exit handler
|
||||
signal.signal(signal.SIGINT, lambda sig, frame: cvipc.kill())
|
||||
|
||||
cvipc.join()
|
||||
129
tools/car_porting/README.md
Normal file
129
tools/car_porting/README.md
Normal file
@@ -0,0 +1,129 @@
|
||||
# tools/car_porting
|
||||
|
||||
Check out [this blog post](https://blog.comma.ai/how-to-write-a-car-port-for-openpilot/) for a high-level overview of porting a car.
|
||||
|
||||
## Useful car porting utilities
|
||||
|
||||
Testing car ports in your car is very time-consuming. Check out these utilities to do basic checks on your work before running it in your car.
|
||||
|
||||
### [Cabana](/tools/cabana/README.md)
|
||||
|
||||
View your car's CAN signals through DBC files, which openpilot uses to parse and create messages that talk to the car.
|
||||
|
||||
Example:
|
||||
```bash
|
||||
> tools/cabana/cabana '1bbe6bf2d62f58a8|2022-07-14--17-11-43'
|
||||
```
|
||||
|
||||
### [tools/car_porting/auto_fingerprint.py](/tools/car_porting/auto_fingerprint.py)
|
||||
|
||||
Given a route and platform, automatically inserts FW fingerprints from the platform into the correct place in fingerprints.py
|
||||
|
||||
Example:
|
||||
```bash
|
||||
> python3 tools/car_porting/auto_fingerprint.py '1bbe6bf2d62f58a8|2022-07-14--17-11-43' 'OUTBACK'
|
||||
Attempting to add fw version for: OUTBACK
|
||||
```
|
||||
|
||||
### [selfdrive/car/tests/test_car_interfaces.py](/selfdrive/car/tests/test_car_interfaces.py)
|
||||
|
||||
Finds common bugs for car interfaces, without even requiring a route.
|
||||
|
||||
|
||||
#### Example: Typo in signal name
|
||||
```bash
|
||||
> pytest selfdrive/car/tests/test_car_interfaces.py -k subaru # replace with the brand you are working on
|
||||
|
||||
=====================================================================
|
||||
FAILED selfdrive/car/tests/test_car_interfaces.py::TestCarInterfaces::test_car_interfaces_165_SUBARU_LEGACY_7TH_GEN - KeyError: 'CruiseControlOOPS'
|
||||
|
||||
```
|
||||
|
||||
### [tools/car_porting/test_car_model.py](/tools/car_porting/test_car_model.py)
|
||||
|
||||
Given a route, runs most of the car interface to check for common errors like missing signals, blocked panda messages, and safety mismatches.
|
||||
|
||||
#### Example: panda safety mismatch for gasPressed
|
||||
```bash
|
||||
> python3 tools/car_porting/test_car_model.py '4822a427b188122a|2023-08-14--16-22-21'
|
||||
|
||||
=====================================================================
|
||||
FAIL: test_panda_safety_carstate (__main__.CarModelTestCase.test_panda_safety_carstate)
|
||||
Assert that panda safety matches openpilot's carState
|
||||
----------------------------------------------------------------------
|
||||
Traceback (most recent call last):
|
||||
File "/home/batman/xx/openpilot/openpilot/selfdrive/car/tests/test_models.py", line 380, in test_panda_safety_carstate
|
||||
self.assertFalse(len(failed_checks), f"panda safety doesn't agree with openpilot: {failed_checks}")
|
||||
AssertionError: 1 is not false : panda safety doesn't agree with openpilot: {'gasPressed': 116}
|
||||
```
|
||||
|
||||
## Jupyter notebooks
|
||||
|
||||
To use these notebooks, install Jupyter within your [openpilot virtual environment](/tools/README.md).
|
||||
|
||||
```bash
|
||||
uv pip install jupyter ipykernel
|
||||
```
|
||||
|
||||
Launching:
|
||||
|
||||
```bash
|
||||
jupyter notebook
|
||||
```
|
||||
|
||||
### [examples/subaru_steer_temp_fault.ipynb](/tools/car_porting/examples/subaru_steer_temp_fault.ipynb)
|
||||
|
||||
An example of searching through a database of segments for a specific condition, and plotting the results.
|
||||
|
||||
|
||||
|
||||
*a plot of the steer_warning vs steering angle, where we can see it is clearly caused by a large steering angle change*
|
||||
|
||||
### [examples/subaru_long_accel.ipynb](/tools/car_porting/examples/subaru_long_accel.ipynb)
|
||||
|
||||
An example of plotting the response of an actuator when it is active.
|
||||
|
||||
|
||||
|
||||
*a plot of the brake_pressure vs acceleration, where we can see it is a fairly linear response.*
|
||||
|
||||
### [examples/ford_vin_fingerprint.ipynb](/tools/car_porting/examples/ford_vin_fingerprint.ipynb)
|
||||
|
||||
In this example, we use the public comma car segments database to check if vin fingerprinting is feasible for ford.
|
||||
|
||||
```
|
||||
vin: 1FM5K8GC7LGXXXXXX real platform: FORD EXPLORER 6TH GEN determined platform: mock correct: False
|
||||
vin: 00000000000XXXXXX real platform: FORD ESCAPE 4TH GEN determined platform: mock correct: False
|
||||
vin: 3FTTW8F98NRXXXXXX real platform: FORD MAVERICK 1ST GEN determined platform: mock correct: False
|
||||
vin: 1FTVW1EL4NWXXXXXX real platform: FORD F-150 LIGHTNING 1ST GEN determined platform: FORD F-150 LIGHTNING 1ST GEN correct: True
|
||||
vin: 1FM5K7LC0MGXXXXXX real platform: FORD EXPLORER 6TH GEN determined platform: mock correct: False
|
||||
vin: WF0NXXGCHNJXXXXXX real platform: FORD FOCUS 4TH GEN determined platform: mock correct: False
|
||||
vin: 1FMCU9J94MUXXXXXX real platform: FORD ESCAPE 4TH GEN determined platform: mock correct: False
|
||||
vin: 5LM5J7XC9LGXXXXXX real platform: FORD EXPLORER 6TH GEN determined platform: mock correct: False
|
||||
vin: 3FMCR9B69NRXXXXXX real platform: FORD BRONCO SPORT 1ST GEN determined platform: mock correct: False
|
||||
vin: 3FMTK3SU0MMXXXXXX real platform: FORD MUSTANG MACH-E 1ST GEN determined platform: FORD MUSTANG MACH-E 1ST GEN correct: True
|
||||
vin: 1FM5K8HC7MGXXXXXX real platform: FORD EXPLORER 6TH GEN determined platform: mock correct: False
|
||||
vin: 1FM5K8GC7NGXXXXXX real platform: FORD EXPLORER 6TH GEN determined platform: mock correct: False
|
||||
vin: 5LM5J7XC8MGXXXXXX real platform: FORD EXPLORER 6TH GEN determined platform: mock correct: False
|
||||
vin: 3FTTW8E31PRXXXXXX real platform: FORD MAVERICK 1ST GEN determined platform: mock correct: False
|
||||
vin: 3FTTW8E99NRXXXXXX real platform: FORD MAVERICK 1ST GEN determined platform: mock correct: False
|
||||
```
|
||||
|
||||
### [examples/find_segments_with_message.ipynb](/tools/car_porting/examples/find_segments_with_message.ipynb)
|
||||
|
||||
Searches for segments where a set of given CAN message IDs are present. In the example, we search for all messages
|
||||
used for CAN-based ignition detection.
|
||||
|
||||
```
|
||||
Match found: 46b21f1c5f7aa885/2024-01-23--15-19-34/20/s JEEP GRAND CHEROKEE V6 2018 ['VW CAN Ign']
|
||||
Match found: a63a23c3e628f288/2023-11-05--18-36-20/8/s JEEP GRAND CHEROKEE V6 2018 ['VW CAN Ign']
|
||||
Match found: ce31b7a998781ba8/2024-01-19--07-05-29/23/s JEEP GRAND CHEROKEE 2019 ['VW CAN Ign']
|
||||
Match found: e1dfba62a4e33f7b/2023-12-25--19-31-00/4/s JEEP GRAND CHEROKEE 2019 ['VW CAN Ign']
|
||||
Match found: e1dfba62a4e33f7b/2024-01-10--14-33-57/2/s JEEP GRAND CHEROKEE 2019 ['VW CAN Ign']
|
||||
Match found: ae679616266f4096/2023-12-05--15-43-46/4/s RAM HD 5TH GEN ['Tesla 3/Y CAN Ign']
|
||||
Match found: ae679616266f4096/2023-11-18--17-49-42/3/s RAM HD 5TH GEN ['Tesla 3/Y CAN Ign']
|
||||
Match found: ae679616266f4096/2024-01-03--21-57-09/25/s RAM HD 5TH GEN ['Tesla 3/Y CAN Ign']
|
||||
Match found: 6dae2984cc53cd7f/2023-12-10--11-53-15/17/s FORD BRONCO SPORT 1ST GEN ['Rivian CAN Ign']
|
||||
Match found: 6dae2984cc53cd7f/2023-12-03--17-31-17/29/s FORD BRONCO SPORT 1ST GEN ['Rivian CAN Ign']
|
||||
Match found: 6dae2984cc53cd7f/2023-11-27--23-29-07/1/s FORD BRONCO SPORT 1ST GEN ['Rivian CAN Ign']
|
||||
```
|
||||
45
tools/car_porting/auto_fingerprint.py
Executable file
45
tools/car_porting/auto_fingerprint.py
Executable file
@@ -0,0 +1,45 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import argparse
|
||||
from collections import defaultdict
|
||||
from iqdbc.car.debug.format_fingerprints import format_brand_fw_versions
|
||||
|
||||
from iqdbc.car.fingerprints import MIGRATION
|
||||
from iqdbc.car.fw_versions import MODEL_TO_BRAND, match_fw_to_car
|
||||
from openpilot.tools.lib.logreader import LogReader, ReadMode
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser(description="Auto fingerprint from a route")
|
||||
parser.add_argument("route", help="The route name to use")
|
||||
parser.add_argument("platform", help="The platform, or leave empty to auto-determine using fuzzy", default=None, nargs="?")
|
||||
args = parser.parse_args()
|
||||
|
||||
lr = LogReader(args.route, ReadMode.QLOG)
|
||||
CP = lr.first("carParams")
|
||||
assert CP is not None, "No carParams in route"
|
||||
|
||||
carPlatform = MIGRATION.get(CP.carFingerprint, CP.carFingerprint)
|
||||
|
||||
if args.platform is not None:
|
||||
platform = args.platform
|
||||
elif carPlatform != "MOCK":
|
||||
platform = carPlatform
|
||||
else:
|
||||
_, matches = match_fw_to_car(CP.carFw, CP.carVin, log=False)
|
||||
assert len(matches) == 1, f"Unable to auto-determine platform, matches: {matches}"
|
||||
platform = list(matches)[0]
|
||||
|
||||
print("Attempting to add fw version for:", platform)
|
||||
|
||||
fw_versions: dict[str, dict[tuple, list[bytes]]] = defaultdict(lambda: defaultdict(list))
|
||||
brand = MODEL_TO_BRAND[platform]
|
||||
|
||||
for fw in CP.carFw:
|
||||
if fw.brand == brand and not fw.logging:
|
||||
addr = fw.address
|
||||
subAddr = None if fw.subAddress == 0 else fw.subAddress
|
||||
key = (fw.ecu.raw, addr, subAddr)
|
||||
|
||||
fw_versions[platform][key].append(fw.fwVersion)
|
||||
|
||||
format_brand_fw_versions(brand, fw_versions)
|
||||
232
tools/car_porting/examples/find_segments_with_message.ipynb
Normal file
232
tools/car_porting/examples/find_segments_with_message.ipynb
Normal file
@@ -0,0 +1,232 @@
|
||||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 85,
|
||||
"id": "facb8edc-9924-491a-a4dd-fe6135b0c6c4",
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"# Import all cars from iqdbc\n",
|
||||
"\n",
|
||||
"from iqdbc.car import structs\n",
|
||||
"from iqdbc.car.values import PLATFORMS as TEST_PLATFORMS\n",
|
||||
"\n",
|
||||
"# Example: add additional platforms/segments to test outside of commaCarSegments\n",
|
||||
"\n",
|
||||
"EXTRA_SEGMENTS = {\n",
|
||||
" # \"81dd9e9fe256c397/0000001f--97c42cf98d\", # Volkswagen ID.4 test route, new car port, not in public dataset\n",
|
||||
"}"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 86,
|
||||
"id": "ed1c8aec-c274-4c61-b83d-711ea194bf86",
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"Searching 221 platforms\n",
|
||||
"No segments available for DODGE_DURANGO\n",
|
||||
"No segments available for FORD_RANGER_MK2\n",
|
||||
"No segments available for HOLDEN_ASTRA\n",
|
||||
"No segments available for CADILLAC_ATS\n",
|
||||
"No segments available for CHEVROLET_MALIBU\n",
|
||||
"No segments available for CADILLAC_XT4\n",
|
||||
"No segments available for CHEVROLET_VOLT_2019\n",
|
||||
"No segments available for CHEVROLET_TRAVERSE\n",
|
||||
"No segments available for GMC_YUKON\n",
|
||||
"No segments available for HONDA_ODYSSEY_CHN\n",
|
||||
"No segments available for HYUNDAI_KONA_2022\n",
|
||||
"No segments available for HYUNDAI_NEXO_1ST_GEN\n",
|
||||
"No segments available for GENESIS_GV70_ELECTRIFIED_1ST_GEN\n",
|
||||
"No segments available for GENESIS_G80_2ND_GEN_FL\n",
|
||||
"No segments available for RIVIAN_R1_GEN1\n",
|
||||
"No segments available for SUBARU_FORESTER_HYBRID\n",
|
||||
"No segments available for TESLA_MODEL_3\n",
|
||||
"No segments available for TESLA_MODEL_Y\n",
|
||||
"No segments available for TOYOTA_RAV4_PRIME\n",
|
||||
"No segments available for TOYOTA_SIENNA_4TH_GEN\n",
|
||||
"No segments available for LEXUS_LC_TSS2\n",
|
||||
"No segments available for VOLKSWAGEN_CADDY_MK3\n",
|
||||
"No segments available for VOLKSWAGEN_CRAFTER_MK2\n",
|
||||
"No segments available for VOLKSWAGEN_JETTA_MK6\n",
|
||||
"Searching 577 segments\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"import random\n",
|
||||
"\n",
|
||||
"from openpilot.tools.lib.logreader import LogReader\n",
|
||||
"from openpilot.tools.lib.comma_car_segments import get_comma_car_segments_database\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"MAX_SEGS_PER_PLATFORM = 3 # Increase this to search more segments\n",
|
||||
"\n",
|
||||
"database = get_comma_car_segments_database()\n",
|
||||
"TEST_SEGMENTS = []\n",
|
||||
"\n",
|
||||
"print(f\"Searching {len(TEST_PLATFORMS)} platforms\")\n",
|
||||
"\n",
|
||||
"for platform in TEST_PLATFORMS:\n",
|
||||
" if platform not in database:\n",
|
||||
" print(f\"No segments available for {platform}\")\n",
|
||||
" continue\n",
|
||||
"\n",
|
||||
" all_segments = database[platform]\n",
|
||||
" NUM_SEGMENTS = min(len(all_segments), MAX_SEGS_PER_PLATFORM)\n",
|
||||
" TEST_SEGMENTS.extend(random.sample(all_segments, NUM_SEGMENTS))\n",
|
||||
"\n",
|
||||
"TEST_SEGMENTS.extend(EXTRA_SEGMENTS)\n",
|
||||
"\n",
|
||||
"print(f\"Searching {len(TEST_SEGMENTS)} segments\")"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"id": "0c75e8f2-4f5f-4f89-b8db-5223a6534a9f",
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"data": {
|
||||
"application/vnd.jupyter.widget-view+json": {
|
||||
"model_id": "27a243c33de44498b2b946190df44b23",
|
||||
"version_major": 2,
|
||||
"version_minor": 0
|
||||
},
|
||||
"text/plain": [
|
||||
"segments searched: 0%| | 0/577 [00:00<?, ?it/s]"
|
||||
]
|
||||
},
|
||||
"metadata": {},
|
||||
"output_type": "display_data"
|
||||
},
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"Match found: 0f53b336851e1384/2023-11-20--09-44-03/12/s CHRYSLER PACIFICA HYBRID 2018 ['VW CAN Ign']\n",
|
||||
"Match found: 7620ad20d3cefc64/2023-10-28--08-14-40/3/s CHRYSLER PACIFICA HYBRID 2018 ['VW CAN Ign']\n",
|
||||
"Match found: 00d247a9bb1f9196/2023-11-06--13-33-17/9/s CHRYSLER PACIFICA HYBRID 2018 ['VW CAN Ign']\n",
|
||||
"Match found: 120a432f63cb0de2/2023-10-30--20-01-34/1/s CHRYSLER PACIFICA HYBRID 2019 ['VW CAN Ign']\n",
|
||||
"Match found: b70b56b76a6217f2/2023-12-19--08-30-22/35/s CHRYSLER PACIFICA HYBRID 2019 ['VW CAN Ign']\n",
|
||||
"Match found: 97e388680a6716ed/2024-01-17--10-15-13/9/s CHRYSLER PACIFICA HYBRID 2019 ['VW CAN Ign']\n",
|
||||
"Match found: 2137b01aa0ca63f9/2024-01-06--22-06-14/70/s CHRYSLER PACIFICA 2018 ['VW CAN Ign']\n",
|
||||
"Match found: 8fc6a1b72c8b1357/2023-11-06--07-50-05/8/s CHRYSLER PACIFICA 2018 ['VW CAN Ign']\n",
|
||||
"Match found: 7e705eb5c27a49cc/2024-01-18--16-51-20/3/s CHRYSLER PACIFICA 2018 ['VW CAN Ign']\n",
|
||||
"Match found: 12208e5acdc97eb3/2024-01-20--14-46-24/12/s CHRYSLER PACIFICA 2020 ['VW CAN Ign']\n",
|
||||
"Match found: 12208e5acdc97eb3/2023-11-30--12-01-09/2/s CHRYSLER PACIFICA 2020 ['VW CAN Ign']\n",
|
||||
"Match found: 9cad19e0efce3650/2024-01-26--10-24-52/27/s CHRYSLER PACIFICA 2020 ['VW CAN Ign']\n",
|
||||
"Match found: 9db428338427dec2/2023-11-05--18-40-09/21/s JEEP GRAND CHEROKEE V6 2018 ['VW CAN Ign']\n",
|
||||
"Match found: d50ada8ee55a5e74/2023-12-11--13-38-09/0/s JEEP GRAND CHEROKEE V6 2018 ['VW CAN Ign']\n",
|
||||
"Match found: 900dfa83b4addfe6/2023-12-30--19-20-08/28/s JEEP GRAND CHEROKEE V6 2018 ['VW CAN Ign']\n",
|
||||
"Match found: 20acda0eb23d7f23/2024-01-19--17-33-26/41/s JEEP GRAND CHEROKEE 2019 ['VW CAN Ign']\n",
|
||||
"Match found: 1cc3b46843cad2ca/2024-01-10--20-20-54/24/s JEEP GRAND CHEROKEE 2019 ['VW CAN Ign']\n",
|
||||
"Match found: 2d9b6425552c52c1/2023-12-07--10-31-46/22/s JEEP GRAND CHEROKEE 2019 ['VW CAN Ign']\n",
|
||||
"Match found: ae679616266f4096/2023-12-04--13-13-56/16/s RAM HD 5TH GEN ['Tesla 3/Y CAN Ign']\n",
|
||||
"Match found: ae679616266f4096/2024-01-08--07-58-12/65/s RAM HD 5TH GEN ['Tesla 3/Y CAN Ign']\n",
|
||||
"Match found: ae679616266f4096/2023-12-05--15-43-46/25/s RAM HD 5TH GEN ['Tesla 3/Y CAN Ign']\n",
|
||||
"Match found: 6dae2984cc53cd7f/2024-01-09--21-41-11/4/s FORD BRONCO SPORT 1ST GEN ['Rivian CAN Ign']\n",
|
||||
"Match found: 440a155809ba2b6d/2023-12-30--08-51-53/2/s FORD BRONCO SPORT 1ST GEN ['Rivian CAN Ign']\n",
|
||||
"Match found: 6dae2984cc53cd7f/2024-01-06--10-11-07/1/s FORD BRONCO SPORT 1ST GEN ['Rivian CAN Ign']\n",
|
||||
"Match found: a4218e6416dfd978/2023-11-27--13-48-46/19/s FORD ESCAPE 4TH GEN ['Rivian CAN Ign']\n",
|
||||
"Match found: a4218e6416dfd978/2023-11-10--14-13-14/0/s FORD ESCAPE 4TH GEN ['Rivian CAN Ign']\n",
|
||||
"Match found: a4218e6416dfd978/2023-11-27--13-48-46/4/s FORD ESCAPE 4TH GEN ['Rivian CAN Ign']\n",
|
||||
"Match found: 8a732841c3a8d5ef/2023-12-10--19-02-33/3/s FORD EXPLORER 6TH GEN ['Rivian CAN Ign']\n",
|
||||
"Match found: 0b91b433b9332780/2023-12-28--14-02-49/4/s FORD EXPLORER 6TH GEN ['Rivian CAN Ign']\n",
|
||||
"Match found: 8a732841c3a8d5ef/2023-11-09--07-28-12/1/s FORD EXPLORER 6TH GEN ['Rivian CAN Ign']\n",
|
||||
"Match found: e886087f430e7fe7/2023-11-05--19-59-40/59/s FORD FOCUS 4TH GEN ['Rivian CAN Ign']\n",
|
||||
"Match found: e886087f430e7fe7/2023-11-05--19-59-40/82/s FORD FOCUS 4TH GEN ['Rivian CAN Ign']\n",
|
||||
"Match found: e886087f430e7fe7/2023-11-05--19-59-40/106/s FORD FOCUS 4TH GEN ['Rivian CAN Ign']\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"from openpilot.tools.lib.logreader import LogReader, comma_car_segments_source\n",
|
||||
"from tqdm.notebook import tqdm, tnrange\n",
|
||||
"\n",
|
||||
"# Example search for CAN ignition messages\n",
|
||||
"# Be careful when filtering by bus, account for odd harness arrangements on Honda/HKG\n",
|
||||
"\n",
|
||||
"BUSES_TO_SEARCH = [0, 1, 2]\n",
|
||||
"\n",
|
||||
"# Support for external Red Panda\n",
|
||||
"EXTERNAL_PANDA_BUSES = [bus + 4 for bus in BUSES_TO_SEARCH]\n",
|
||||
"\n",
|
||||
"MESSAGES_TO_FIND = {\n",
|
||||
" 0x1F1: \"GM CAN Ign\",\n",
|
||||
" 0x152: \"Rivian CAN Ign\",\n",
|
||||
" 0x221: \"Tesla 3/Y CAN Ign\",\n",
|
||||
" 0x9E: \"Mazda CAN Ign\",\n",
|
||||
" 0x3C0: \"VW CAN Ign\",\n",
|
||||
"}\n",
|
||||
"\n",
|
||||
"progress_bar = tnrange(len(TEST_SEGMENTS), desc=\"segments searched\")\n",
|
||||
"\n",
|
||||
"for segment in TEST_SEGMENTS:\n",
|
||||
" lr = LogReader(segment, sources=[comma_car_segments_source])\n",
|
||||
" CP = lr.first(\"carParams\")\n",
|
||||
" if CP is None:\n",
|
||||
" progress_bar.update()\n",
|
||||
" continue\n",
|
||||
"\n",
|
||||
" can_packets = [msg for msg in lr if msg.which() == \"can\"]\n",
|
||||
" matched_messages = set()\n",
|
||||
"\n",
|
||||
" for packet in can_packets:\n",
|
||||
" for msg in packet.can:\n",
|
||||
" if msg.address in MESSAGES_TO_FIND and msg.src in (BUSES_TO_SEARCH + EXTERNAL_PANDA_BUSES):\n",
|
||||
" # print(msg)\n",
|
||||
" matched_messages.add(msg.address)\n",
|
||||
"\n",
|
||||
" if len(matched_messages) > 0:\n",
|
||||
" message_names = [MESSAGES_TO_FIND[message] for message in matched_messages]\n",
|
||||
" print(f\"Match found: {segment:<45} {CP.carFingerprint:<38} {message_names}\")\n",
|
||||
"\n",
|
||||
" progress_bar.update()\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"id": "7724dd97-f62e-4fd3-9f64-63d49be669d2",
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": []
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"id": "9f393e00-8efd-40fb-a41e-d312531a83e8",
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": []
|
||||
}
|
||||
],
|
||||
"metadata": {
|
||||
"kernelspec": {
|
||||
"display_name": "Python 3 (ipykernel)",
|
||||
"language": "python",
|
||||
"name": "python3"
|
||||
},
|
||||
"language_info": {
|
||||
"codemirror_mode": {
|
||||
"name": "ipython",
|
||||
"version": 3
|
||||
},
|
||||
"file_extension": ".py",
|
||||
"mimetype": "text/x-python",
|
||||
"name": "python",
|
||||
"nbconvert_exporter": "python",
|
||||
"pygments_lexer": "ipython3",
|
||||
"version": "3.12.0"
|
||||
}
|
||||
},
|
||||
"nbformat": 4,
|
||||
"nbformat_minor": 5
|
||||
}
|
||||
175
tools/car_porting/examples/ford_vin_fingerprint.ipynb
Normal file
175
tools/car_porting/examples/ford_vin_fingerprint.ipynb
Normal file
@@ -0,0 +1,175 @@
|
||||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 12,
|
||||
"metadata": {
|
||||
"jupyter": {
|
||||
"is_executing": true
|
||||
}
|
||||
},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"Got 9 Ford cars from iqdbc\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"\"\"\"In this example, we use the public comma car segments database to check if vin fingerprinting is feasible for ford.\"\"\"\n",
|
||||
"\n",
|
||||
"from openpilot.tools.lib.logreader import LogReader, comma_car_segments_source\n",
|
||||
"from openpilot.tools.lib.comma_car_segments import get_comma_car_segments_database\n",
|
||||
"from iqdbc.car.ford.values import CAR\n",
|
||||
"\n",
|
||||
"database = get_comma_car_segments_database()\n",
|
||||
"\n",
|
||||
"platforms = [c.value for c in CAR]\n",
|
||||
"print(f\"Got {len(platforms)} Ford cars from iqdbc\")"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 6,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"# Adapted from https://github.com/commaai/openpilot/issues/31052#issuecomment-1902690083\n",
|
||||
"\n",
|
||||
"MODEL_YEAR_CODES = {'M': 2021, 'N': 2022, 'P': 2023, 'R': 2024, 'S': 2025}\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"F150_CODES = ['F1C', 'F1E', 'W1C', 'W1E', 'X1C', 'X1E', 'W1R', 'W1P', 'W1S', 'W1T']\n",
|
||||
"LIGHTNING_CODES = ['L', 'V']\n",
|
||||
"MACHE_CODES = ['K1R', 'K1S', 'K2S', 'K3R', 'K3S', 'K4S']\n",
|
||||
"\n",
|
||||
"FORD_VIN_START = ['1FT', '3FM', '5LM']\n",
|
||||
"\n",
|
||||
"def ford_vin_fingerprint(vin): # Check if it's a Ford vehicle and determine the model\n",
|
||||
" vin_positions_567 = vin[4:7]\n",
|
||||
"\n",
|
||||
" if vin.startswith('1FT'):\n",
|
||||
" if vin_positions_567 in F150_CODES:\n",
|
||||
" if vin[7] in LIGHTNING_CODES:\n",
|
||||
" return f\"FORD F-150 LIGHTNING 1ST GEN\"\n",
|
||||
" else:\n",
|
||||
" return f\"FORD F-150 14TH GEN\"\n",
|
||||
" elif vin.startswith('3FM'):\n",
|
||||
" if vin_positions_567 in MACHE_CODES:\n",
|
||||
" return f\"FORD MUSTANG MACH-E 1ST GEN\"\n",
|
||||
" elif vin.startswith('5LM'):\n",
|
||||
" pass\n",
|
||||
"\n",
|
||||
" return \"mock\""
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 13,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"Collecting segments from commaCarSegments dataset:\n",
|
||||
"Got 287 segments for platform FORD_BRONCO_SPORT_MK1, sampling 5 segments\n",
|
||||
"Got 137 segments for platform FORD_ESCAPE_MK4, sampling 5 segments\n",
|
||||
"Got 1041 segments for platform FORD_EXPLORER_MK6, sampling 5 segments\n",
|
||||
"Got 5 segments for platform FORD_F_150_MK14, sampling 5 segments\n",
|
||||
"Got 3 segments for platform FORD_F_150_LIGHTNING_MK1, sampling 3 segments\n",
|
||||
"Got 56 segments for platform FORD_FOCUS_MK4, sampling 5 segments\n",
|
||||
"Got 637 segments for platform FORD_MAVERICK_MK1, sampling 5 segments\n",
|
||||
"Got 3 segments for platform FORD_MUSTANG_MACH_E_MK1, sampling 3 segments\n",
|
||||
"Skipping platform: FORD_RANGER_MK2, no data available\n",
|
||||
"Segment collection finished\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"import random\n",
|
||||
"\n",
|
||||
"MAX_SEGS_PER_PLATFORM = 5\n",
|
||||
"\n",
|
||||
"VINS_TO_CHECK = set()\n",
|
||||
"\n",
|
||||
"print(\"Collecting segments from commaCarSegments dataset:\")\n",
|
||||
"for platform in platforms:\n",
|
||||
" if platform not in database:\n",
|
||||
" print(f\"Skipping platform: {platform}, no data available\")\n",
|
||||
" continue\n",
|
||||
"\n",
|
||||
" all_segments = database[platform]\n",
|
||||
"\n",
|
||||
" NUM_SEGMENTS = min(len(all_segments), MAX_SEGS_PER_PLATFORM)\n",
|
||||
"\n",
|
||||
" print(f\"Got {len(all_segments)} segments for platform {platform}, sampling {NUM_SEGMENTS} segments\")\n",
|
||||
"\n",
|
||||
" segments = random.sample(all_segments, NUM_SEGMENTS)\n",
|
||||
"\n",
|
||||
" for segment in segments:\n",
|
||||
" lr = LogReader(segment, sources=[comma_car_segments_source])\n",
|
||||
" CP = lr.first(\"carParams\")\n",
|
||||
" if \"FORD\" not in CP.carFingerprint:\n",
|
||||
" print(segment, CP.carFingerprint)\n",
|
||||
" VINS_TO_CHECK.add((CP.carVin, CP.carFingerprint))\n",
|
||||
"\n",
|
||||
"print(\"Segment collection finished\")"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 16,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"vin: 3FMCR9B69NRXXXXXX real platform: FORD BRONCO SPORT 1ST GEN determined platform: mock correct: False\n",
|
||||
"vin: 00000000000XXXXXX real platform: FORD F-150 14TH GEN determined platform: mock correct: False\n",
|
||||
"vin: 1FMCU9J94MUXXXXXX real platform: FORD ESCAPE 4TH GEN determined platform: mock correct: False\n",
|
||||
"vin: 3FMTK3SU0MMXXXXXX real platform: FORD MUSTANG MACH-E 1ST GEN determined platform: FORD MUSTANG MACH-E 1ST GEN correct: True\n",
|
||||
"vin: 1FM5K8HC7MGXXXXXX real platform: FORD EXPLORER 6TH GEN determined platform: mock correct: False\n",
|
||||
"vin: 5LM5J7XC9LGXXXXXX real platform: FORD EXPLORER 6TH GEN determined platform: mock correct: False\n",
|
||||
"vin: 1FTVW1EL4NWXXXXXX real platform: FORD F-150 LIGHTNING 1ST GEN determined platform: FORD F-150 LIGHTNING 1ST GEN correct: True\n",
|
||||
"vin: WF0NXXGCHNJXXXXXX real platform: FORD FOCUS 4TH GEN determined platform: mock correct: False\n",
|
||||
"vin: 3FTTW8E99NRXXXXXX real platform: FORD MAVERICK 1ST GEN determined platform: mock correct: False\n",
|
||||
"vin: 1FM5K8GC7LGXXXXXX real platform: FORD EXPLORER 6TH GEN determined platform: mock correct: False\n",
|
||||
"vin: 3FTTW8E33NRXXXXXX real platform: FORD MAVERICK 1ST GEN determined platform: mock correct: False\n",
|
||||
"vin: 5LM5J7XC1LGXXXXXX real platform: FORD EXPLORER 6TH GEN determined platform: mock correct: False\n",
|
||||
"vin: 3FTTW8E3XPRXXXXXX real platform: FORD MAVERICK 1ST GEN determined platform: mock correct: False\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"for vin, real_fingerprint in VINS_TO_CHECK:\n",
|
||||
" determined_fingerprint = ford_vin_fingerprint(vin)\n",
|
||||
" print(f\"vin: {vin} real platform: {real_fingerprint: <30} determined platform: {determined_fingerprint: <30} correct: {real_fingerprint == determined_fingerprint}\")"
|
||||
]
|
||||
}
|
||||
],
|
||||
"metadata": {
|
||||
"kernelspec": {
|
||||
"display_name": "Python 3 (ipykernel)",
|
||||
"language": "python",
|
||||
"name": "python3"
|
||||
},
|
||||
"language_info": {
|
||||
"codemirror_mode": {
|
||||
"name": "ipython",
|
||||
"version": 3
|
||||
},
|
||||
"file_extension": ".py",
|
||||
"mimetype": "text/x-python",
|
||||
"name": "python",
|
||||
"nbconvert_exporter": "python",
|
||||
"pygments_lexer": "ipython3",
|
||||
"version": "3.12.3"
|
||||
}
|
||||
},
|
||||
"nbformat": 4,
|
||||
"nbformat_minor": 4
|
||||
}
|
||||
277
tools/car_porting/examples/hkg_canfd_gear_message.ipynb
Normal file
277
tools/car_porting/examples/hkg_canfd_gear_message.ipynb
Normal file
@@ -0,0 +1,277 @@
|
||||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 62,
|
||||
"id": "228a6736-de31-4255-9d72-a6ff391b968d",
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"Found 6 qualifying vehicles:\n",
|
||||
" KIA_EV6\n",
|
||||
" HYUNDAI_KONA_EV_2ND_GEN\n",
|
||||
" HYUNDAI_IONIQ_5\n",
|
||||
" KIA_NIRO_EV_2ND_GEN\n",
|
||||
" HYUNDAI_IONIQ_6\n",
|
||||
" GENESIS_GV60_EV_1ST_GEN\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"from iqdbc.car import structs\n",
|
||||
"from iqdbc.car.hyundai.values import CAR, HyundaiFlags\n",
|
||||
"from iqdbc.car.hyundai.fingerprints import FW_VERSIONS\n",
|
||||
"\n",
|
||||
"TEST_PLATFORMS = set(CAR.with_flags(HyundaiFlags.CANFD)) & set(CAR.with_flags(HyundaiFlags.EV)) # CAN-FD electric vehicles only\n",
|
||||
"#TEST_PLATFORMS = set(CAR.with_flags(HyundaiFlags.CANFD)) - set(CAR.with_flags(HyundaiFlags.EV)) # CAN-FD hybrid and ICE vehicles only\n",
|
||||
"\n",
|
||||
"print(f\"Found {len(TEST_PLATFORMS)} qualifying vehicles:\")\n",
|
||||
"for platform in TEST_PLATFORMS:\n",
|
||||
" print(f\" {platform}\")"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 63,
|
||||
"id": "ed1c8aec-c274-4c61-b83d-711ea194bf86",
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"Collecting segments from commaCarSegments dataset:\n",
|
||||
"Got 1300 segments for platform KIA_EV6, sampling 5 segments\n",
|
||||
"Got 9 segments for platform HYUNDAI_KONA_EV_2ND_GEN, sampling 5 segments\n",
|
||||
"Got 1570 segments for platform HYUNDAI_IONIQ_5, sampling 5 segments\n",
|
||||
"Got 34 segments for platform KIA_NIRO_EV_2ND_GEN, sampling 5 segments\n",
|
||||
"Got 974 segments for platform HYUNDAI_IONIQ_6, sampling 5 segments\n",
|
||||
"Got 157 segments for platform GENESIS_GV60_EV_1ST_GEN, sampling 5 segments\n",
|
||||
"Collected 30 segments for analysis\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"import random\n",
|
||||
"\n",
|
||||
"from openpilot.tools.lib.logreader import LogReader\n",
|
||||
"from openpilot.tools.lib.comma_car_segments import get_comma_car_segments_database\n",
|
||||
"from iqdbc.car.hyundai.values import CAR\n",
|
||||
"\n",
|
||||
"database = get_comma_car_segments_database()\n",
|
||||
"TEST_SEGMENTS = []\n",
|
||||
"\n",
|
||||
"MAX_SEGS_PER_PLATFORM = 5 # TODO: Increase this to search more segments\n",
|
||||
"\n",
|
||||
"print(\"Collecting segments from commaCarSegments dataset:\")\n",
|
||||
"for platform in TEST_PLATFORMS:\n",
|
||||
" assert(platform in database)\n",
|
||||
" #if platform not in database:\n",
|
||||
" # print(f\"Skipping platform: {platform}, no data available\")\n",
|
||||
" # continue\n",
|
||||
"\n",
|
||||
" all_segments = database[platform]\n",
|
||||
"\n",
|
||||
" NUM_SEGMENTS = min(len(all_segments), MAX_SEGS_PER_PLATFORM)\n",
|
||||
"\n",
|
||||
" print(f\"Got {len(all_segments)} segments for platform {platform}, sampling {NUM_SEGMENTS} segments\")\n",
|
||||
"\n",
|
||||
" TEST_SEGMENTS.extend(random.sample(all_segments, NUM_SEGMENTS))\n",
|
||||
"\n",
|
||||
"print(f\"Collected {len(TEST_SEGMENTS)} segments for analysis\")\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 64,
|
||||
"id": "0c75e8f2-4f5f-4f89-b8db-5223a6534a9f",
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"Analyzing segment ff2bd20623fcaeaa/2023-11-26--16-27-04/5/s for KIA EV6 2022\n",
|
||||
" GEAR_SHIFTER gear=4.0\n",
|
||||
" ACCELERATOR gear=5.0\n",
|
||||
"Analyzing segment 3f1a6480f940cf9a/2024-01-10--23-06-11/16/s for KIA EV6 2022\n",
|
||||
" GEAR_SHIFTER gear=4.0\n",
|
||||
" ACCELERATOR gear=5.0\n",
|
||||
"Analyzing segment b0a9998109ed0053/2023-12-15--11-10-18/12/s for KIA EV6 2022\n",
|
||||
" GEAR_SHIFTER gear=4.0\n",
|
||||
" ACCELERATOR gear=5.0\n",
|
||||
"Analyzing segment 6e14aa2ed85025df/2023-11-15--13-18-12/24/s for KIA EV6 2022\n",
|
||||
" GEAR_SHIFTER gear=4.0\n",
|
||||
" ACCELERATOR gear=5.0\n",
|
||||
"Analyzing segment a43f21df3a1ca12d/2024-01-25--08-56-22/16/s for KIA EV6 2022\n",
|
||||
" GEAR_SHIFTER gear=4.0\n",
|
||||
" ACCELERATOR gear=5.0\n",
|
||||
"Analyzing segment 1618132d68afc876/2023-12-05--13-49-24/11/s for HYUNDAI KONA ELECTRIC 2ND GEN\n",
|
||||
" GEAR_SHIFTER gear=4.0\n",
|
||||
" ACCELERATOR gear=5.0\n",
|
||||
"Analyzing segment 1618132d68afc876/2023-11-26--12-31-18/17/s for HYUNDAI KONA ELECTRIC 2ND GEN\n",
|
||||
" GEAR_SHIFTER gear=4.0\n",
|
||||
" ACCELERATOR gear=5.0\n",
|
||||
"Analyzing segment 1618132d68afc876/2023-12-05--11-51-44/3/s for HYUNDAI KONA ELECTRIC 2ND GEN\n",
|
||||
" GEAR_SHIFTER gear=4.0\n",
|
||||
" ACCELERATOR gear=5.0\n",
|
||||
"Analyzing segment 1618132d68afc876/2023-08-27--09-32-14/13/s for HYUNDAI KONA 2ND GEN\n",
|
||||
" GEAR_SHIFTER gear=4.0\n",
|
||||
" ACCELERATOR gear=5.0\n",
|
||||
"Analyzing segment 1618132d68afc876/2024-01-25--15-07-04/24/s for HYUNDAI KONA ELECTRIC 2ND GEN\n",
|
||||
" GEAR_SHIFTER gear=4.0\n",
|
||||
" ACCELERATOR gear=5.0\n",
|
||||
"Analyzing segment 223780ed74116bc2/2023-11-16--09-44-56/15/s for HYUNDAI IONIQ 5 2022\n",
|
||||
" GEAR_SHIFTER gear=4.0\n",
|
||||
" ACCELERATOR gear=5.0\n",
|
||||
"Analyzing segment ba9951252624f37d/2024-01-20--22-33-23/118/s for HYUNDAI IONIQ 5 2022\n",
|
||||
" GEAR_SHIFTER gear=4.0\n",
|
||||
" ACCELERATOR gear=5.0\n",
|
||||
"Analyzing segment 8379b28e51ceb3b1/2023-11-09--23-21-58/92/s for HYUNDAI IONIQ 5 2022\n",
|
||||
" GEAR_SHIFTER gear=4.0\n",
|
||||
" ACCELERATOR gear=5.0\n",
|
||||
"Analyzing segment 26fac43e27cd6091/2023-11-06--12-23-21/9/s for HYUNDAI IONIQ 5 2022\n",
|
||||
" GEAR_SHIFTER gear=1.0\n",
|
||||
" ACCELERATOR gear=0.0\n",
|
||||
"Analyzing segment 5edb897a0ec7a477/2024-01-13--20-41-36/101/s for HYUNDAI IONIQ 5 2022\n",
|
||||
" GEAR_SHIFTER gear=4.0\n",
|
||||
" ACCELERATOR gear=5.0\n",
|
||||
"Analyzing segment 66cf8ea23b7c2789/2023-12-04--13-48-53/5/s for KIA NIRO EV 2ND GEN\n",
|
||||
" GEAR_SHIFTER gear=4.0\n",
|
||||
" ACCELERATOR gear=5.0\n",
|
||||
"Analyzing segment b153671049a867b3/2023-12-10--20-31-37/2/s for KIA NIRO EV 2ND GEN\n",
|
||||
" GEAR_SHIFTER gear=4.0\n",
|
||||
" ACCELERATOR gear=5.0\n",
|
||||
"Analyzing segment b153671049a867b3/2023-12-03--21-08-30/14/s for KIA NIRO EV 2ND GEN\n",
|
||||
" GEAR_SHIFTER gear=4.0\n",
|
||||
" ACCELERATOR gear=5.0\n",
|
||||
"Analyzing segment b153671049a867b3/2023-11-07--19-52-23/0/s for KIA NIRO EV 2ND GEN\n",
|
||||
" GEAR_SHIFTER gear=4.0\n",
|
||||
" ACCELERATOR gear=5.0\n",
|
||||
"Analyzing segment b153671049a867b3/2023-07-12--19-25-18/6/s for KIA NIRO EV 2ND GEN\n",
|
||||
" GEAR_SHIFTER gear=4.0\n",
|
||||
" ACCELERATOR gear=5.0\n",
|
||||
"Analyzing segment 9ea4578ee2b1abcb/2023-11-18--07-59-26/11/s for HYUNDAI IONIQ 6 2023\n",
|
||||
" GEAR_SHIFTER gear=4.0\n",
|
||||
" ACCELERATOR gear=5.0\n",
|
||||
"Analyzing segment 0ad7facc77922c3e/2023-12-21--17-47-25/18/s for HYUNDAI IONIQ 6 2023\n",
|
||||
" GEAR_SHIFTER gear=4.0\n",
|
||||
" ACCELERATOR gear=5.0\n",
|
||||
"Analyzing segment 26968f888e7330d3/2024-01-02--11-18-37/8/s for HYUNDAI IONIQ 6 2023\n",
|
||||
" GEAR_SHIFTER gear=4.0\n",
|
||||
" ACCELERATOR gear=5.0\n",
|
||||
"Analyzing segment 9ea4578ee2b1abcb/2023-11-27--21-03-24/33/s for HYUNDAI IONIQ 6 2023\n",
|
||||
" GEAR_SHIFTER gear=4.0\n",
|
||||
" ACCELERATOR gear=5.0\n",
|
||||
"Analyzing segment df7fdd56970d90fe/2024-01-07--01-04-39/26/s for HYUNDAI IONIQ 6 2023\n",
|
||||
" GEAR_SHIFTER gear=4.0\n",
|
||||
" ACCELERATOR gear=5.0\n",
|
||||
"Analyzing segment 94542b2d06f7a9a6/2023-12-11--14-45-44/0/s for GENESIS GV60 ELECTRIC 1ST GEN\n",
|
||||
" GEAR_SHIFTER gear=4.0\n",
|
||||
" ACCELERATOR gear=5.0\n",
|
||||
"Analyzing segment 94542b2d06f7a9a6/2023-12-11--20-57-09/8/s for GENESIS GV60 ELECTRIC 1ST GEN\n",
|
||||
" GEAR_SHIFTER gear=4.0\n",
|
||||
" ACCELERATOR gear=5.0\n",
|
||||
"Analyzing segment 94542b2d06f7a9a6/2024-01-03--12-52-38/5/s for GENESIS GV60 ELECTRIC 1ST GEN\n",
|
||||
" GEAR_SHIFTER gear=4.0\n",
|
||||
" ACCELERATOR gear=5.0\n",
|
||||
"Analyzing segment 94542b2d06f7a9a6/2024-01-19--19-57-52/47/s for GENESIS GV60 ELECTRIC 1ST GEN\n",
|
||||
" GEAR_SHIFTER gear=4.0\n",
|
||||
" ACCELERATOR gear=5.0\n",
|
||||
"Analyzing segment 94542b2d06f7a9a6/2024-01-03--13-01-23/1/s for GENESIS GV60 ELECTRIC 1ST GEN\n",
|
||||
" GEAR_SHIFTER gear=4.0\n",
|
||||
" ACCELERATOR gear=5.0\n",
|
||||
"Analysis finished\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"import copy\n",
|
||||
"import matplotlib.pyplot as plt\n",
|
||||
"import numpy as np\n",
|
||||
"\n",
|
||||
"from iqdbc.can.parser import CANParser\n",
|
||||
"from iqdbc.car.hyundai.values import DBC\n",
|
||||
"from iqdbc.car.hyundai.hyundaicanfd import CanBus\n",
|
||||
"\n",
|
||||
"from openpilot.selfdrive.pandad import can_capnp_to_list\n",
|
||||
"from openpilot.tools.lib.logreader import LogReader, comma_car_segments_source\n",
|
||||
"\n",
|
||||
"message_names = [\"GEAR_SHIFTER\", \"ACCELERATOR\", \"GEAR\", \"GEAR_ALT\", \"GEAR_ALT_2\"]\n",
|
||||
"\n",
|
||||
"for segment in TEST_SEGMENTS:\n",
|
||||
" lr = LogReader(segment, sources=[comma_car_segments_source])\n",
|
||||
" CP = lr.first(\"carParams\")\n",
|
||||
" if CP is None:\n",
|
||||
" continue\n",
|
||||
"\n",
|
||||
" can_msgs = [msg for msg in lr if msg.which() == \"can\"]\n",
|
||||
" parser_messages = []\n",
|
||||
" for name in message_names:\n",
|
||||
" parser_messages.append((name, 0))\n",
|
||||
" cp = CANParser(DBC[platform][\"pt\"], parser_messages, CanBus(CP).ECAN)\n",
|
||||
"\n",
|
||||
" parsed_message_history = []\n",
|
||||
" examples = []\n",
|
||||
"\n",
|
||||
" for msg in can_msgs:\n",
|
||||
" cp.update_strings(can_capnp_to_list([msg.as_builder().to_bytes()]))\n",
|
||||
" parsed_message_history.append(copy.copy(cp.vl))\n",
|
||||
"\n",
|
||||
" print(f\"Analyzing segment {segment:<44} for {CP.carFingerprint}\")\n",
|
||||
" for name in message_names:\n",
|
||||
" if parsed_message_history[0][name][\"CHECKSUM\"] != 0: # Message is present for this segment\n",
|
||||
" gear_prev = parsed_message_history[0][name][\"GEAR\"]\n",
|
||||
" print(f\" {name:<15} gear={gear_prev}\")\n",
|
||||
" for i, parsed_messages in enumerate(parsed_message_history):\n",
|
||||
" gear = parsed_messages[name][\"GEAR\"]\n",
|
||||
" if gear != gear_prev:\n",
|
||||
" print(f\" *** Signal transition found! ***\")\n",
|
||||
" examples.append(i)\n",
|
||||
" gear_prev = gear\n",
|
||||
"\n",
|
||||
"print(f\"Analysis finished\")\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"id": "7724dd97-f62e-4fd3-9f64-63d49be669d2",
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": []
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"id": "9f393e00-8efd-40fb-a41e-d312531a83e8",
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": []
|
||||
}
|
||||
],
|
||||
"metadata": {
|
||||
"kernelspec": {
|
||||
"display_name": "Python 3 (ipykernel)",
|
||||
"language": "python",
|
||||
"name": "python3"
|
||||
},
|
||||
"language_info": {
|
||||
"codemirror_mode": {
|
||||
"name": "ipython",
|
||||
"version": 3
|
||||
},
|
||||
"file_extension": ".py",
|
||||
"mimetype": "text/x-python",
|
||||
"name": "python",
|
||||
"nbconvert_exporter": "python",
|
||||
"pygments_lexer": "ipython3",
|
||||
"version": "3.12.3"
|
||||
}
|
||||
},
|
||||
"nbformat": 4,
|
||||
"nbformat_minor": 5
|
||||
}
|
||||
260
tools/car_porting/examples/subaru_fuzzy_fingerprint.ipynb
Normal file
260
tools/car_porting/examples/subaru_fuzzy_fingerprint.ipynb
Normal file
@@ -0,0 +1,260 @@
|
||||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 6,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"from iqdbc.car import structs\n",
|
||||
"from iqdbc.car.subaru.values import CAR, SubaruFlags\n",
|
||||
"from iqdbc.car.subaru.fingerprints import FW_VERSIONS\n",
|
||||
"\n",
|
||||
"TEST_PLATFORMS = set(CAR) - CAR.with_flags(SubaruFlags.PREGLOBAL)\n",
|
||||
"\n",
|
||||
"Ecu = structs.CarParams.Ecu\n",
|
||||
"\n",
|
||||
"FW_BY_ECU = {platform: {ecu: versions for (ecu, addr, sub_addr), versions in fw_versions.items()} for platform, fw_versions in FW_VERSIONS.items()}"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 7,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"PLATFORM_CODES = {\n",
|
||||
" Ecu.abs: {\n",
|
||||
" 0: {\n",
|
||||
" b'\\xa5': [CAR.SUBARU_ASCENT, CAR.SUBARU_ASCENT_2023],\n",
|
||||
" b'\\xa2': [CAR.SUBARU_IMPREZA, CAR.SUBARU_IMPREZA_2020, CAR.SUBARU_CROSSTREK_HYBRID],\n",
|
||||
" b'\\xa1': [CAR.SUBARU_OUTBACK, CAR.SUBARU_LEGACY, CAR.SUBARU_OUTBACK_2023],\n",
|
||||
" b'\\xa3': [CAR.SUBARU_FORESTER, CAR.SUBARU_FORESTER_HYBRID, CAR.SUBARU_FORESTER_2022],\n",
|
||||
" b'z': [CAR.SUBARU_IMPREZA],\n",
|
||||
" }\n",
|
||||
" }\n",
|
||||
"}\n",
|
||||
"\n",
|
||||
"YEAR_CODES = {\n",
|
||||
" Ecu.abs: {\n",
|
||||
" 2: {\n",
|
||||
" b'\\x18': 2018,\n",
|
||||
" b'\\x19': 2019,\n",
|
||||
" b'\\x20': 2020,\n",
|
||||
" b'\\x21': 2021,\n",
|
||||
" b'\\x22': 2022,\n",
|
||||
" b'\\x23': 2023,\n",
|
||||
" }\n",
|
||||
" }\n",
|
||||
"}"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 8,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"def get_codes(platforms, codes):\n",
|
||||
" results = []\n",
|
||||
" for platform in platforms:\n",
|
||||
" for ecu in codes:\n",
|
||||
" for i in codes[ecu]:\n",
|
||||
" if isinstance(i, tuple):\n",
|
||||
" j = slice(i[0], i[1])\n",
|
||||
" else:\n",
|
||||
" j = slice(i, i+1)\n",
|
||||
" for version in FW_BY_ECU[platform][ecu]:\n",
|
||||
" code = version[j]\n",
|
||||
" if code not in codes[ecu][i]:\n",
|
||||
" print(f\"{platform} {code.hex()} not in {codes[ecu][i].keys()}\")\n",
|
||||
" else:\n",
|
||||
" results.append((platform, codes[ecu][i][code]))\n",
|
||||
" return results"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 9,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"SUBARU_IMPREZA 08 not in dict_keys([b'\\x18', b'\\x19', b' ', b'!', b'\"', b'#'])\n",
|
||||
"SUBARU_IMPREZA 08 not in dict_keys([b'\\x18', b'\\x19', b' ', b'!', b'\"', b'#'])\n",
|
||||
"SUBARU_IMPREZA 0c not in dict_keys([b'\\x18', b'\\x19', b' ', b'!', b'\"', b'#'])\n",
|
||||
"SUBARU_IMPREZA 0c not in dict_keys([b'\\x18', b'\\x19', b' ', b'!', b'\"', b'#'])\n",
|
||||
"SUBARU_IMPREZA 2e not in dict_keys([b'\\x18', b'\\x19', b' ', b'!', b'\"', b'#'])\n",
|
||||
"SUBARU_IMPREZA 3f not in dict_keys([b'\\x18', b'\\x19', b' ', b'!', b'\"', b'#'])\n",
|
||||
"correct_year=False platform=SUBARU_FORESTER year=2018 years=[2019, 2020, 2021]\n",
|
||||
"correct_year=False platform=SUBARU_FORESTER year=2018 years=[2019, 2020, 2021]\n",
|
||||
"correct_year=True platform=SUBARU_FORESTER year=2019 years=[2019, 2020, 2021]\n",
|
||||
"correct_year=True platform=SUBARU_FORESTER year=2019 years=[2019, 2020, 2021]\n",
|
||||
"correct_year=True platform=SUBARU_FORESTER year=2019 years=[2019, 2020, 2021]\n",
|
||||
"correct_year=True platform=SUBARU_FORESTER year=2020 years=[2019, 2020, 2021]\n",
|
||||
"correct_year=True platform=SUBARU_FORESTER year=2020 years=[2019, 2020, 2021]\n",
|
||||
"correct_year=True platform=SUBARU_OUTBACK year=2020 years=[2020, 2021, 2022]\n",
|
||||
"correct_year=True platform=SUBARU_OUTBACK year=2020 years=[2020, 2021, 2022]\n",
|
||||
"correct_year=True platform=SUBARU_OUTBACK year=2020 years=[2020, 2021, 2022]\n",
|
||||
"correct_year=True platform=SUBARU_OUTBACK year=2020 years=[2020, 2021, 2022]\n",
|
||||
"correct_year=True platform=SUBARU_OUTBACK year=2020 years=[2020, 2021, 2022]\n",
|
||||
"correct_year=True platform=SUBARU_OUTBACK year=2020 years=[2020, 2021, 2022]\n",
|
||||
"correct_year=True platform=SUBARU_OUTBACK year=2020 years=[2020, 2021, 2022]\n",
|
||||
"correct_year=True platform=SUBARU_OUTBACK year=2020 years=[2020, 2021, 2022]\n",
|
||||
"correct_year=True platform=SUBARU_OUTBACK year=2020 years=[2020, 2021, 2022]\n",
|
||||
"correct_year=True platform=SUBARU_OUTBACK year=2022 years=[2020, 2021, 2022]\n",
|
||||
"correct_year=True platform=SUBARU_OUTBACK year=2022 years=[2020, 2021, 2022]\n",
|
||||
"correct_year=False platform=SUBARU_FORESTER_HYBRID year=2019 years=[2020]\n",
|
||||
"correct_year=False platform=SUBARU_CROSSTREK_HYBRID year=2019 years=[2020]\n",
|
||||
"correct_year=False platform=SUBARU_CROSSTREK_HYBRID year=2021 years=[2020]\n",
|
||||
"correct_year=True platform=SUBARU_ASCENT_2023 year=2023 years=[2023]\n",
|
||||
"correct_year=True platform=SUBARU_IMPREZA year=2019 years=[2017, 2018, 2019]\n",
|
||||
"correct_year=True platform=SUBARU_IMPREZA year=2019 years=[2017, 2018, 2019]\n",
|
||||
"correct_year=True platform=SUBARU_IMPREZA year=2018 years=[2017, 2018, 2019]\n",
|
||||
"correct_year=True platform=SUBARU_IMPREZA year=2019 years=[2017, 2018, 2019]\n",
|
||||
"correct_year=True platform=SUBARU_IMPREZA year=2019 years=[2017, 2018, 2019]\n",
|
||||
"correct_year=True platform=SUBARU_IMPREZA year=2019 years=[2017, 2018, 2019]\n",
|
||||
"correct_year=False platform=SUBARU_FORESTER_2022 year=2021 years=[2022, 2023, 2024]\n",
|
||||
"correct_year=False platform=SUBARU_FORESTER_2022 year=2021 years=[2022, 2023, 2024]\n",
|
||||
"correct_year=True platform=SUBARU_FORESTER_2022 year=2022 years=[2022, 2023, 2024]\n",
|
||||
"correct_year=True platform=SUBARU_FORESTER_2022 year=2022 years=[2022, 2023, 2024]\n",
|
||||
"correct_year=True platform=SUBARU_ASCENT year=2019 years=[2019, 2020, 2021]\n",
|
||||
"correct_year=True platform=SUBARU_ASCENT year=2021 years=[2019, 2020, 2021]\n",
|
||||
"correct_year=True platform=SUBARU_OUTBACK_2023 year=2023 years=[2023]\n",
|
||||
"correct_year=True platform=SUBARU_OUTBACK_2023 year=2023 years=[2023]\n",
|
||||
"correct_year=False platform=SUBARU_IMPREZA_2020 year=2019 years=[2020, 2021, 2022]\n",
|
||||
"correct_year=False platform=SUBARU_IMPREZA_2020 year=2019 years=[2020, 2021, 2022]\n",
|
||||
"correct_year=True platform=SUBARU_IMPREZA_2020 year=2020 years=[2020, 2021, 2022]\n",
|
||||
"correct_year=True platform=SUBARU_IMPREZA_2020 year=2021 years=[2020, 2021, 2022]\n",
|
||||
"correct_year=True platform=SUBARU_IMPREZA_2020 year=2021 years=[2020, 2021, 2022]\n",
|
||||
"correct_year=True platform=SUBARU_IMPREZA_2020 year=2021 years=[2020, 2021, 2022]\n",
|
||||
"correct_year=True platform=SUBARU_IMPREZA_2020 year=2021 years=[2020, 2021, 2022]\n",
|
||||
"correct_year=True platform=SUBARU_LEGACY year=2020 years=[2020, 2021, 2022]\n",
|
||||
"correct_year=True platform=SUBARU_LEGACY year=2020 years=[2020, 2021, 2022]\n",
|
||||
"correct_year=True platform=SUBARU_LEGACY year=2020 years=[2020, 2021, 2022]\n",
|
||||
"correct_year=True platform=SUBARU_LEGACY year=2020 years=[2020, 2021, 2022]\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"def test_year_code(platform, year):\n",
|
||||
" car_docs = CAR(platform).config.car_docs\n",
|
||||
" if isinstance(car_docs, list):\n",
|
||||
" car_docs = car_docs[0]\n",
|
||||
" years = [int(y) for y in car_docs.year_list]\n",
|
||||
" correct_year = year in years\n",
|
||||
" print(f\"{correct_year=!s: <6} {platform=: <32} {year=: <5} {years=}\")\n",
|
||||
"\n",
|
||||
"codes = get_codes(TEST_PLATFORMS, YEAR_CODES)\n",
|
||||
"for platform, year in codes:\n",
|
||||
" test_year_code(platform, year)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 10,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"in_possible_platforms=True platform=SUBARU_FORESTER platforms=['SUBARU_FORESTER', 'SUBARU_FORESTER_HYBRID', 'SUBARU_FORESTER_2022']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_FORESTER platforms=['SUBARU_FORESTER', 'SUBARU_FORESTER_HYBRID', 'SUBARU_FORESTER_2022']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_FORESTER platforms=['SUBARU_FORESTER', 'SUBARU_FORESTER_HYBRID', 'SUBARU_FORESTER_2022']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_FORESTER platforms=['SUBARU_FORESTER', 'SUBARU_FORESTER_HYBRID', 'SUBARU_FORESTER_2022']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_FORESTER platforms=['SUBARU_FORESTER', 'SUBARU_FORESTER_HYBRID', 'SUBARU_FORESTER_2022']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_FORESTER platforms=['SUBARU_FORESTER', 'SUBARU_FORESTER_HYBRID', 'SUBARU_FORESTER_2022']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_FORESTER platforms=['SUBARU_FORESTER', 'SUBARU_FORESTER_HYBRID', 'SUBARU_FORESTER_2022']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_OUTBACK platforms=['SUBARU_OUTBACK', 'SUBARU_LEGACY', 'SUBARU_OUTBACK_2023']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_OUTBACK platforms=['SUBARU_OUTBACK', 'SUBARU_LEGACY', 'SUBARU_OUTBACK_2023']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_OUTBACK platforms=['SUBARU_OUTBACK', 'SUBARU_LEGACY', 'SUBARU_OUTBACK_2023']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_OUTBACK platforms=['SUBARU_OUTBACK', 'SUBARU_LEGACY', 'SUBARU_OUTBACK_2023']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_OUTBACK platforms=['SUBARU_OUTBACK', 'SUBARU_LEGACY', 'SUBARU_OUTBACK_2023']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_OUTBACK platforms=['SUBARU_OUTBACK', 'SUBARU_LEGACY', 'SUBARU_OUTBACK_2023']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_OUTBACK platforms=['SUBARU_OUTBACK', 'SUBARU_LEGACY', 'SUBARU_OUTBACK_2023']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_OUTBACK platforms=['SUBARU_OUTBACK', 'SUBARU_LEGACY', 'SUBARU_OUTBACK_2023']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_OUTBACK platforms=['SUBARU_OUTBACK', 'SUBARU_LEGACY', 'SUBARU_OUTBACK_2023']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_OUTBACK platforms=['SUBARU_OUTBACK', 'SUBARU_LEGACY', 'SUBARU_OUTBACK_2023']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_OUTBACK platforms=['SUBARU_OUTBACK', 'SUBARU_LEGACY', 'SUBARU_OUTBACK_2023']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_FORESTER_HYBRID platforms=['SUBARU_FORESTER', 'SUBARU_FORESTER_HYBRID', 'SUBARU_FORESTER_2022']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_CROSSTREK_HYBRID platforms=['SUBARU_IMPREZA', 'SUBARU_IMPREZA_2020', 'SUBARU_CROSSTREK_HYBRID']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_CROSSTREK_HYBRID platforms=['SUBARU_IMPREZA', 'SUBARU_IMPREZA_2020', 'SUBARU_CROSSTREK_HYBRID']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_ASCENT_2023 platforms=['SUBARU_ASCENT', 'SUBARU_ASCENT_2023']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_IMPREZA platforms=['SUBARU_IMPREZA']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_IMPREZA platforms=['SUBARU_IMPREZA']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_IMPREZA platforms=['SUBARU_IMPREZA']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_IMPREZA platforms=['SUBARU_IMPREZA']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_IMPREZA platforms=['SUBARU_IMPREZA']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_IMPREZA platforms=['SUBARU_IMPREZA']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_IMPREZA platforms=['SUBARU_IMPREZA']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_IMPREZA platforms=['SUBARU_IMPREZA']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_IMPREZA platforms=['SUBARU_IMPREZA', 'SUBARU_IMPREZA_2020', 'SUBARU_CROSSTREK_HYBRID']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_IMPREZA platforms=['SUBARU_IMPREZA', 'SUBARU_IMPREZA_2020', 'SUBARU_CROSSTREK_HYBRID']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_IMPREZA platforms=['SUBARU_IMPREZA', 'SUBARU_IMPREZA_2020', 'SUBARU_CROSSTREK_HYBRID']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_IMPREZA platforms=['SUBARU_IMPREZA', 'SUBARU_IMPREZA_2020', 'SUBARU_CROSSTREK_HYBRID']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_FORESTER_2022 platforms=['SUBARU_FORESTER', 'SUBARU_FORESTER_HYBRID', 'SUBARU_FORESTER_2022']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_FORESTER_2022 platforms=['SUBARU_FORESTER', 'SUBARU_FORESTER_HYBRID', 'SUBARU_FORESTER_2022']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_FORESTER_2022 platforms=['SUBARU_FORESTER', 'SUBARU_FORESTER_HYBRID', 'SUBARU_FORESTER_2022']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_FORESTER_2022 platforms=['SUBARU_FORESTER', 'SUBARU_FORESTER_HYBRID', 'SUBARU_FORESTER_2022']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_ASCENT platforms=['SUBARU_ASCENT', 'SUBARU_ASCENT_2023']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_ASCENT platforms=['SUBARU_ASCENT', 'SUBARU_ASCENT_2023']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_OUTBACK_2023 platforms=['SUBARU_OUTBACK', 'SUBARU_LEGACY', 'SUBARU_OUTBACK_2023']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_OUTBACK_2023 platforms=['SUBARU_OUTBACK', 'SUBARU_LEGACY', 'SUBARU_OUTBACK_2023']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_IMPREZA_2020 platforms=['SUBARU_IMPREZA', 'SUBARU_IMPREZA_2020', 'SUBARU_CROSSTREK_HYBRID']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_IMPREZA_2020 platforms=['SUBARU_IMPREZA', 'SUBARU_IMPREZA_2020', 'SUBARU_CROSSTREK_HYBRID']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_IMPREZA_2020 platforms=['SUBARU_IMPREZA', 'SUBARU_IMPREZA_2020', 'SUBARU_CROSSTREK_HYBRID']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_IMPREZA_2020 platforms=['SUBARU_IMPREZA', 'SUBARU_IMPREZA_2020', 'SUBARU_CROSSTREK_HYBRID']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_IMPREZA_2020 platforms=['SUBARU_IMPREZA', 'SUBARU_IMPREZA_2020', 'SUBARU_CROSSTREK_HYBRID']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_IMPREZA_2020 platforms=['SUBARU_IMPREZA', 'SUBARU_IMPREZA_2020', 'SUBARU_CROSSTREK_HYBRID']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_IMPREZA_2020 platforms=['SUBARU_IMPREZA', 'SUBARU_IMPREZA_2020', 'SUBARU_CROSSTREK_HYBRID']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_LEGACY platforms=['SUBARU_OUTBACK', 'SUBARU_LEGACY', 'SUBARU_OUTBACK_2023']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_LEGACY platforms=['SUBARU_OUTBACK', 'SUBARU_LEGACY', 'SUBARU_OUTBACK_2023']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_LEGACY platforms=['SUBARU_OUTBACK', 'SUBARU_LEGACY', 'SUBARU_OUTBACK_2023']\n",
|
||||
"in_possible_platforms=True platform=SUBARU_LEGACY platforms=['SUBARU_OUTBACK', 'SUBARU_LEGACY', 'SUBARU_OUTBACK_2023']\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"def test_platform_code(platform, platforms):\n",
|
||||
" platforms = [str(p) for p in platforms]\n",
|
||||
" in_possible_platforms = platform in platforms\n",
|
||||
" print(f\"{in_possible_platforms=!s: <6} {platform=: <32} {platforms=}\")\n",
|
||||
"\n",
|
||||
"codes = get_codes(TEST_PLATFORMS, PLATFORM_CODES)\n",
|
||||
"for platform, possible_platforms in codes:\n",
|
||||
" test_platform_code(platform, possible_platforms)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": []
|
||||
}
|
||||
],
|
||||
"metadata": {
|
||||
"kernelspec": {
|
||||
"display_name": "Python 3 (ipykernel)",
|
||||
"language": "python",
|
||||
"name": "python3"
|
||||
},
|
||||
"language_info": {
|
||||
"codemirror_mode": {
|
||||
"name": "ipython",
|
||||
"version": 3
|
||||
},
|
||||
"file_extension": ".py",
|
||||
"mimetype": "text/x-python",
|
||||
"name": "python",
|
||||
"nbconvert_exporter": "python",
|
||||
"pygments_lexer": "ipython3",
|
||||
"version": "3.12.3"
|
||||
}
|
||||
},
|
||||
"nbformat": 4,
|
||||
"nbformat_minor": 4
|
||||
}
|
||||
128
tools/car_porting/examples/subaru_long_accel.ipynb
Normal file
128
tools/car_porting/examples/subaru_long_accel.ipynb
Normal file
@@ -0,0 +1,128 @@
|
||||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 1,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"segments = [\n",
|
||||
" \"d9df6f87e8feff94|2023-03-28--17-41-10/1:12\"\n",
|
||||
"]\n",
|
||||
"platform = \"SUBARU_OUTBACK\"\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 3,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"import copy\n",
|
||||
"import numpy as np\n",
|
||||
"\n",
|
||||
"from iqdbc.can.parser import CANParser\n",
|
||||
"from iqdbc.car.subaru.values import DBC\n",
|
||||
"\n",
|
||||
"from openpilot.selfdrive.pandad import can_capnp_to_list\n",
|
||||
"from openpilot.tools.lib.logreader import LogReader\n",
|
||||
"\n",
|
||||
"\"\"\"\n",
|
||||
"In this example, we plot the relationship between Cruise_Brake and Acceleration for stock eyesight.\n",
|
||||
"\"\"\"\n",
|
||||
"\n",
|
||||
"for segment in segments:\n",
|
||||
" lr = LogReader(segment)\n",
|
||||
"\n",
|
||||
" messages = [\n",
|
||||
" (\"ES_Distance\", 20),\n",
|
||||
" (\"ES_Brake\", 20),\n",
|
||||
" (\"ES_Status\", 20),\n",
|
||||
" ]\n",
|
||||
"\n",
|
||||
" cp = CANParser(DBC[platform][\"pt\"], messages, 1)\n",
|
||||
"\n",
|
||||
" es_distance_history = []\n",
|
||||
" es_status_history = []\n",
|
||||
" es_brake_history = []\n",
|
||||
" acceleration_history = []\n",
|
||||
"\n",
|
||||
" last_acc = 0\n",
|
||||
"\n",
|
||||
" for msg in lr:\n",
|
||||
" if msg.which() == \"can\":\n",
|
||||
" cp.update_strings(can_capnp_to_list([msg.as_builder().to_bytes()]))\n",
|
||||
" es_distance_history.append(copy.copy(cp.vl[\"ES_Distance\"]))\n",
|
||||
" es_brake_history.append(copy.copy(cp.vl[\"ES_Brake\"]))\n",
|
||||
" es_status_history.append(copy.copy(cp.vl[\"ES_Status\"]))\n",
|
||||
"\n",
|
||||
" acceleration_history.append(last_acc)\n",
|
||||
"\n",
|
||||
" if msg.which() == \"carState\":\n",
|
||||
" last_acc = msg.carState.aEgo"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 4,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"def process(history, func):\n",
|
||||
" return np.array([func(h) for h in history])\n",
|
||||
"\n",
|
||||
"cruise_activated = process(es_status_history, lambda es_status: es_status[\"Cruise_Activated\"])\n",
|
||||
"cruise_throttle = process(es_distance_history, lambda es_distance: es_distance[\"Cruise_Throttle\"])\n",
|
||||
"cruise_rpm = process(es_status_history, lambda es_status: es_status[\"Cruise_RPM\"])\n",
|
||||
"cruise_brake = process(es_brake_history, lambda es_brake: es_brake[\"Brake_Pressure\"])\n",
|
||||
"acceleration = process(acceleration_history, lambda acc: acc)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"import matplotlib.pyplot as plt\n",
|
||||
"\n",
|
||||
"valid_brake = (cruise_activated==1) & (cruise_brake>0) # only when cruise is activated and eyesight is braking\n",
|
||||
"\n",
|
||||
"ax = plt.figure().add_subplot()\n",
|
||||
"\n",
|
||||
"ax.set_title(\"Brake_Pressure vs Acceleration\")\n",
|
||||
"ax.set_xlabel(\"Brake_Pessure\")\n",
|
||||
"ax.set_ylabel(\"Acceleration\")\n",
|
||||
"ax.scatter(cruise_brake[valid_brake], -acceleration[valid_brake])"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": []
|
||||
}
|
||||
],
|
||||
"metadata": {
|
||||
"kernelspec": {
|
||||
"display_name": "Python 3 (ipykernel)",
|
||||
"language": "python",
|
||||
"name": "python3"
|
||||
},
|
||||
"language_info": {
|
||||
"codemirror_mode": {
|
||||
"name": "ipython",
|
||||
"version": 3
|
||||
},
|
||||
"file_extension": ".py",
|
||||
"mimetype": "text/x-python",
|
||||
"name": "python",
|
||||
"nbconvert_exporter": "python",
|
||||
"pygments_lexer": "ipython3",
|
||||
"version": "3.12.3"
|
||||
}
|
||||
},
|
||||
"nbformat": 4,
|
||||
"nbformat_minor": 4
|
||||
}
|
||||
118
tools/car_porting/examples/subaru_steer_temp_fault.ipynb
Normal file
118
tools/car_porting/examples/subaru_steer_temp_fault.ipynb
Normal file
@@ -0,0 +1,118 @@
|
||||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"# An example of searching through a database of segments for a specific condition, and plotting the results.\n",
|
||||
"\n",
|
||||
"segments = [\n",
|
||||
" \"c3d1ccb52f5f9d65|2023-07-22--01-23-20/6:10\",\n",
|
||||
"]\n",
|
||||
"platform = \"SUBARU_OUTBACK\""
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"import copy\n",
|
||||
"import matplotlib.pyplot as plt\n",
|
||||
"import numpy as np\n",
|
||||
"\n",
|
||||
"from iqdbc.can.parser import CANParser\n",
|
||||
"from iqdbc.car.subaru.values import CanBus, DBC\n",
|
||||
"\n",
|
||||
"from openpilot.selfdrive.pandad import can_capnp_to_list\n",
|
||||
"from openpilot.tools.lib.logreader import LogReader\n",
|
||||
"\n",
|
||||
"\"\"\"\n",
|
||||
"In this example, we search for positive transitions of Steer_Warning, which indicate that the EPS\n",
|
||||
"has stopped responding to our messages. This analysis would allow you to find the cause of these\n",
|
||||
"steer warnings and potentially work around them.\n",
|
||||
"\"\"\"\n",
|
||||
"\n",
|
||||
"for segment in segments:\n",
|
||||
" lr = LogReader(segment)\n",
|
||||
"\n",
|
||||
" can_msgs = [msg for msg in lr if msg.which() == \"can\"]\n",
|
||||
"\n",
|
||||
" messages = [\n",
|
||||
" (\"Steering_Torque\", 50)\n",
|
||||
" ]\n",
|
||||
"\n",
|
||||
" cp = CANParser(DBC[platform][\"pt\"], messages, CanBus.main)\n",
|
||||
"\n",
|
||||
" steering_torque_history = []\n",
|
||||
" examples = []\n",
|
||||
"\n",
|
||||
" for msg in can_msgs:\n",
|
||||
" cp.update_strings(can_capnp_to_list([msg.as_builder().to_bytes()]))\n",
|
||||
" steering_torque_history.append(copy.copy(cp.vl[\"Steering_Torque\"]))\n",
|
||||
"\n",
|
||||
" steer_warning_last = False\n",
|
||||
" for i, steering_torque_msg in enumerate(steering_torque_history):\n",
|
||||
" steer_warning = steering_torque_msg[\"Steer_Warning\"]\n",
|
||||
"\n",
|
||||
" steer_angle = steering_torque_msg[\"Steering_Angle\"]\n",
|
||||
"\n",
|
||||
" if steer_warning and not steer_warning_last: # positive transition of \"Steer_Warning\"\n",
|
||||
" examples.append(i)\n",
|
||||
"\n",
|
||||
" steer_warning_last = steer_warning\n",
|
||||
"\n",
|
||||
" FRAME_DELTA = 100 # plot this many frames around the positive transition\n",
|
||||
"\n",
|
||||
" for example in examples:\n",
|
||||
" fig, axs = plt.subplots(2)\n",
|
||||
"\n",
|
||||
" min_frame = int(example-FRAME_DELTA/2)\n",
|
||||
" max_frame = int(example+FRAME_DELTA/2)\n",
|
||||
"\n",
|
||||
" steering_angle_history = [msg[\"Steering_Angle\"] for msg in steering_torque_history[min_frame:max_frame]]\n",
|
||||
" steering_warning_history = [msg[\"Steer_Warning\"] for msg in steering_torque_history[min_frame:max_frame]]\n",
|
||||
"\n",
|
||||
" xs = np.arange(-FRAME_DELTA/2, FRAME_DELTA/2)\n",
|
||||
"\n",
|
||||
" axs[0].plot(xs, steering_angle_history)\n",
|
||||
" axs[0].set_ylabel(\"Steering Angle (deg)\")\n",
|
||||
" axs[1].plot(xs, steering_warning_history)\n",
|
||||
" axs[1].set_ylabel(\"Steer Warning\")\n",
|
||||
"\n",
|
||||
" plt.show()\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": []
|
||||
}
|
||||
],
|
||||
"metadata": {
|
||||
"kernelspec": {
|
||||
"display_name": "Python 3 (ipykernel)",
|
||||
"language": "python",
|
||||
"name": "python3"
|
||||
},
|
||||
"language_info": {
|
||||
"codemirror_mode": {
|
||||
"name": "ipython",
|
||||
"version": 3
|
||||
},
|
||||
"file_extension": ".py",
|
||||
"mimetype": "text/x-python",
|
||||
"name": "python",
|
||||
"nbconvert_exporter": "python",
|
||||
"pygments_lexer": "ipython3",
|
||||
"version": "3.12.3"
|
||||
}
|
||||
},
|
||||
"nbformat": 4,
|
||||
"nbformat_minor": 4
|
||||
}
|
||||
36
tools/car_porting/test_car_model.py
Executable file
36
tools/car_porting/test_car_model.py
Executable file
@@ -0,0 +1,36 @@
|
||||
#!/usr/bin/env python3
|
||||
import argparse
|
||||
import sys
|
||||
import unittest # noqa: TID251
|
||||
|
||||
from iqdbc.car.tests.routes import CarTestRoute
|
||||
from openpilot.selfdrive.car.tests.test_models import TestCarModel
|
||||
from openpilot.tools.lib.route import SegmentRange
|
||||
|
||||
|
||||
def create_test_models_suite(routes: list[CarTestRoute]) -> unittest.TestSuite:
|
||||
test_suite = unittest.TestSuite()
|
||||
for test_route in routes:
|
||||
# create new test case and discover tests
|
||||
test_case_args = {"platform": test_route.car_model, "test_route": test_route}
|
||||
CarModelTestCase = type("CarModelTestCase", (TestCarModel,), test_case_args)
|
||||
test_suite.addTest(unittest.TestLoader().loadTestsFromTestCase(CarModelTestCase))
|
||||
return test_suite
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser(description="Test any route against common issues with a new car port. " +
|
||||
"Uses selfdrive/car/tests/test_models.py")
|
||||
parser.add_argument("route_or_segment_name", help="Specify route to run tests on")
|
||||
parser.add_argument("--car", help="Specify car model for test route")
|
||||
args = parser.parse_args()
|
||||
if len(sys.argv) == 1:
|
||||
parser.print_help()
|
||||
sys.exit()
|
||||
|
||||
sr = SegmentRange(args.route_or_segment_name)
|
||||
|
||||
test_routes = [CarTestRoute(sr.route_name, args.car, segment=seg_idx) for seg_idx in sr.seg_idxs]
|
||||
test_suite = create_test_models_suite(test_routes)
|
||||
|
||||
unittest.TextTestRunner().run(test_suite)
|
||||
456
tools/clip/run.py
Executable file
456
tools/clip/run.py
Executable file
@@ -0,0 +1,456 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
import logging
|
||||
import subprocess
|
||||
import threading
|
||||
import queue
|
||||
import multiprocessing
|
||||
import itertools
|
||||
import numpy as np
|
||||
import tqdm
|
||||
from argparse import ArgumentParser
|
||||
from pathlib import Path
|
||||
from concurrent.futures import ThreadPoolExecutor, as_completed
|
||||
|
||||
from openpilot.tools.lib.route import Route
|
||||
from openpilot.tools.lib.logreader import LogReader
|
||||
from openpilot.tools.lib.filereader import FileReader
|
||||
from openpilot.tools.lib.framereader import FrameReader, ffprobe
|
||||
from openpilot.selfdrive.test.process_replay.migration import migrate_all
|
||||
from openpilot.common.prefix import OpenpilotPrefix
|
||||
from openpilot.common.utils import Timer
|
||||
from msgq.visionipc import VisionIpcServer, VisionStreamType
|
||||
|
||||
FRAMERATE = 20
|
||||
DEMO_ROUTE, DEMO_START, DEMO_END = 'a2a0ccea32023010/2023-07-27--13-01-19', 90, 105
|
||||
|
||||
logger = logging.getLogger('clip')
|
||||
|
||||
|
||||
def parse_args():
|
||||
parser = ArgumentParser(description="Direct clip renderer")
|
||||
parser.add_argument("route", nargs="?", help="Route ID (dongle/route or dongle/route/start/end)")
|
||||
parser.add_argument("-s", "--start", type=int, help="Start time in seconds")
|
||||
parser.add_argument("-e", "--end", type=int, help="End time in seconds")
|
||||
parser.add_argument("-o", "--output", default="output.mp4", help="Output file path")
|
||||
parser.add_argument("-d", "--data-dir", help="Local directory with route data")
|
||||
parser.add_argument("-t", "--title", help="Title overlay text")
|
||||
parser.add_argument("-f", "--file-size", type=float, default=9.0, help="Target file size in MB")
|
||||
parser.add_argument("-x", "--speed", type=int, default=1, help="Speed multiplier")
|
||||
parser.add_argument("--demo", action="store_true", help="Use demo route with default timing")
|
||||
ui_group = parser.add_mutually_exclusive_group()
|
||||
ui_group.add_argument("--big", dest="big", action="store_true", default=None, help="Force big UI (2160x1080)")
|
||||
ui_group.add_argument("--mici", dest="big", action="store_false", help="Force mici UI (536x240)")
|
||||
parser.add_argument("--qcam", action="store_true", help="Use qcamera instead of fcamera")
|
||||
parser.add_argument("--windowed", action="store_true", help="Show window")
|
||||
parser.add_argument("--no-metadata", action="store_true", help="Disable metadata overlay")
|
||||
parser.add_argument("--no-time-overlay", action="store_true", help="Disable time overlay")
|
||||
args = parser.parse_args()
|
||||
|
||||
if args.demo:
|
||||
args.route, args.start, args.end = args.route or DEMO_ROUTE, args.start or DEMO_START, args.end or DEMO_END
|
||||
elif not args.route:
|
||||
parser.error("route is required (or use --demo)")
|
||||
|
||||
if args.route and args.route.count('/') == 3:
|
||||
parts = args.route.split('/')
|
||||
args.route, args.start, args.end = '/'.join(parts[:2]), args.start or int(parts[2]), args.end or int(parts[3])
|
||||
|
||||
if args.start is None or args.end is None:
|
||||
parser.error("--start and --end are required")
|
||||
if args.end <= args.start:
|
||||
parser.error(f"end ({args.end}) must be greater than start ({args.start})")
|
||||
return args
|
||||
|
||||
|
||||
def setup_env(output_path: str, big: bool = False, speed: int = 1, target_mb: float = 0, duration: int = 0):
|
||||
os.environ.update({"RECORD": "1", "OFFSCREEN": "1", "RECORD_OUTPUT": str(Path(output_path).with_suffix(".mp4"))})
|
||||
if speed > 1:
|
||||
os.environ["RECORD_SPEED"] = str(speed)
|
||||
if target_mb > 0 and duration > 0:
|
||||
os.environ["RECORD_BITRATE"] = f"{int(target_mb * 8 * 1024 / (duration / speed))}k"
|
||||
if big:
|
||||
os.environ["BIG"] = "1"
|
||||
else:
|
||||
os.environ["BIG"] = "0"
|
||||
|
||||
|
||||
def _download_segment(path: str) -> bytes:
|
||||
with FileReader(path) as f:
|
||||
return bytes(f.read())
|
||||
|
||||
|
||||
def _parse_and_chunk_segment(args: tuple) -> list[dict]:
|
||||
raw_data, fps = args
|
||||
from openpilot.tools.lib.logreader import _LogFileReader
|
||||
messages = migrate_all(list(_LogFileReader("", dat=raw_data, sort_by_time=True)))
|
||||
if not messages:
|
||||
return []
|
||||
|
||||
dt_ns, chunks, current, next_time = 1e9 / fps, [], {}, messages[0].logMonoTime + 1e9 / fps # type: ignore[var-annotated]
|
||||
for msg in messages:
|
||||
if msg.logMonoTime >= next_time:
|
||||
chunks.append(current)
|
||||
current, next_time = {}, next_time + dt_ns * ((msg.logMonoTime - next_time) // dt_ns + 1)
|
||||
current[msg.which()] = msg
|
||||
return chunks + [current] if current else chunks
|
||||
|
||||
|
||||
def load_logs_parallel(log_paths: list[str], fps: int = 20) -> list[dict]:
|
||||
num_workers = min(16, len(log_paths), (multiprocessing.cpu_count() or 1))
|
||||
logger.info(f"Downloading {len(log_paths)} segments with {num_workers} workers...")
|
||||
|
||||
with ThreadPoolExecutor(max_workers=num_workers) as pool:
|
||||
futures = {pool.submit(_download_segment, path): idx for idx, path in enumerate(log_paths)}
|
||||
raw_data = {futures[f]: f.result() for f in as_completed(futures)}
|
||||
|
||||
logger.info("Parsing and chunking segments...")
|
||||
with multiprocessing.Pool(num_workers) as pool:
|
||||
return list(itertools.chain.from_iterable(pool.map(_parse_and_chunk_segment, [(raw_data[i], fps) for i in range(len(log_paths))])))
|
||||
|
||||
|
||||
def patch_submaster(message_chunks, ui_state):
|
||||
# Reset started_frame so alerts render correctly (recv_frame must be >= started_frame)
|
||||
ui_state.started_frame = 0
|
||||
ui_state.started_time = time.monotonic()
|
||||
|
||||
def mock_update(timeout=None):
|
||||
sm, t = ui_state.sm, time.monotonic()
|
||||
sm.updated = dict.fromkeys(sm.services, False)
|
||||
if sm.frame < len(message_chunks):
|
||||
for svc, msg in message_chunks[sm.frame].items():
|
||||
if svc in sm.data:
|
||||
sm.seen[svc] = sm.updated[svc] = sm.alive[svc] = sm.valid[svc] = True
|
||||
sm.data[svc] = getattr(msg.as_builder(), svc)
|
||||
sm.logMonoTime[svc], sm.recv_time[svc], sm.recv_frame[svc] = msg.logMonoTime, t, sm.frame
|
||||
sm.frame += 1
|
||||
ui_state.sm.update = mock_update
|
||||
|
||||
|
||||
def get_frame_dimensions(camera_path: str) -> tuple[int, int]:
|
||||
"""Get frame dimensions from a video file using ffprobe."""
|
||||
probe = ffprobe(camera_path)
|
||||
stream = probe["streams"][0]
|
||||
return stream["width"], stream["height"]
|
||||
|
||||
|
||||
def iter_segment_frames(camera_paths, start_time, end_time, fps=20, use_qcam=False,
|
||||
frame_size: tuple[int, int] | None = None, on_segment_open=None):
|
||||
frames_per_seg = fps * 60
|
||||
start_frame, end_frame = int(start_time * fps), int(end_time * fps)
|
||||
current_seg: int = -1
|
||||
seg_frames: FrameReader | np.ndarray | None = None
|
||||
|
||||
for global_idx in range(start_frame, end_frame):
|
||||
seg_idx, local_idx = global_idx // frames_per_seg, global_idx % frames_per_seg
|
||||
|
||||
if seg_idx != current_seg:
|
||||
current_seg = seg_idx
|
||||
path = camera_paths[seg_idx] if seg_idx < len(camera_paths) else None
|
||||
if not path:
|
||||
raise RuntimeError(f"No camera file for segment {seg_idx}")
|
||||
if on_segment_open is not None:
|
||||
on_segment_open(seg_idx, path)
|
||||
|
||||
if use_qcam:
|
||||
w, h = frame_size or get_frame_dimensions(path)
|
||||
with FileReader(path) as f:
|
||||
result = subprocess.run(["ffmpeg", "-v", "quiet", "-i", "-", "-f", "rawvideo", "-pix_fmt", "nv12", "-"],
|
||||
input=f.read(), capture_output=True)
|
||||
if result.returncode != 0:
|
||||
raise RuntimeError(f"ffmpeg failed: {result.stderr.decode()}")
|
||||
seg_frames = np.frombuffer(result.stdout, dtype=np.uint8).reshape(-1, w * h * 3 // 2)
|
||||
else:
|
||||
seg_frames = FrameReader(path, pix_fmt="nv12")
|
||||
|
||||
assert seg_frames is not None
|
||||
frame = seg_frames[local_idx] if use_qcam else seg_frames.get(local_idx) # type: ignore[index, union-attr]
|
||||
yield global_idx, frame
|
||||
|
||||
|
||||
class FrameQueue:
|
||||
def __init__(self, camera_paths, start_time, end_time, fps=20, prefetch_count=60, use_qcam=False):
|
||||
# Probe first valid camera file for dimensions
|
||||
first_path = next((p for p in camera_paths if p), None)
|
||||
if not first_path:
|
||||
raise RuntimeError("No valid camera paths")
|
||||
self.frame_w, self.frame_h = get_frame_dimensions(first_path)
|
||||
|
||||
self._queue, self._stop, self._error = queue.Queue(maxsize=prefetch_count), threading.Event(), None
|
||||
self._use_qcam = use_qcam
|
||||
self._current_seg_idx = None
|
||||
self._current_path = first_path
|
||||
self._state_lock = threading.Lock()
|
||||
self._thread = threading.Thread(target=self._worker,
|
||||
args=(camera_paths, start_time, end_time, fps, use_qcam, (self.frame_w, self.frame_h)), daemon=True)
|
||||
self._thread.start()
|
||||
|
||||
def _set_current_source(self, seg_idx, path):
|
||||
with self._state_lock:
|
||||
self._current_seg_idx = seg_idx
|
||||
self._current_path = path
|
||||
|
||||
def _worker(self, camera_paths, start_time, end_time, fps, use_qcam, frame_size):
|
||||
try:
|
||||
for idx, data in iter_segment_frames(camera_paths, start_time, end_time, fps, use_qcam, frame_size, self._set_current_source):
|
||||
if self._stop.is_set():
|
||||
break
|
||||
self._queue.put((idx, data.tobytes()))
|
||||
except Exception as e:
|
||||
logger.exception("Decode error")
|
||||
self._error = e
|
||||
finally:
|
||||
self._queue.put(None)
|
||||
|
||||
def get(self, timeout=60.0):
|
||||
deadline = time.monotonic() + timeout
|
||||
while True:
|
||||
if self._error:
|
||||
raise self._error
|
||||
|
||||
remaining = max(0.0, deadline - time.monotonic())
|
||||
if remaining == 0.0:
|
||||
break
|
||||
|
||||
try:
|
||||
result = self._queue.get(timeout=min(0.5, remaining))
|
||||
except queue.Empty:
|
||||
continue
|
||||
|
||||
if result is None:
|
||||
if self._error:
|
||||
raise self._error
|
||||
raise StopIteration("No more frames")
|
||||
return result
|
||||
|
||||
if self._error:
|
||||
raise self._error
|
||||
|
||||
with self._state_lock:
|
||||
seg_idx = self._current_seg_idx
|
||||
path = self._current_path
|
||||
|
||||
camera_kind = "qcamera" if self._use_qcam else "fcamera"
|
||||
source = f"segment {seg_idx}" if seg_idx is not None else "the initial segment"
|
||||
if path:
|
||||
source = f"{source} ({path})"
|
||||
hint = ""
|
||||
if path and path.startswith(("http://", "https://")):
|
||||
hint = " Try downloading the route locally with --data-dir or verify the remote camera endpoint supports timely range reads."
|
||||
raise TimeoutError(f"Timed out after {timeout:.0f}s waiting for {camera_kind} frames from {source}; camera fetch or decode is stalled.{hint}")
|
||||
|
||||
def stop(self):
|
||||
self._stop.set()
|
||||
while not self._queue.empty():
|
||||
try:
|
||||
self._queue.get_nowait()
|
||||
except queue.Empty:
|
||||
break
|
||||
self._thread.join(timeout=2.0)
|
||||
|
||||
|
||||
def load_route_metadata(route):
|
||||
from openpilot.common.params import Params, UnknownKeyName
|
||||
lr = LogReader(route.log_paths()[0])
|
||||
init_data, car_params = lr.first('initData'), lr.first('carParams')
|
||||
|
||||
params = Params()
|
||||
for entry in init_data.params.entries:
|
||||
try:
|
||||
value = params.cpp2python(entry.key, entry.value)
|
||||
if value is None:
|
||||
logger.warning("Skipping malformed route param %s while loading clip metadata", entry.key)
|
||||
continue
|
||||
params.put(entry.key, value)
|
||||
except UnknownKeyName:
|
||||
pass
|
||||
except TypeError:
|
||||
logger.warning("Skipping route param %s due to type mismatch while loading clip metadata", entry.key)
|
||||
|
||||
origin = init_data.gitRemote.split('/')[3] if len(init_data.gitRemote.split('/')) > 3 else 'unknown'
|
||||
return {
|
||||
'version': init_data.version, 'route': route.name.canonical_name,
|
||||
'car': car_params.carFingerprint if car_params else 'unknown', 'origin': origin,
|
||||
'commit': init_data.gitCommit[:7],
|
||||
}
|
||||
|
||||
|
||||
def detect_big_ui(route: Route) -> bool:
|
||||
try:
|
||||
init_data = LogReader(route.log_paths()[0]).first('initData')
|
||||
git_branch = (init_data.gitBranch or "").lower()
|
||||
device_type = str(init_data.deviceType).lower()
|
||||
if device_type in ("mici", "tizi", "tici"):
|
||||
big = device_type != "mici"
|
||||
reason = f"route device type {device_type}"
|
||||
elif "mici" in git_branch:
|
||||
big = False
|
||||
reason = f"route branch {git_branch}"
|
||||
else:
|
||||
big = True
|
||||
reason = f"route branch {git_branch or 'unknown'}"
|
||||
logger.info("Auto-detected %s UI from %s", "big" if big else "mici", reason)
|
||||
return big
|
||||
except Exception:
|
||||
logger.warning("Falling back to big UI; failed to auto-detect UI mode", exc_info=True)
|
||||
return True
|
||||
|
||||
|
||||
def draw_text_box(rl, text, x, y, size, gui_app, font, font_scale, color=None, center=False):
|
||||
box_color, text_color = rl.Color(0, 0, 0, 85), color or rl.WHITE
|
||||
# measure_text_ex is NOT auto-scaled, so multiply by font_scale
|
||||
# draw_text_ex IS auto-scaled, so pass size directly
|
||||
text_size = rl.measure_text_ex(font, text, size * font_scale, 0)
|
||||
text_width, text_height = int(text_size.x), int(text_size.y)
|
||||
if center:
|
||||
x = (gui_app.width - text_width) // 2
|
||||
rl.draw_rectangle(x - 8, y - 4, text_width + 16, text_height + 8, box_color)
|
||||
rl.draw_text_ex(font, text, rl.Vector2(x, y), size, 0, text_color)
|
||||
|
||||
|
||||
def render_overlays(rl, gui_app, font, font_scale, metadata, title, start_time, frame_idx, show_metadata, show_time):
|
||||
if show_metadata and metadata and frame_idx < FRAMERATE * 5:
|
||||
m = metadata
|
||||
text = ", ".join([f"IQ.Pilot v{m['version']}", f"route: {m['route']}", f"car: {m['car']}", f"origin: {m['origin']}",
|
||||
f"commit: {m['commit']}"])
|
||||
# Truncate if too wide (leave 20px margin on each side)
|
||||
max_width = gui_app.width - 40
|
||||
while rl.measure_text_ex(font, text, 15 * font_scale, 0).x > max_width and len(text) > 20:
|
||||
text = text[:-4] + "..."
|
||||
draw_text_box(rl, text, 0, 8, 15, gui_app, font, font_scale, center=True)
|
||||
|
||||
if title:
|
||||
draw_text_box(rl, title, 0, 60, 32, gui_app, font, font_scale, center=True)
|
||||
|
||||
if show_time:
|
||||
t = start_time + frame_idx / FRAMERATE
|
||||
time_text = f"{int(t)//60:02d}:{int(t)%60:02d}"
|
||||
time_width = int(rl.measure_text_ex(font, time_text, 24 * font_scale, 0).x)
|
||||
draw_text_box(rl, time_text, gui_app.width - time_width - 45, 45, 24, gui_app, font, font_scale)
|
||||
|
||||
|
||||
def prefetch_nav_tiles(road_view, ui_state, message_chunks) -> None:
|
||||
"""If the route drove with on-screen maps enabled, feed the panel the first messages and block
|
||||
until the opening viewport's tiles are fetched, so the clip doesn't start on the placeholder grid."""
|
||||
nav_panel = getattr(getattr(road_view, "_hud_renderer", None), "nav_map_panel", None)
|
||||
if nav_panel is None or not getattr(nav_panel, "maps_enabled", lambda: False)():
|
||||
return
|
||||
|
||||
logger.info("Route has on-screen maps enabled, prefetching map tiles...")
|
||||
for _ in range(min(len(message_chunks), FRAMERATE * 2)):
|
||||
ui_state.sm.update()
|
||||
nav_panel.update()
|
||||
if nav_panel.active:
|
||||
break
|
||||
if nav_panel.active:
|
||||
if not nav_panel.warm_up_tiles():
|
||||
logger.warning("Map tiles incomplete after warmup (missing MapboxToken or slow network); map may render partially")
|
||||
else:
|
||||
logger.warning("No nav position in the first seconds of the clip; map tiles will load mid-clip")
|
||||
ui_state.sm.frame = 0
|
||||
|
||||
|
||||
def clip(route: Route, output: str, start: int, end: int, headless: bool = True, big: bool = False,
|
||||
title: str | None = None, show_metadata: bool = True, show_time: bool = True, use_qcam: bool = False):
|
||||
timer, duration = Timer(), end - start
|
||||
|
||||
# The prefix must wrap the UI imports: ui_state and the nav map panel bind Params() to the
|
||||
# params path active when they're constructed, and load_route_metadata below seeds the route's
|
||||
# params (on-screen maps gate, units, Mapbox token) into the prefixed dir.
|
||||
with OpenpilotPrefix(shared_download_cache=True):
|
||||
import pyray as rl
|
||||
if big:
|
||||
from openpilot.selfdrive.ui.onroad.augmented_road_view import AugmentedRoadView
|
||||
else:
|
||||
from openpilot.selfdrive.ui.mici.onroad.augmented_road_view import AugmentedRoadView # type: ignore[assignment]
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
from openpilot.system.ui.lib.application import gui_app, FontWeight, FONT_SCALE
|
||||
timer.lap("import")
|
||||
|
||||
logger.info(f"Clipping {route.name.canonical_name}, {start}s-{end}s ({duration}s)")
|
||||
seg_start, seg_end = start // 60, (end - 1) // 60 + 1
|
||||
all_chunks = load_logs_parallel(route.log_paths()[seg_start:seg_end], fps=FRAMERATE)
|
||||
timer.lap("logs")
|
||||
|
||||
frame_start = (start - seg_start * 60) * FRAMERATE
|
||||
message_chunks = all_chunks[frame_start:frame_start + duration * FRAMERATE]
|
||||
if not message_chunks:
|
||||
logger.error("No messages to render")
|
||||
sys.exit(1)
|
||||
|
||||
metadata = load_route_metadata(route)
|
||||
if not show_metadata:
|
||||
metadata = None
|
||||
if headless:
|
||||
rl.set_config_flags(rl.ConfigFlags.FLAG_WINDOW_HIDDEN)
|
||||
|
||||
camera_paths = route.qcamera_paths() if use_qcam else route.camera_paths()
|
||||
frame_queue = FrameQueue(camera_paths, start, end, fps=FRAMERATE, use_qcam=use_qcam)
|
||||
|
||||
ecamera_paths = route.ecamera_paths() if not use_qcam else []
|
||||
wide_frame_queue: FrameQueue | None = None
|
||||
if any(p for p in ecamera_paths[seg_start:seg_end] if p):
|
||||
wide_frame_queue = FrameQueue(ecamera_paths, start, end, fps=FRAMERATE)
|
||||
|
||||
vipc = VisionIpcServer("camerad")
|
||||
vipc.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 4, frame_queue.frame_w, frame_queue.frame_h)
|
||||
if wide_frame_queue:
|
||||
vipc.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 4, wide_frame_queue.frame_w, wide_frame_queue.frame_h)
|
||||
vipc.start_listener()
|
||||
|
||||
patch_submaster(message_chunks, ui_state)
|
||||
gui_app.init_window("clip", fps=FRAMERATE)
|
||||
|
||||
road_view = AugmentedRoadView()
|
||||
road_view.set_rect(rl.Rectangle(0, 0, gui_app.width, gui_app.height))
|
||||
font = gui_app.font(FontWeight.NORMAL)
|
||||
prefetch_nav_tiles(road_view, ui_state, message_chunks)
|
||||
timer.lap("setup")
|
||||
|
||||
frame_idx = 0
|
||||
with tqdm.tqdm(total=len(message_chunks), desc="Rendering", unit="frame") as pbar:
|
||||
for should_render in gui_app.render():
|
||||
if frame_idx >= len(message_chunks):
|
||||
break
|
||||
_, frame_bytes = frame_queue.get()
|
||||
vipc.send(VisionStreamType.VISION_STREAM_ROAD, frame_bytes, frame_idx, int(frame_idx * 5e7), int(frame_idx * 5e7))
|
||||
if wide_frame_queue:
|
||||
_, wide_bytes = wide_frame_queue.get()
|
||||
vipc.send(VisionStreamType.VISION_STREAM_WIDE_ROAD, wide_bytes, frame_idx, int(frame_idx * 5e7), int(frame_idx * 5e7))
|
||||
ui_state.update()
|
||||
if should_render:
|
||||
road_view.render()
|
||||
render_overlays(rl, gui_app, font, FONT_SCALE, metadata, title, start, frame_idx, show_metadata, show_time)
|
||||
frame_idx += 1
|
||||
pbar.update(1)
|
||||
timer.lap("render")
|
||||
|
||||
frame_queue.stop()
|
||||
if wide_frame_queue:
|
||||
wide_frame_queue.stop()
|
||||
gui_app.close()
|
||||
timer.lap("ffmpeg")
|
||||
|
||||
logger.info(f"Clip saved to: {Path(output).resolve()}")
|
||||
logger.info(f"Generated {timer.fmt(duration)}")
|
||||
|
||||
|
||||
def main():
|
||||
logging.basicConfig(level=logging.INFO, format="%(asctime)s %(levelname)s\t%(message)s")
|
||||
args = parse_args()
|
||||
route = Route(args.route, data_dir=args.data_dir)
|
||||
big = args.big if args.big is not None else detect_big_ui(route)
|
||||
|
||||
setup_env(args.output, big=big, speed=args.speed, target_mb=args.file_size, duration=args.end - args.start)
|
||||
try:
|
||||
clip(route, args.output, args.start, args.end, not args.windowed,
|
||||
big, args.title, not args.no_metadata, not args.no_time_overlay, args.qcam)
|
||||
except TimeoutError as e:
|
||||
logger.error("%s", e)
|
||||
sys.exit(1)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
76
tools/diagnostics/lag_probe.py
Normal file
76
tools/diagnostics/lag_probe.py
Normal file
@@ -0,0 +1,76 @@
|
||||
#!/usr/bin/env python3
|
||||
import argparse
|
||||
import statistics
|
||||
import time
|
||||
|
||||
import cereal.messaging as messaging
|
||||
|
||||
|
||||
SERVICES = [
|
||||
"carState",
|
||||
"selfdriveState",
|
||||
"controlsState",
|
||||
"modelV2",
|
||||
"uiDebug",
|
||||
"liveCalibration",
|
||||
]
|
||||
|
||||
|
||||
def summarize(values: list[float]) -> str:
|
||||
if not values:
|
||||
return "n=0"
|
||||
return f"n={len(values)} avg_ms={statistics.fmean(values):.2f} max_ms={max(values):.2f}"
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser(description="On-device runtime lag probe")
|
||||
parser.add_argument("--seconds", type=float, default=15.0, help="Sampling window")
|
||||
args = parser.parse_args()
|
||||
|
||||
sm = messaging.SubMaster(SERVICES)
|
||||
last_seen: dict[str, float] = {}
|
||||
gaps: dict[str, list[float]] = {service: [] for service in SERVICES}
|
||||
ui_draw_times: list[float] = []
|
||||
car_cum_lag: list[float] = []
|
||||
model_frame_drop: list[float] = []
|
||||
|
||||
deadline = time.monotonic() + args.seconds
|
||||
while time.monotonic() < deadline:
|
||||
sm.update(100)
|
||||
now = time.monotonic()
|
||||
for service in SERVICES:
|
||||
if not sm.updated[service]:
|
||||
continue
|
||||
previous = last_seen.get(service)
|
||||
if previous is not None:
|
||||
gaps[service].append((now - previous) * 1000.0)
|
||||
last_seen[service] = now
|
||||
|
||||
if sm.updated["uiDebug"]:
|
||||
ui_draw_times.append(float(sm["uiDebug"].drawTimeMillis))
|
||||
if sm.updated["carState"]:
|
||||
car_cum_lag.append(float(sm["carState"].cumLagMs))
|
||||
if sm.updated["modelV2"]:
|
||||
model_frame_drop.append(float(sm["modelV2"].frameDropPerc))
|
||||
|
||||
print("Lag probe summary")
|
||||
for service in SERVICES:
|
||||
print(f"{service}: {summarize(gaps[service])}")
|
||||
|
||||
print(f"uiDebug.drawTimeMillis: {summarize(ui_draw_times)}")
|
||||
print(f"carState.cumLagMs: {summarize(car_cum_lag)}")
|
||||
print(f"modelV2.frameDropPerc: {summarize(model_frame_drop)}")
|
||||
|
||||
if sm.seen["liveCalibration"]:
|
||||
live_calib = sm["liveCalibration"]
|
||||
print(
|
||||
"liveCalibration:"
|
||||
f" status={int(live_calib.calStatus)}"
|
||||
f" calPerc={int(live_calib.calPerc)}"
|
||||
f" rpy={list(live_calib.rpyCalib)}"
|
||||
f" spread={list(live_calib.rpyCalibSpread)}"
|
||||
)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
285
tools/diagnostics/nav_lag_matrix.py
Normal file
285
tools/diagnostics/nav_lag_matrix.py
Normal file
@@ -0,0 +1,285 @@
|
||||
#!/usr/bin/env python3
|
||||
import argparse
|
||||
import json
|
||||
import os
|
||||
import re
|
||||
import shlex
|
||||
import subprocess
|
||||
import tempfile
|
||||
import time
|
||||
from collections import defaultdict
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
REPO_ROOT = Path(__file__).resolve().parents[2]
|
||||
VIDEO_AUDIT = REPO_ROOT / "tools" / "diagnostics" / "video_lag_audit.py"
|
||||
|
||||
NAV_ALL_FALSE = {
|
||||
"allow_mapd": False,
|
||||
"allow_offline_fallback": False,
|
||||
"allow_offline_routing": False,
|
||||
"allow_route_updates": False,
|
||||
"allow_live_data": False,
|
||||
"allow_nav_state": False,
|
||||
"allow_render": False,
|
||||
"allow_nav_influence": False,
|
||||
"allow_on_screen_navigation": False,
|
||||
"allow_lane_position": False,
|
||||
}
|
||||
|
||||
SCENARIOS: dict[str, dict | None] = {
|
||||
"default": None,
|
||||
"all_false": NAV_ALL_FALSE,
|
||||
"no_nav_state": {"allow_nav_state": False},
|
||||
"no_render": {"allow_render": False},
|
||||
"no_live_data": {"allow_live_data": False},
|
||||
"no_route_updates": {"allow_route_updates": False},
|
||||
"no_mapd_offline_fallback": {"allow_mapd": False, "allow_offline_fallback": False},
|
||||
"no_offline_routing": {"allow_offline_routing": False},
|
||||
"no_influence_lane": {"allow_nav_influence": False, "allow_lane_position": False},
|
||||
"no_onscreen": {"allow_on_screen_navigation": False},
|
||||
}
|
||||
|
||||
LAG_PATTERNS = {
|
||||
"navd": re.compile(r"navd step slow total_ms=(?P<total>[0-9.]+)"),
|
||||
"card": re.compile(r"card step slow total_ms=(?P<total>[0-9.]+)"),
|
||||
"controlsd": re.compile(r"controlsd step slow total_ms=(?P<total>[0-9.]+)"),
|
||||
"selfdrived": re.compile(r"selfdrived step slow total_ms=(?P<total>[0-9.]+)"),
|
||||
"selfdrived_sample": re.compile(r"selfdrived sample slow total_ms=(?P<total>[0-9.]+)"),
|
||||
}
|
||||
|
||||
|
||||
def run(cmd: list[str], *, check: bool = True, capture: bool = True, cwd: Path | None = None) -> str:
|
||||
result = subprocess.run(
|
||||
cmd,
|
||||
cwd=cwd,
|
||||
check=check,
|
||||
capture_output=capture,
|
||||
text=True,
|
||||
)
|
||||
return result.stdout if capture else ""
|
||||
|
||||
|
||||
def ssh(host: str, command: str, *, check: bool = True) -> str:
|
||||
return run(["ssh", "-o", "BatchMode=yes", "-o", "ConnectTimeout=8", host, command], check=check)
|
||||
|
||||
|
||||
def scp_from(host: str, remote_path: str, local_path: Path) -> None:
|
||||
local_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
subprocess.run(
|
||||
["scp", "-q", f"{host}:{remote_path}", str(local_path)],
|
||||
check=True,
|
||||
capture_output=True,
|
||||
text=True,
|
||||
)
|
||||
|
||||
|
||||
def remote_write_json(host: str, remote_path: str, payload: dict) -> None:
|
||||
encoded = json.dumps(payload, sort_keys=True)
|
||||
ssh(host, f"cat > {shlex.quote(remote_path)} <<'EOF'\n{encoded}\nEOF")
|
||||
|
||||
|
||||
def remote_remove(host: str, remote_path: str) -> None:
|
||||
ssh(host, f"rm -f {shlex.quote(remote_path)}")
|
||||
|
||||
|
||||
def set_nav_flags(host: str, flags: dict | None) -> None:
|
||||
remote_path = "/data/params/d/NavigationDebugFlags"
|
||||
if flags is None:
|
||||
remote_remove(host, remote_path)
|
||||
else:
|
||||
remote_write_json(host, remote_path, flags)
|
||||
|
||||
|
||||
def set_screen_recording(host: str, enabled: bool) -> None:
|
||||
value = "1" if enabled else "0"
|
||||
ssh(host, f"printf '{value}' > /data/params/d/ScreenRecording")
|
||||
|
||||
|
||||
def clear_issue_debug(host: str) -> None:
|
||||
ssh(host, "mkdir -p /data/community && : > /data/community/iqpilot_issue_debug.txt")
|
||||
|
||||
|
||||
def list_screen_recordings(host: str) -> list[str]:
|
||||
output = ssh(host, "ls -1t /data/media/0/screen_recordings/*.mp4 2>/dev/null || true")
|
||||
return [line.strip() for line in output.splitlines() if line.strip()]
|
||||
|
||||
|
||||
def newest_recording_after(host: str, before: set[str]) -> str | None:
|
||||
after = list_screen_recordings(host)
|
||||
for candidate in after:
|
||||
if candidate not in before:
|
||||
return candidate
|
||||
return after[0] if after else None
|
||||
|
||||
|
||||
def fetch_issue_debug(host: str, output_dir: Path) -> Path:
|
||||
local_path = output_dir / "iqpilot_issue_debug.txt"
|
||||
scp_from(host, "/data/community/iqpilot_issue_debug.txt", local_path)
|
||||
return local_path
|
||||
|
||||
|
||||
def parse_issue_debug(path: Path) -> dict:
|
||||
counts = defaultdict(int)
|
||||
maxima = defaultdict(float)
|
||||
calibration_lines = 0
|
||||
|
||||
if not path.exists():
|
||||
return {"counts": {}, "max_total_ms": {}, "calibration_lines": 0}
|
||||
|
||||
for line in path.read_text(errors="replace").splitlines():
|
||||
if "calibrationd" in line:
|
||||
calibration_lines += 1
|
||||
for key, pattern in LAG_PATTERNS.items():
|
||||
match = pattern.search(line)
|
||||
if match:
|
||||
counts[key] += 1
|
||||
maxima[key] = max(maxima[key], float(match.group("total")))
|
||||
|
||||
return {
|
||||
"counts": dict(counts),
|
||||
"max_total_ms": dict(maxima),
|
||||
"calibration_lines": calibration_lines,
|
||||
}
|
||||
|
||||
|
||||
def run_remote_demo(host: str, scenario_dir: str, fixture: str, provider: str) -> str:
|
||||
cmd = (
|
||||
f"cd /data/openpilot && "
|
||||
f"scripts/iqpilot/run_device_nav_demo.sh --fixture {shlex.quote(fixture)} "
|
||||
f"--provider {shlex.quote(provider)} --output-dir {shlex.quote(scenario_dir)} --no-gif"
|
||||
)
|
||||
return ssh(host, cmd)
|
||||
|
||||
|
||||
def run_remote_lag_probe(host: str, seconds: float, output_path: str) -> None:
|
||||
cmd = (
|
||||
"cd /data/openpilot && "
|
||||
f"PYTHONPATH=. python3 tools/diagnostics/lag_probe.py --seconds {seconds:.1f} > {shlex.quote(output_path)} 2>&1"
|
||||
)
|
||||
ssh(host, cmd)
|
||||
|
||||
|
||||
def fetch_latest_demo_video(host: str, scenario_dir: str, output_dir: Path) -> Path | None:
|
||||
remote_video = f"{scenario_dir}/nav_demo.mp4"
|
||||
try:
|
||||
local_path = output_dir / "nav_demo.mp4"
|
||||
scp_from(host, remote_video, local_path)
|
||||
return local_path
|
||||
except subprocess.CalledProcessError:
|
||||
return None
|
||||
|
||||
|
||||
def fetch_remote_file(host: str, remote_path: str, output_dir: Path, local_name: str) -> Path | None:
|
||||
local_path = output_dir / local_name
|
||||
try:
|
||||
scp_from(host, remote_path, local_path)
|
||||
return local_path
|
||||
except subprocess.CalledProcessError:
|
||||
return None
|
||||
|
||||
|
||||
def summarize_video(video_path: Path, output_dir: Path) -> dict | None:
|
||||
if not video_path or not video_path.exists():
|
||||
return None
|
||||
payload = run([
|
||||
"python3",
|
||||
str(VIDEO_AUDIT),
|
||||
str(video_path),
|
||||
"--output-dir",
|
||||
str(output_dir / "video_audit"),
|
||||
])
|
||||
return json.loads(payload)
|
||||
|
||||
|
||||
def run_live_capture(host: str, seconds: float) -> tuple[str | None, str | None]:
|
||||
before = set(list_screen_recordings(host))
|
||||
set_screen_recording(host, True)
|
||||
try:
|
||||
time.sleep(seconds)
|
||||
finally:
|
||||
set_screen_recording(host, False)
|
||||
time.sleep(3.0)
|
||||
remote_video = newest_recording_after(host, before)
|
||||
probe_remote = "/data/community/nav_lag_probe.txt"
|
||||
try:
|
||||
run_remote_lag_probe(host, min(seconds, 20.0), probe_remote)
|
||||
except subprocess.CalledProcessError:
|
||||
probe_remote = None
|
||||
return remote_video, probe_remote
|
||||
|
||||
|
||||
def scenario_flags(name: str) -> dict | None:
|
||||
if name not in SCENARIOS:
|
||||
raise KeyError(f"unknown scenario: {name}")
|
||||
return SCENARIOS[name]
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser(description="Run nav lag feature matrix on a comma device and collect videos/logs.")
|
||||
parser.add_argument("--host", default="arman3x", help="SSH host alias")
|
||||
parser.add_argument("--mode", choices=["live", "demo"], default="live", help="Capture live screen recording or deterministic UI nav demo")
|
||||
parser.add_argument("--duration", type=float, default=20.0, help="Live capture duration in seconds")
|
||||
parser.add_argument("--fixture", default="bolingbrook-carol-stream", help="Fixture alias/path for demo mode")
|
||||
parser.add_argument("--provider", default="offline", choices=["offline", "cached", "mapbox"], help="Provider for demo mode")
|
||||
parser.add_argument("--scenarios", nargs="+", default=["default", "all_false"], help="Scenario names to run")
|
||||
parser.add_argument("--output-dir", type=Path, default=Path("nav_lag_matrix_runs"), help="Local artifact directory")
|
||||
args = parser.parse_args()
|
||||
|
||||
run_root = args.output_dir / time.strftime("%Y%m%d_%H%M%S")
|
||||
run_root.mkdir(parents=True, exist_ok=True)
|
||||
summary = {
|
||||
"host": args.host,
|
||||
"mode": args.mode,
|
||||
"fixture": args.fixture,
|
||||
"provider": args.provider,
|
||||
"scenarios": [],
|
||||
}
|
||||
|
||||
for scenario_name in args.scenarios:
|
||||
flags = scenario_flags(scenario_name)
|
||||
scenario_dir = run_root / scenario_name
|
||||
scenario_dir.mkdir(parents=True, exist_ok=True)
|
||||
clear_issue_debug(args.host)
|
||||
set_nav_flags(args.host, flags)
|
||||
time.sleep(2.0)
|
||||
|
||||
remote_probe = None
|
||||
local_video = None
|
||||
if args.mode == "demo":
|
||||
remote_dir = f"/data/nav_demo_tests/{scenario_name}_{int(time.time())}"
|
||||
demo_stdout = run_remote_demo(args.host, remote_dir, args.fixture, args.provider)
|
||||
(scenario_dir / "demo_stdout.txt").write_text(demo_stdout)
|
||||
local_video = fetch_latest_demo_video(args.host, remote_dir, scenario_dir)
|
||||
else:
|
||||
remote_video, remote_probe = run_live_capture(args.host, args.duration)
|
||||
if remote_video:
|
||||
local_video = fetch_remote_file(args.host, remote_video, scenario_dir, Path(remote_video).name)
|
||||
|
||||
iqdebug_path = fetch_issue_debug(args.host, scenario_dir)
|
||||
probe_path = None
|
||||
if remote_probe:
|
||||
probe_path = fetch_remote_file(args.host, remote_probe, scenario_dir, "lag_probe.txt")
|
||||
|
||||
video_summary = summarize_video(local_video, scenario_dir) if local_video else None
|
||||
debug_summary = parse_issue_debug(iqdebug_path)
|
||||
|
||||
scenario_summary = {
|
||||
"scenario": scenario_name,
|
||||
"flags": flags,
|
||||
"video": str(local_video) if local_video else None,
|
||||
"probe": str(probe_path) if probe_path else None,
|
||||
"issue_debug": str(iqdebug_path),
|
||||
"debug_summary": debug_summary,
|
||||
"video_summary": video_summary,
|
||||
}
|
||||
summary["scenarios"].append(scenario_summary)
|
||||
(scenario_dir / "summary.json").write_text(json.dumps(scenario_summary, indent=2, sort_keys=True) + "\n")
|
||||
|
||||
summary_path = run_root / "summary.json"
|
||||
summary_path.write_text(json.dumps(summary, indent=2, sort_keys=True) + "\n")
|
||||
print(json.dumps(summary, indent=2, sort_keys=True))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
243
tools/diagnostics/video_lag_audit.py
Normal file
243
tools/diagnostics/video_lag_audit.py
Normal file
@@ -0,0 +1,243 @@
|
||||
#!/usr/bin/env python3
|
||||
import argparse
|
||||
import csv
|
||||
import hashlib
|
||||
import json
|
||||
import math
|
||||
import shutil
|
||||
import subprocess
|
||||
import tempfile
|
||||
from pathlib import Path
|
||||
|
||||
try:
|
||||
from PIL import Image, ImageChops, ImageStat
|
||||
except ModuleNotFoundError:
|
||||
Image = None
|
||||
ImageChops = None
|
||||
ImageStat = None
|
||||
|
||||
|
||||
DEFAULT_PLANNER_ROI = (0.22, 0.54, 0.78, 0.97)
|
||||
|
||||
|
||||
def run(cmd: list[str]) -> str:
|
||||
result = subprocess.run(cmd, check=True, capture_output=True, text=True)
|
||||
return result.stdout
|
||||
|
||||
|
||||
def ffprobe_video(path: Path) -> dict:
|
||||
payload = run([
|
||||
"ffprobe",
|
||||
"-v", "error",
|
||||
"-print_format", "json",
|
||||
"-show_streams",
|
||||
"-show_format",
|
||||
str(path),
|
||||
])
|
||||
return json.loads(payload)
|
||||
|
||||
|
||||
def parse_fraction(value: str) -> float:
|
||||
if "/" in value:
|
||||
num, den = value.split("/", 1)
|
||||
return float(num) / float(den)
|
||||
return float(value)
|
||||
|
||||
|
||||
def extract_frames(video_path: Path, output_dir: Path) -> list[Path]:
|
||||
output_dir.mkdir(parents=True, exist_ok=True)
|
||||
run([
|
||||
"ffmpeg",
|
||||
"-loglevel", "error",
|
||||
"-i", str(video_path),
|
||||
"-vsync", "0",
|
||||
str(output_dir / "frame_%06d.png"),
|
||||
"-y",
|
||||
])
|
||||
return sorted(output_dir.glob("frame_*.png"))
|
||||
|
||||
|
||||
def file_hash(path: Path) -> str:
|
||||
digest = hashlib.sha256()
|
||||
with path.open("rb") as f:
|
||||
for chunk in iter(lambda: f.read(1024 * 1024), b""):
|
||||
digest.update(chunk)
|
||||
return digest.hexdigest()
|
||||
|
||||
|
||||
def crop_box(width: int, height: int, roi: tuple[float, float, float, float]) -> tuple[int, int, int, int]:
|
||||
left = int(width * roi[0])
|
||||
top = int(height * roi[1])
|
||||
right = int(width * roi[2])
|
||||
bottom = int(height * roi[3])
|
||||
return left, top, right, bottom
|
||||
|
||||
|
||||
def rms_diff(prev_img, cur_img) -> float:
|
||||
diff = ImageChops.difference(prev_img, cur_img)
|
||||
return float(ImageStat.Stat(diff).rms[0])
|
||||
|
||||
|
||||
def summarize(values: list[float]) -> dict[str, float]:
|
||||
if not values:
|
||||
return {"count": 0, "avg": 0.0, "max": 0.0, "min": 0.0}
|
||||
return {
|
||||
"count": len(values),
|
||||
"avg": sum(values) / len(values),
|
||||
"max": max(values),
|
||||
"min": min(values),
|
||||
}
|
||||
|
||||
|
||||
def write_contact_sheet(flagged: list[Path], output_path: Path, *, columns: int = 3) -> None:
|
||||
if Image is None or not flagged:
|
||||
return
|
||||
|
||||
images = []
|
||||
for path in flagged:
|
||||
with Image.open(path) as img:
|
||||
images.append(img.convert("RGB").copy())
|
||||
|
||||
thumb_w = min(img.width for img in images)
|
||||
thumb_h = min(img.height for img in images)
|
||||
rows = math.ceil(len(images) / columns)
|
||||
sheet = Image.new("RGB", (thumb_w * columns, thumb_h * rows), color=(0, 0, 0))
|
||||
|
||||
for idx, img in enumerate(images):
|
||||
thumb = img.resize((thumb_w, thumb_h))
|
||||
x = (idx % columns) * thumb_w
|
||||
y = (idx // columns) * thumb_h
|
||||
sheet.paste(thumb, (x, y))
|
||||
|
||||
sheet.save(output_path)
|
||||
|
||||
|
||||
def audit_video(video_path: Path, output_dir: Path, roi: tuple[float, float, float, float]) -> dict:
|
||||
output_dir.mkdir(parents=True, exist_ok=True)
|
||||
temp_root = Path(tempfile.mkdtemp(prefix="iqpilot_video_audit_"))
|
||||
try:
|
||||
frames = extract_frames(video_path, temp_root / "frames")
|
||||
probe = ffprobe_video(video_path)
|
||||
streams = probe.get("streams", [])
|
||||
video_stream = next((stream for stream in streams if stream.get("codec_type") == "video"), {})
|
||||
fps = parse_fraction(video_stream.get("avg_frame_rate", "0")) if video_stream.get("avg_frame_rate") else 0.0
|
||||
duration = float(probe.get("format", {}).get("duration", 0.0) or 0.0)
|
||||
|
||||
records: list[dict] = []
|
||||
duplicate_runs: list[int] = []
|
||||
current_duplicate_run = 0
|
||||
exact_duplicate_indices: list[int] = []
|
||||
low_motion_indices: list[int] = []
|
||||
suspicious_paths: list[Path] = []
|
||||
full_rms_values: list[float] = []
|
||||
planner_rms_values: list[float] = []
|
||||
|
||||
prev_hash = None
|
||||
prev_img = None
|
||||
prev_planner = None
|
||||
|
||||
for idx, frame_path in enumerate(frames):
|
||||
frame_hash = file_hash(frame_path)
|
||||
exact_duplicate = prev_hash == frame_hash
|
||||
|
||||
full_rms = 0.0
|
||||
planner_rms = 0.0
|
||||
if Image is not None:
|
||||
with Image.open(frame_path) as img:
|
||||
current = img.convert("L")
|
||||
planner = current.crop(crop_box(current.width, current.height, roi))
|
||||
if prev_img is not None:
|
||||
full_rms = rms_diff(prev_img, current)
|
||||
planner_rms = rms_diff(prev_planner, planner)
|
||||
full_rms_values.append(full_rms)
|
||||
planner_rms_values.append(planner_rms)
|
||||
prev_img = current.copy()
|
||||
prev_planner = planner.copy()
|
||||
|
||||
if exact_duplicate:
|
||||
current_duplicate_run += 1
|
||||
exact_duplicate_indices.append(idx)
|
||||
elif current_duplicate_run:
|
||||
duplicate_runs.append(current_duplicate_run)
|
||||
current_duplicate_run = 0
|
||||
|
||||
if idx > 0 and (exact_duplicate or planner_rms < 1.2 or full_rms < 1.0):
|
||||
low_motion_indices.append(idx)
|
||||
suspicious_paths.append(frame_path)
|
||||
|
||||
records.append({
|
||||
"frame": idx,
|
||||
"exact_duplicate": exact_duplicate,
|
||||
"full_rms": round(full_rms, 4),
|
||||
"planner_rms": round(planner_rms, 4),
|
||||
})
|
||||
prev_hash = frame_hash
|
||||
|
||||
if current_duplicate_run:
|
||||
duplicate_runs.append(current_duplicate_run)
|
||||
|
||||
csv_path = output_dir / f"{video_path.stem}_frame_metrics.csv"
|
||||
with csv_path.open("w", newline="") as f:
|
||||
writer = csv.DictWriter(f, fieldnames=["frame", "exact_duplicate", "full_rms", "planner_rms"])
|
||||
writer.writeheader()
|
||||
writer.writerows(records)
|
||||
|
||||
flagged_dir = output_dir / f"{video_path.stem}_flagged_frames"
|
||||
flagged_dir.mkdir(parents=True, exist_ok=True)
|
||||
for path in suspicious_paths[:24]:
|
||||
shutil.copy2(path, flagged_dir / path.name)
|
||||
|
||||
write_contact_sheet(sorted(flagged_dir.glob("*.png"))[:12], output_dir / f"{video_path.stem}_contact_sheet.png")
|
||||
|
||||
summary = {
|
||||
"video": str(video_path),
|
||||
"frame_count": len(frames),
|
||||
"fps": fps,
|
||||
"duration_s": duration,
|
||||
"exact_duplicate_frames": len(exact_duplicate_indices),
|
||||
"max_duplicate_run": max(duplicate_runs) if duplicate_runs else 0,
|
||||
"duplicate_runs": duplicate_runs,
|
||||
"low_motion_frames": len(low_motion_indices),
|
||||
"full_rms": summarize(full_rms_values),
|
||||
"planner_rms": summarize(planner_rms_values),
|
||||
"planner_roi": {
|
||||
"left": roi[0],
|
||||
"top": roi[1],
|
||||
"right": roi[2],
|
||||
"bottom": roi[3],
|
||||
},
|
||||
"artifacts": {
|
||||
"metrics_csv": str(csv_path),
|
||||
"flagged_frames_dir": str(flagged_dir),
|
||||
"contact_sheet": str(output_dir / f"{video_path.stem}_contact_sheet.png"),
|
||||
},
|
||||
}
|
||||
|
||||
summary_path = output_dir / f"{video_path.stem}_summary.json"
|
||||
summary_path.write_text(json.dumps(summary, indent=2, sort_keys=True) + "\n")
|
||||
return summary
|
||||
finally:
|
||||
shutil.rmtree(temp_root, ignore_errors=True)
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser(description="Audit an MP4 for duplicate/low-motion frame runs.")
|
||||
parser.add_argument("video", type=Path, help="Input MP4")
|
||||
parser.add_argument("--output-dir", type=Path, default=Path("video_audit"), help="Artifact output directory")
|
||||
parser.add_argument(
|
||||
"--planner-roi",
|
||||
default="0.22,0.54,0.78,0.97",
|
||||
help="ROI fractions left,top,right,bottom for planner-focused RMS stats",
|
||||
)
|
||||
args = parser.parse_args()
|
||||
|
||||
roi = tuple(float(part.strip()) for part in args.planner_roi.split(","))
|
||||
if len(roi) != 4:
|
||||
raise ValueError("planner-roi must have four comma-separated floats")
|
||||
|
||||
summary = audit_video(args.video, args.output_dir, roi)
|
||||
print(json.dumps(summary, indent=2, sort_keys=True))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
29
tools/install_python_dependencies.sh
Executable file
29
tools/install_python_dependencies.sh
Executable file
@@ -0,0 +1,29 @@
|
||||
#!/usr/bin/env bash
|
||||
set -euo pipefail
|
||||
|
||||
# Increase the pip timeout to handle TimeoutError
|
||||
export PIP_DEFAULT_TIMEOUT=200
|
||||
|
||||
DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )"
|
||||
ROOT="$DIR"/../
|
||||
cd "$ROOT"
|
||||
|
||||
if ! command -v "uv" > /dev/null 2>&1; then
|
||||
echo "installing uv..."
|
||||
curl -LsSf --retry 5 --retry-delay 5 --retry-all-errors https://astral.sh/uv/install.sh | sh
|
||||
UV_BIN="$HOME/.local/bin"
|
||||
PATH="$UV_BIN:$PATH"
|
||||
fi
|
||||
|
||||
echo "updating uv..."
|
||||
# ok to fail, can also fail due to installing with brew
|
||||
uv self update || true
|
||||
|
||||
echo "installing python packages..."
|
||||
uv sync --frozen --all-extras
|
||||
source .venv/bin/activate
|
||||
|
||||
if [[ "$(uname)" == 'Darwin' ]]; then
|
||||
touch "$ROOT"/.env
|
||||
echo "export OBJC_DISABLE_INITIALIZE_FORK_SAFETY=YES" >> "$ROOT"/.env
|
||||
fi
|
||||
132
tools/install_ubuntu_dependencies.sh
Executable file
132
tools/install_ubuntu_dependencies.sh
Executable file
@@ -0,0 +1,132 @@
|
||||
#!/usr/bin/env bash
|
||||
set -e
|
||||
|
||||
SUDO=""
|
||||
|
||||
# Use sudo if not root
|
||||
if [[ ! $(id -u) -eq 0 ]]; then
|
||||
if [[ -z $(which sudo) ]]; then
|
||||
echo "Please install sudo or run as root"
|
||||
exit 1
|
||||
fi
|
||||
SUDO="sudo"
|
||||
fi
|
||||
|
||||
# Check if stdin is open
|
||||
if [ -t 0 ]; then
|
||||
INTERACTIVE=1
|
||||
fi
|
||||
|
||||
# Install common packages
|
||||
function install_ubuntu_common_requirements() {
|
||||
$SUDO apt-get update
|
||||
|
||||
# normal stuff, mostly for the bare docker image
|
||||
$SUDO apt-get install -y --no-install-recommends \
|
||||
ca-certificates \
|
||||
clang \
|
||||
build-essential \
|
||||
curl \
|
||||
libssl-dev \
|
||||
libcurl4-openssl-dev \
|
||||
locales \
|
||||
git \
|
||||
git-lfs \
|
||||
xvfb
|
||||
|
||||
# TODO: vendor the rest of these in third_party/
|
||||
$SUDO apt-get install -y --no-install-recommends \
|
||||
gcc-arm-none-eabi \
|
||||
capnproto \
|
||||
libcapnp-dev \
|
||||
ffmpeg \
|
||||
libavformat-dev \
|
||||
libavcodec-dev \
|
||||
libavdevice-dev \
|
||||
libavutil-dev \
|
||||
libavfilter-dev \
|
||||
libbz2-dev \
|
||||
libeigen3-dev \
|
||||
libffi-dev \
|
||||
libgles2-mesa-dev \
|
||||
libglfw3-dev \
|
||||
libglib2.0-0 \
|
||||
libjpeg-dev \
|
||||
libqt5charts5-dev \
|
||||
libncurses5-dev \
|
||||
libusb-1.0-0-dev \
|
||||
libzmq3-dev \
|
||||
libzstd-dev \
|
||||
libsqlite3-dev \
|
||||
opencl-headers \
|
||||
ocl-icd-libopencl1 \
|
||||
ocl-icd-opencl-dev \
|
||||
portaudio19-dev \
|
||||
qttools5-dev-tools \
|
||||
libqt5svg5-dev \
|
||||
libqt5serialbus5-dev \
|
||||
libqt5x11extras5-dev \
|
||||
libqt5opengl5-dev \
|
||||
gettext
|
||||
}
|
||||
|
||||
# Install Ubuntu 24.04 LTS packages
|
||||
function install_ubuntu_lts_latest_requirements() {
|
||||
install_ubuntu_common_requirements
|
||||
|
||||
$SUDO apt-get install -y --no-install-recommends \
|
||||
g++-12 \
|
||||
qtbase5-dev \
|
||||
qtbase5-dev-tools \
|
||||
python3-dev \
|
||||
python3-venv
|
||||
}
|
||||
|
||||
# Detect OS using /etc/os-release file
|
||||
if [ -f "/etc/os-release" ]; then
|
||||
source /etc/os-release
|
||||
case "$VERSION_CODENAME" in
|
||||
"jammy" | "kinetic" | "noble")
|
||||
install_ubuntu_lts_latest_requirements
|
||||
;;
|
||||
*)
|
||||
echo "$ID $VERSION_ID is unsupported. This setup script is written for Ubuntu 24.04."
|
||||
read -p "Would you like to attempt installation anyway? " -n 1 -r
|
||||
echo ""
|
||||
if [[ ! $REPLY =~ ^[Yy]$ ]]; then
|
||||
exit 1
|
||||
fi
|
||||
install_ubuntu_lts_latest_requirements
|
||||
esac
|
||||
|
||||
if [[ -d "/etc/udev/rules.d/" ]]; then
|
||||
# Setup jungle udev rules
|
||||
$SUDO tee /etc/udev/rules.d/12-panda_jungle.rules > /dev/null <<EOF
|
||||
SUBSYSTEM=="usb", ATTRS{idVendor}=="3801", ATTRS{idProduct}=="ddcf", MODE="0666"
|
||||
SUBSYSTEM=="usb", ATTRS{idVendor}=="3801", ATTRS{idProduct}=="ddef", MODE="0666"
|
||||
SUBSYSTEM=="usb", ATTRS{idVendor}=="bbaa", ATTRS{idProduct}=="ddcf", MODE="0666"
|
||||
SUBSYSTEM=="usb", ATTRS{idVendor}=="bbaa", ATTRS{idProduct}=="ddef", MODE="0666"
|
||||
|
||||
EOF
|
||||
|
||||
# Setup panda udev rules
|
||||
$SUDO tee /etc/udev/rules.d/11-panda.rules > /dev/null <<EOF
|
||||
SUBSYSTEM=="usb", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="df11", MODE="0666"
|
||||
SUBSYSTEM=="usb", ATTRS{idVendor}=="3801", ATTRS{idProduct}=="ddcc", MODE="0666"
|
||||
SUBSYSTEM=="usb", ATTRS{idVendor}=="3801", ATTRS{idProduct}=="ddee", MODE="0666"
|
||||
SUBSYSTEM=="usb", ATTRS{idVendor}=="bbaa", ATTRS{idProduct}=="ddcc", MODE="0666"
|
||||
SUBSYSTEM=="usb", ATTRS{idVendor}=="bbaa", ATTRS{idProduct}=="ddee", MODE="0666"
|
||||
EOF
|
||||
|
||||
# Setup adb udev rules
|
||||
$SUDO tee /etc/udev/rules.d/50-comma-adb.rules > /dev/null <<EOF
|
||||
SUBSYSTEM=="usb", ATTR{idVendor}=="04d8", ATTR{idProduct}=="1234", ENV{adb_user}="yes"
|
||||
EOF
|
||||
|
||||
$SUDO udevadm control --reload-rules && $SUDO udevadm trigger || true
|
||||
fi
|
||||
|
||||
else
|
||||
echo "No /etc/os-release in the system. Make sure you're running on Ubuntu, or similar."
|
||||
exit 1
|
||||
fi
|
||||
54
tools/iqmacvisiond/README.md
Normal file
54
tools/iqmacvisiond/README.md
Normal file
@@ -0,0 +1,54 @@
|
||||
# iqmacvisiond — IQ Vision offload server
|
||||
|
||||
Runs the iqvd perception model on an Apple-Silicon Mac and serves 2D detections
|
||||
to one IQ device over wifi. The device (`iqvd`) ships the camera frame, the Mac
|
||||
runs YOLO on the Metal GPU/NPU, and only boxes come back — the device does no
|
||||
inference, so vision dots no longer contend with the driving model.
|
||||
|
||||
## Wire path
|
||||
|
||||
```
|
||||
IQ device (auto-hotspot AP) ──wifi──▶ Mac (IQ Vision.app)
|
||||
iqvd VisionClient iqmacvisiond server
|
||||
read frame → downscale 640w cv2 decode → YOLOv8n (Metal)
|
||||
JPEG encode → INFER ───────────────▶ detect
|
||||
RESULT ◀─────────────────────────── {tracks: [2D boxes]}
|
||||
publish iqVehicleTracks (dots)
|
||||
publish iqEnvironment (3D via ground-plane + calibration)
|
||||
```
|
||||
|
||||
- Discovery: the device UDP-broadcasts `IQVISION_DISCOVER_V1` on the subnet; the
|
||||
Mac replies `IQVISION_HERE_V1:<tcp_port>`. No config, no pairing.
|
||||
- Protocol: `iqvd_private_src/offload/protocol.py` (length-prefixed frames, JSON
|
||||
header + optional binary blob). Shared verbatim by both sides — it is the ABI.
|
||||
- Ports: tcp/51998 inference, udp/51999 discovery, tcp/51995 localhost status.
|
||||
|
||||
## The Mac app
|
||||
|
||||
- Menu-bar app (`◎` waiting, `◉` connected). Menu shows device, frames served,
|
||||
inference p50/p99, and **Quit**.
|
||||
- Keeps the Mac awake while running (`caffeinate`).
|
||||
- Ships the model in the dmg — no download on first run.
|
||||
- First launch creates a small venv (numpy, opencv-headless, rumps); tinygrad is
|
||||
bundled.
|
||||
|
||||
## Build
|
||||
|
||||
```
|
||||
tools/iqmacvisiond/macos/build_dmg.sh
|
||||
```
|
||||
|
||||
Produces `IQ Vision.app` and `IQVision.dmg`. See `macos/SIGNING.md` for signing +
|
||||
notarization.
|
||||
|
||||
## Run from source (dev)
|
||||
|
||||
```
|
||||
DEV=METAL python3 tools/iqmacvisiond/server.py # server only
|
||||
python3 tools/iqmacvisiond/menubar.py # menu-bar wrapper
|
||||
python3 tools/iqmacvisiond/test_offload.py # protocol/geometry/loopback
|
||||
```
|
||||
|
||||
Gating on the device: `VisionVehicleTracks` enables iqvd; when `maciqmodeld` (the
|
||||
eMac driving offload) is running, iqvd is Mac-or-nothing — it never runs local
|
||||
inference. On non-eMac setups iqvd falls back to on-device YOLO if no Mac is found.
|
||||
23
tools/iqmacvisiond/macos/SIGNING.md
Normal file
23
tools/iqmacvisiond/macos/SIGNING.md
Normal file
@@ -0,0 +1,23 @@
|
||||
# Signing & notarization — IQ Vision.app
|
||||
|
||||
Unsigned, the app runs after a right-click ▸ Open (Gatekeeper first-run). For
|
||||
distribution, sign + notarize:
|
||||
|
||||
```bash
|
||||
APP="tools/iqmacvisiond/macos/dist/IQ Vision.app"
|
||||
ENT="tools/iqmacvisiond/macos/entitlements.plist"
|
||||
IDENTITY="Developer ID Application: <YOUR NAME> (<TEAMID>)"
|
||||
|
||||
codesign --force --deep --options runtime --entitlements "$ENT" \
|
||||
--sign "$IDENTITY" "$APP"
|
||||
|
||||
hdiutil create -volname "IQ Vision" -srcfolder "$(dirname "$APP")" -ov -format UDZO \
|
||||
tools/iqmacvisiond/macos/dist/IQVision.dmg
|
||||
|
||||
xcrun notarytool submit tools/iqmacvisiond/macos/dist/IQVision.dmg \
|
||||
--apple-id "<APPLE_ID>" --team-id "<TEAMID>" --password "<APP_PW>" --wait
|
||||
xcrun stapler staple tools/iqmacvisiond/macos/dist/IQVision.dmg
|
||||
```
|
||||
|
||||
The entitlements cover: JIT + unsigned exec memory + library-validation off
|
||||
(tinygrad Metal JIT) and network server/client (LAN discovery + inference).
|
||||
59
tools/iqmacvisiond/macos/build_dmg.sh
Executable file
59
tools/iqmacvisiond/macos/build_dmg.sh
Executable file
@@ -0,0 +1,59 @@
|
||||
#!/bin/bash
|
||||
# Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
||||
set -euo pipefail
|
||||
|
||||
HERE="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
|
||||
REPO="$(cd "$HERE/../../.." >/dev/null && pwd)"
|
||||
OUT="${1:-$HERE/dist}"
|
||||
APP="$OUT/IQ Vision.app"
|
||||
|
||||
rm -rf "$OUT"
|
||||
mkdir -p "$APP/Contents/MacOS" "$APP/Contents/Resources"
|
||||
|
||||
cat > "$APP/Contents/Info.plist" <<'PLIST'
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!DOCTYPE plist PUBLIC "-//Apple//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
|
||||
<plist version="1.0">
|
||||
<dict>
|
||||
<key>CFBundleName</key><string>IQ Vision</string>
|
||||
<key>CFBundleIdentifier</key><string>com.iqpilot.iqvision</string>
|
||||
<key>CFBundleVersion</key><string>1.0</string>
|
||||
<key>CFBundleShortVersionString</key><string>1.0</string>
|
||||
<key>CFBundlePackageType</key><string>APPL</string>
|
||||
<key>CFBundleExecutable</key><string>iqvision</string>
|
||||
<key>LSMinimumSystemVersion</key><string>13.0</string>
|
||||
<key>LSUIElement</key><true/>
|
||||
</dict>
|
||||
</plist>
|
||||
PLIST
|
||||
|
||||
cat > "$APP/Contents/MacOS/iqvision" <<'LAUNCH'
|
||||
#!/bin/bash
|
||||
RES="$(cd "$(dirname "$0")/../Resources" && pwd)"
|
||||
if [ ! -x "$HOME/Library/Application Support/IQVision/venv/bin/python" ]; then
|
||||
osascript -e "tell application \"Terminal\"
|
||||
activate
|
||||
do script \"bash '$RES/macos/setup.sh'\"
|
||||
end tell"
|
||||
else
|
||||
exec bash "$RES/macos/setup.sh" >/tmp/iqvision.log 2>&1
|
||||
fi
|
||||
LAUNCH
|
||||
chmod +x "$APP/Contents/MacOS/iqvision"
|
||||
|
||||
RES="$APP/Contents/Resources"
|
||||
SRC="$RES/openpilot/iqpilot/iqvd_private_src"
|
||||
mkdir -p "$RES/tools/iqmacvisiond" "$RES/macos" "$SRC/offload" "$SRC/models"
|
||||
|
||||
cp "$REPO/tools/iqmacvisiond/server.py" "$REPO/tools/iqmacvisiond/menubar.py" "$RES/tools/iqmacvisiond/"
|
||||
cp "$HERE/setup.sh" "$RES/macos/"
|
||||
cp "$REPO/iqpilot/iqvd_private_src/__init__.py" "$REPO/iqpilot/iqvd_private_src/yolov8_net.py" "$SRC/"
|
||||
cp "$REPO/iqpilot/iqvd_private_src/offload/"*.py "$SRC/offload/"
|
||||
cp "$REPO/iqpilot/iqvd_private_src/models/yolov8n.safetensors" "$SRC/models/"
|
||||
touch "$RES/openpilot/__init__.py" "$RES/openpilot/iqpilot/__init__.py" "$RES/tools/__init__.py" \
|
||||
"$RES/tools/iqmacvisiond/__init__.py"
|
||||
rsync -a --exclude=".git" --exclude="__pycache__" --exclude="extra" --exclude="test" \
|
||||
--exclude="examples" --exclude="docs" "$REPO/tinygrad_repo/" "$RES/tinygrad_repo/"
|
||||
|
||||
hdiutil create -volname "IQ Vision" -srcfolder "$OUT" -ov -format UDZO "$OUT/IQVision.dmg" >/dev/null
|
||||
echo "built: $OUT/IQVision.dmg ($(du -h "$OUT/IQVision.dmg" | cut -f1))"
|
||||
11
tools/iqmacvisiond/macos/entitlements.plist
Normal file
11
tools/iqmacvisiond/macos/entitlements.plist
Normal file
@@ -0,0 +1,11 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!DOCTYPE plist PUBLIC "-//Apple//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
|
||||
<plist version="1.0">
|
||||
<dict>
|
||||
<key>com.apple.security.cs.allow-jit</key><true/>
|
||||
<key>com.apple.security.cs.allow-unsigned-executable-memory</key><true/>
|
||||
<key>com.apple.security.cs.disable-library-validation</key><true/>
|
||||
<key>com.apple.security.network.server</key><true/>
|
||||
<key>com.apple.security.network.client</key><true/>
|
||||
</dict>
|
||||
</plist>
|
||||
21
tools/iqmacvisiond/macos/setup.sh
Executable file
21
tools/iqmacvisiond/macos/setup.sh
Executable file
@@ -0,0 +1,21 @@
|
||||
#!/bin/bash
|
||||
# Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
||||
set -euo pipefail
|
||||
|
||||
RES="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." >/dev/null && pwd)"
|
||||
SUPPORT="$HOME/Library/Application Support/IQVision"
|
||||
VENV="$SUPPORT/venv"
|
||||
PY="$VENV/bin/python"
|
||||
|
||||
mkdir -p "$SUPPORT"
|
||||
|
||||
if [ ! -x "$PY" ]; then
|
||||
echo "Creating IQ Vision environment (one time)…"
|
||||
/usr/bin/python3 -m venv "$VENV"
|
||||
"$PY" -m pip install --upgrade --quiet pip
|
||||
"$PY" -m pip install --quiet numpy "opencv-python-headless>=4.8" rumps
|
||||
fi
|
||||
|
||||
export PYTHONPATH="$RES:$RES/tinygrad_repo"
|
||||
export DEV=METAL
|
||||
exec "$PY" "$RES/tools/iqmacvisiond/menubar.py"
|
||||
80
tools/iqmacvisiond/menubar.py
Executable file
80
tools/iqmacvisiond/menubar.py
Executable file
@@ -0,0 +1,80 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import os
|
||||
import subprocess
|
||||
import sys
|
||||
import time
|
||||
import urllib.request
|
||||
from pathlib import Path
|
||||
|
||||
import rumps
|
||||
|
||||
HERE = Path(__file__).resolve().parent
|
||||
STATUS_URL = "http://127.0.0.1:51995/status.json"
|
||||
DASHBOARD_URL = "http://127.0.0.1:51995"
|
||||
|
||||
|
||||
class IQVisionApp(rumps.App):
|
||||
def __init__(self):
|
||||
super().__init__("IQ Vision", title="◎", quit_button=None)
|
||||
self.item_state = rumps.MenuItem("Starting…")
|
||||
self.item_device = rumps.MenuItem("Device: —")
|
||||
self.item_frames = rumps.MenuItem("Frames: —")
|
||||
self.item_exec = rumps.MenuItem("Inference: —")
|
||||
self.item_awake = rumps.MenuItem("Keep awake: —")
|
||||
self.menu = [
|
||||
self.item_state, None,
|
||||
self.item_device, self.item_frames, self.item_exec, self.item_awake, None,
|
||||
rumps.MenuItem("Open Dashboard", callback=self.open_dashboard),
|
||||
rumps.MenuItem("Quit IQ Vision", callback=self.quit_app),
|
||||
]
|
||||
self.proc: subprocess.Popen | None = None
|
||||
self._start_server()
|
||||
self.timer = rumps.Timer(self.refresh, 1)
|
||||
self.timer.start()
|
||||
|
||||
def _start_server(self) -> None:
|
||||
env = dict(os.environ)
|
||||
self.proc = subprocess.Popen([sys.executable, str(HERE / "server.py")], env=env)
|
||||
|
||||
def refresh(self, _) -> None:
|
||||
if self.proc is not None and self.proc.poll() is not None:
|
||||
self.title = "◎!"
|
||||
self.item_state.title = "Server stopped — reopen the app"
|
||||
return
|
||||
try:
|
||||
with urllib.request.urlopen(STATUS_URL, timeout=0.8) as r:
|
||||
s = json.load(r)
|
||||
except Exception:
|
||||
self.title = "◎"
|
||||
self.item_state.title = "Warming up…"
|
||||
return
|
||||
live = s.get("connected") and s.get("fresh")
|
||||
self.title = "◉" if live else "◎"
|
||||
self.item_state.title = "Connected" if live else "Waiting for device"
|
||||
self.item_device.title = f"Device: {s.get('peer') or '—'}"
|
||||
self.item_frames.title = f"Frames: {s.get('infer_count', 0):,}"
|
||||
p50, p99 = s.get("exec_p50_ms", 0.0), s.get("exec_p99_ms", 0.0)
|
||||
self.item_exec.title = f"Inference: {p50:.0f} / {p99:.0f} ms" if p50 else "Inference: —"
|
||||
self.item_awake.title = f"Keep awake: {'on' if s.get('awake') else 'off'}"
|
||||
|
||||
def open_dashboard(self, _) -> None:
|
||||
subprocess.Popen(["open", DASHBOARD_URL])
|
||||
|
||||
def quit_app(self, _) -> None:
|
||||
if self.proc is not None:
|
||||
self.proc.terminate()
|
||||
try:
|
||||
self.proc.wait(timeout=3)
|
||||
except subprocess.TimeoutExpired:
|
||||
self.proc.kill()
|
||||
rumps.quit_application()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
IQVisionApp().run()
|
||||
290
tools/iqmacvisiond/server.py
Executable file
290
tools/iqmacvisiond/server.py
Executable file
@@ -0,0 +1,290 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import logging
|
||||
import os
|
||||
import signal
|
||||
import socket
|
||||
import subprocess
|
||||
import sys
|
||||
import threading
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
os.environ.setdefault("DEV", "METAL")
|
||||
os.environ.setdefault("JIT_BATCH_SIZE", "0")
|
||||
|
||||
REPO_ROOT = Path(__file__).resolve().parents[2]
|
||||
sys.path.insert(0, str(REPO_ROOT))
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
from openpilot.iqpilot.iqvd_private_src.offload.protocol import (
|
||||
DISCOVERY_MAGIC, DISCOVERY_REPLY, DISCOVERY_PORT_DEFAULT, MSG_HELLO, MSG_HELLO_ACK, MSG_INFER,
|
||||
MSG_PING, MSG_PONG, MSG_RESULT, ProtocolError, recv_msg, send_msg,
|
||||
)
|
||||
from openpilot.iqpilot.iqvd_private_src.offload.perception import Detector
|
||||
|
||||
log = logging.getLogger("iqmacvisiond")
|
||||
|
||||
DEFAULT_PORT = 51998
|
||||
STATUS_PORT = 51995
|
||||
MODEL_NAME = "yolov8n"
|
||||
SESSION_IDLE_TIMEOUT_S = 8.0
|
||||
|
||||
STATUS: dict = {
|
||||
"connected": False, "peer": "", "model": MODEL_NAME,
|
||||
"exec_p50_ms": 0.0, "exec_p99_ms": 0.0, "infer_count": 0,
|
||||
"last_seen": 0.0, "awake": False,
|
||||
}
|
||||
|
||||
|
||||
class KeepAwake:
|
||||
def __init__(self):
|
||||
self._proc: subprocess.Popen | None = None
|
||||
|
||||
def start(self) -> None:
|
||||
if sys.platform != "darwin" or self._proc is not None:
|
||||
return
|
||||
try:
|
||||
self._proc = subprocess.Popen(["caffeinate", "-dimsu"])
|
||||
STATUS["awake"] = True
|
||||
log.info("keep-awake active (caffeinate pid=%d)", self._proc.pid)
|
||||
except OSError:
|
||||
log.warning("caffeinate unavailable; display may sleep")
|
||||
|
||||
def stop(self) -> None:
|
||||
if self._proc is not None:
|
||||
self._proc.terminate()
|
||||
self._proc = None
|
||||
STATUS["awake"] = False
|
||||
|
||||
|
||||
class DiscoveryResponder:
|
||||
def __init__(self, tcp_port: int, disc_port: int = DISCOVERY_PORT_DEFAULT):
|
||||
self.tcp_port = tcp_port
|
||||
self.disc_port = disc_port
|
||||
|
||||
def start(self) -> None:
|
||||
threading.Thread(target=self._serve, daemon=True).start()
|
||||
|
||||
def _serve(self) -> None:
|
||||
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
|
||||
try:
|
||||
sock.bind(("0.0.0.0", self.disc_port))
|
||||
except OSError:
|
||||
log.exception("discovery bind failed on udp/%d", self.disc_port)
|
||||
return
|
||||
reply = DISCOVERY_REPLY + f":{self.tcp_port}".encode()
|
||||
log.info("discovery responder on udp/%d -> tcp/%d", self.disc_port, self.tcp_port)
|
||||
while True:
|
||||
try:
|
||||
data, addr = sock.recvfrom(256)
|
||||
except OSError:
|
||||
continue
|
||||
if data.startswith(DISCOVERY_MAGIC):
|
||||
try:
|
||||
sock.sendto(reply, addr)
|
||||
except OSError:
|
||||
pass
|
||||
|
||||
|
||||
class StatusServer:
|
||||
def __init__(self, port: int = STATUS_PORT):
|
||||
self.port = port
|
||||
|
||||
def start(self) -> None:
|
||||
threading.Thread(target=self._serve, daemon=True).start()
|
||||
|
||||
def _serve(self) -> None:
|
||||
from http.server import BaseHTTPRequestHandler, HTTPServer
|
||||
|
||||
class H(BaseHTTPRequestHandler):
|
||||
def log_message(self, *a):
|
||||
pass
|
||||
|
||||
def _send(self, code, ctype, body):
|
||||
self.send_response(code)
|
||||
self.send_header("Content-Type", ctype)
|
||||
self.send_header("Content-Length", str(len(body)))
|
||||
self.end_headers()
|
||||
self.wfile.write(body)
|
||||
|
||||
def do_GET(self):
|
||||
if self.path.startswith("/status.json"):
|
||||
st = dict(STATUS)
|
||||
st["fresh"] = (time.time() - st["last_seen"]) < 6 if st["last_seen"] else False
|
||||
self._send(200, "application/json", json.dumps(st).encode())
|
||||
else:
|
||||
self._send(200, "text/html; charset=utf-8", _STATUS_HTML.encode())
|
||||
|
||||
try:
|
||||
HTTPServer(("127.0.0.1", self.port), H).serve_forever()
|
||||
except OSError:
|
||||
log.exception("status server failed on %d", self.port)
|
||||
|
||||
|
||||
_STATUS_HTML = """<!doctype html><html><head><meta charset=utf-8>
|
||||
<title>IQ Vision</title><meta name=viewport content="width=device-width,initial-scale=1">
|
||||
<style>
|
||||
:root{color-scheme:dark}
|
||||
body{margin:0;font:15px -apple-system,system-ui,sans-serif;background:#0b0d10;color:#e6e9ef}
|
||||
.wrap{max-width:520px;margin:0 auto;padding:32px 20px}
|
||||
h1{font-size:20px;margin:0 0 20px;display:flex;align-items:center;gap:10px}
|
||||
.dot{width:12px;height:12px;border-radius:50%;background:#555}
|
||||
.dot.green{background:#28d2c8;box-shadow:0 0 10px #28d2c8}
|
||||
.dot.red{background:#e74c3c}
|
||||
.row{display:flex;justify-content:space-between;padding:12px 0;border-bottom:1px solid #1c2027}
|
||||
.k{color:#8a91a0}.v{font-variant-numeric:tabular-nums}
|
||||
</style></head><body><div class=wrap>
|
||||
<h1><span class=dot id=dot></span><span id=title>IQ Vision</span></h1>
|
||||
<div class=row><span class=k>Device</span><span class="v" id=peer>—</span></div>
|
||||
<div class=row><span class=k>Model</span><span class="v" id=model>—</span></div>
|
||||
<div class=row><span class=k>Inference (p50 / p99)</span><span class="v" id=exec>—</span></div>
|
||||
<div class=row><span class=k>Frames served</span><span class="v" id=count>—</span></div>
|
||||
<div class=row><span class=k>Keep awake</span><span class="v" id=awake>—</span></div>
|
||||
</div><script>
|
||||
async function tick(){
|
||||
try{
|
||||
const s=await (await fetch('/status.json')).json();
|
||||
const live=s.connected&&s.fresh;
|
||||
document.getElementById('dot').className='dot '+(live?'green':'red');
|
||||
document.getElementById('title').textContent=live?'IQ Vision — connected':'IQ Vision — waiting for device';
|
||||
document.getElementById('peer').textContent=s.peer||'not connected';
|
||||
document.getElementById('model').textContent=s.model||'—';
|
||||
document.getElementById('exec').textContent=s.exec_p50_ms?`${s.exec_p50_ms.toFixed(1)} / ${s.exec_p99_ms.toFixed(1)} ms`:'—';
|
||||
document.getElementById('count').textContent=s.infer_count?s.infer_count.toLocaleString():'—';
|
||||
document.getElementById('awake').textContent=s.awake?'on':'off';
|
||||
}catch(e){document.getElementById('dot').className='dot red';}
|
||||
}
|
||||
tick();setInterval(tick,1000);
|
||||
</script></body></html>"""
|
||||
|
||||
|
||||
class Session:
|
||||
def __init__(self, conn: socket.socket, detector: Detector):
|
||||
self.conn = conn
|
||||
self.detector = detector
|
||||
try:
|
||||
self.peer = conn.getpeername()[0]
|
||||
except OSError:
|
||||
self.peer = ""
|
||||
self.infer_count = 0
|
||||
self.exec_ms: list[float] = []
|
||||
|
||||
def handshake(self) -> bool:
|
||||
msg_type, header, _ = recv_msg(self.conn)
|
||||
if msg_type != MSG_HELLO:
|
||||
raise ProtocolError(f"expected HELLO, got {msg_type}")
|
||||
send_msg(self.conn, MSG_HELLO_ACK, {"ok": True, "model": MODEL_NAME, "hostname": socket.gethostname()})
|
||||
STATUS.update(connected=True, peer=self.peer, last_seen=time.time())
|
||||
log.info("device connected: %s dongle=%s", self.peer, header.get("dongle_id", ""))
|
||||
return True
|
||||
|
||||
def serve(self) -> None:
|
||||
while True:
|
||||
msg_type, header, blob = recv_msg(self.conn)
|
||||
if msg_type == MSG_INFER:
|
||||
self._infer(header, blob)
|
||||
elif msg_type == MSG_PING:
|
||||
send_msg(self.conn, MSG_PONG, {})
|
||||
else:
|
||||
raise ProtocolError(f"unexpected message type {msg_type}")
|
||||
|
||||
def _infer(self, header: dict, jpeg: bytes) -> None:
|
||||
st = time.perf_counter()
|
||||
tracks = []
|
||||
try:
|
||||
rgb = cv2.imdecode(np.frombuffer(jpeg, np.uint8), cv2.IMREAD_COLOR)
|
||||
if rgb is not None:
|
||||
tracks = self.detector.detect(cv2.cvtColor(rgb, cv2.COLOR_BGR2RGB))
|
||||
except Exception:
|
||||
log.exception("inference failed for frame %s", header.get("frame_id"))
|
||||
dt = (time.perf_counter() - st) * 1e3
|
||||
send_msg(self.conn, MSG_RESULT, {"frame_id": header.get("frame_id", 0), "tracks": tracks,
|
||||
"exec_ms": dt})
|
||||
self.infer_count += 1
|
||||
self.exec_ms.append(dt)
|
||||
if len(self.exec_ms) > 400:
|
||||
del self.exec_ms[:200]
|
||||
if self.infer_count % 10 == 0 or self.infer_count == 1:
|
||||
recent = self.exec_ms[-200:]
|
||||
STATUS.update(connected=True, infer_count=self.infer_count, last_seen=time.time(),
|
||||
exec_p50_ms=float(np.percentile(recent, 50)),
|
||||
exec_p99_ms=float(np.percentile(recent, 99)))
|
||||
|
||||
|
||||
def _weights_ok() -> bool:
|
||||
from openpilot.iqpilot.iqvd_private_src.offload.perception import _weights_dir
|
||||
return (_weights_dir() / "yolov8n.safetensors").exists()
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--host", default="0.0.0.0")
|
||||
parser.add_argument("--port", type=int, default=DEFAULT_PORT)
|
||||
parser.add_argument("--no-keep-awake", action="store_true")
|
||||
args = parser.parse_args()
|
||||
|
||||
logging.basicConfig(level=logging.INFO, format="%(asctime)s %(name)s %(levelname)s %(message)s")
|
||||
|
||||
if not _weights_ok():
|
||||
log.error("yolov8n.safetensors not found; the app ships the model with it")
|
||||
sys.exit(1)
|
||||
|
||||
keep_awake = KeepAwake()
|
||||
if not args.no_keep_awake:
|
||||
keep_awake.start()
|
||||
|
||||
def _shutdown(*_):
|
||||
keep_awake.stop()
|
||||
sys.exit(0)
|
||||
try:
|
||||
signal.signal(signal.SIGTERM, _shutdown)
|
||||
signal.signal(signal.SIGINT, _shutdown)
|
||||
except ValueError:
|
||||
pass
|
||||
|
||||
t0 = time.perf_counter()
|
||||
detector = Detector(None)
|
||||
for _ in range(3):
|
||||
detector.detect(np.zeros((416, 640, 3), dtype=np.uint8))
|
||||
log.info("model warm in %.1fs", time.perf_counter() - t0)
|
||||
|
||||
DiscoveryResponder(args.port).start()
|
||||
StatusServer().start()
|
||||
log.info("status dashboard on http://127.0.0.1:%d", STATUS_PORT)
|
||||
|
||||
server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
|
||||
server.bind((args.host, args.port))
|
||||
server.listen(1)
|
||||
log.info("READY listening on %s:%d", args.host, args.port)
|
||||
|
||||
try:
|
||||
while True:
|
||||
conn, addr = server.accept()
|
||||
conn.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
|
||||
conn.settimeout(SESSION_IDLE_TIMEOUT_S)
|
||||
try:
|
||||
session = Session(conn, detector)
|
||||
if session.handshake():
|
||||
session.serve()
|
||||
except (ConnectionError, ProtocolError, OSError) as e:
|
||||
log.info("session ended: %s", e)
|
||||
finally:
|
||||
conn.close()
|
||||
STATUS.update(connected=False, peer="")
|
||||
finally:
|
||||
keep_awake.stop()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
117
tools/iqmacvisiond/test_offload.py
Executable file
117
tools/iqmacvisiond/test_offload.py
Executable file
@@ -0,0 +1,117 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
||||
"""
|
||||
import socket
|
||||
import sys
|
||||
import threading
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
REPO_ROOT = Path(__file__).resolve().parents[2]
|
||||
sys.path.insert(0, str(REPO_ROOT))
|
||||
|
||||
from openpilot.iqpilot.iqvd_private_src.offload import protocol
|
||||
from openpilot.iqpilot.iqvd_private_src.offload.client import VisionClient, discover_server
|
||||
from openpilot.iqpilot.iqvd_private_src.offload.geometry import pixel_to_ground, tracks_to_objects
|
||||
|
||||
|
||||
def test_protocol_roundtrip():
|
||||
a, b = socket.socketpair()
|
||||
protocol.send_msg(a, protocol.MSG_INFER, {"frame_id": 7, "w": 640}, b"\x00\x01\x02payload")
|
||||
mt, header, blob = protocol.recv_msg(b)
|
||||
assert mt == protocol.MSG_INFER
|
||||
assert header == {"frame_id": 7, "w": 640}
|
||||
assert blob == b"\x00\x01\x02payload"
|
||||
a.close()
|
||||
b.close()
|
||||
|
||||
|
||||
def test_protocol_empty_blob():
|
||||
a, b = socket.socketpair()
|
||||
protocol.send_msg(a, protocol.MSG_HELLO_ACK, {"ok": True, "model": "yolov8n"})
|
||||
mt, header, blob = protocol.recv_msg(b)
|
||||
assert mt == protocol.MSG_HELLO_ACK and header["model"] == "yolov8n" and blob == b""
|
||||
a.close()
|
||||
b.close()
|
||||
|
||||
|
||||
def test_geometry_center_projects_forward():
|
||||
intr = np.array([[900.0, 0.0, 960.0], [0.0, 900.0, 604.0], [0.0, 0.0, 1.0]])
|
||||
device_from_calib = np.eye(3)
|
||||
p = pixel_to_ground(960.0, 900.0, intr, device_from_calib, 1.22)
|
||||
assert p is not None
|
||||
assert p[0] > 0
|
||||
assert abs(p[1]) < 1.0
|
||||
|
||||
|
||||
def test_geometry_above_horizon_none():
|
||||
intr = np.array([[900.0, 0.0, 960.0], [0.0, 900.0, 604.0], [0.0, 0.0, 1.0]])
|
||||
assert pixel_to_ground(960.0, 100.0, intr, np.eye(3), 1.22) is None
|
||||
|
||||
|
||||
def test_tracks_to_objects():
|
||||
intr = np.array([[900.0, 0.0, 960.0], [0.0, 900.0, 604.0], [0.0, 0.0, 1.0]])
|
||||
tracks = [{"x1": 0.45, "y1": 0.5, "x2": 0.55, "y2": 0.75, "prob": 0.9, "label": "car"}]
|
||||
objs = tracks_to_objects(tracks, 1928, 1208, intr, [0.0, 0.0, 0.0])
|
||||
assert len(objs) == 1
|
||||
assert objs[0]["x"] > 0 and objs[0]["label"] == "car"
|
||||
|
||||
|
||||
class _StubDetector:
|
||||
def detect(self, rgb):
|
||||
return [{"x1": 0.1, "y1": 0.2, "x2": 0.3, "y2": 0.5, "prob": 0.8, "label": "car"}]
|
||||
|
||||
|
||||
def _run_server(port, ready):
|
||||
from openpilot.iqpilot.iqvd_private_src.offload import protocol as p
|
||||
import tools.iqmacvisiond.server as srv
|
||||
disc = srv.DiscoveryResponder(port)
|
||||
disc.start()
|
||||
server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
|
||||
server.bind(("0.0.0.0", port))
|
||||
server.listen(1)
|
||||
ready.set()
|
||||
conn, _ = server.accept()
|
||||
conn.settimeout(5)
|
||||
session = srv.Session(conn, _StubDetector())
|
||||
session.handshake()
|
||||
try:
|
||||
session.serve()
|
||||
except (p.ProtocolError, OSError):
|
||||
pass
|
||||
|
||||
|
||||
def test_discovery_and_loopback():
|
||||
port = 52050
|
||||
ready = threading.Event()
|
||||
threading.Thread(target=_run_server, args=(port, ready), daemon=True).start()
|
||||
assert ready.wait(5)
|
||||
time.sleep(0.2)
|
||||
|
||||
found = discover_server(timeout=2.0)
|
||||
assert found is not None, "discovery failed"
|
||||
assert found[1] == port
|
||||
|
||||
import cv2
|
||||
ok, jpeg = cv2.imencode(".jpg", np.zeros((400, 640, 3), dtype=np.uint8))
|
||||
assert ok
|
||||
|
||||
client = VisionClient("test-dongle")
|
||||
assert client.connect(), "connect failed"
|
||||
meta = {"frame_id": 42, "wide": False, "w": 640, "h": 400}
|
||||
tracks = client.infer(jpeg.tobytes(), meta)
|
||||
assert tracks is not None and len(tracks) == 1
|
||||
assert tracks[0]["label"] == "car"
|
||||
client.close()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
fns = [v for k, v in sorted(globals().items()) if k.startswith("test_") and callable(v)]
|
||||
for fn in fns:
|
||||
fn()
|
||||
print(f"ok {fn.__name__}")
|
||||
print(f"\n{len(fns)} passed")
|
||||
104
tools/iqpilot/console_pretty.py
Normal file
104
tools/iqpilot/console_pretty.py
Normal file
@@ -0,0 +1,104 @@
|
||||
#!/usr/bin/env python3
|
||||
# console path for the boot/manager tmux: must never die or block; on any error, forward raw.
|
||||
import os
|
||||
import re
|
||||
import signal
|
||||
import sys
|
||||
|
||||
try:
|
||||
signal.signal(signal.SIGPIPE, signal.SIG_IGN)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
_COLOR = sys.stdout.isatty() and os.environ.get("NO_COLOR") is None
|
||||
|
||||
_DROP = re.compile(
|
||||
r"(\x1b\[\?\d+[a-z])"
|
||||
r"|ln: failed to create symbolic link '[^']*(cursor-server|windsurf-server|vscode-server)"
|
||||
r"|^Last login:"
|
||||
r"|gbm_create_device\(\d+\): Info:"
|
||||
r"|No IRQs found for '"
|
||||
r"|^pid \d+'s (current|new) affinity list:"
|
||||
r"|kj/filesystem-disk-unix\.c\+\+:\d+: warning: PWD"
|
||||
)
|
||||
_CLOUDLOG = re.compile(r"^([\w./+-]+\.(?:cc|cpp|c|h|py)): (.*)$")
|
||||
_ALREADY = re.compile(r"^\s*(?:\x1b\[[0-9;]*m)?\s*(CRIT|ERR|WARN|info|dbg)\b")
|
||||
_ERRISH = re.compile(r"not supported|fail|error|invalid|cannot|timed out|timeout|unable", re.I)
|
||||
|
||||
|
||||
def _sev(msg):
|
||||
return ("\033[1;38;5;203m", " ERR", "\033[1;38;5;210m") if _ERRISH.search(msg) \
|
||||
else ("\033[38;5;110m", "info", "")
|
||||
|
||||
|
||||
def _restyle(line):
|
||||
if _DROP.search(line):
|
||||
return None
|
||||
if _ALREADY.match(line):
|
||||
return line
|
||||
m = _CLOUDLOG.match(re.sub(r"\x1b\[[0-9;]*m", "", line))
|
||||
if m and _COLOR:
|
||||
src, msg = m.group(1), m.group(2)
|
||||
lc, ln, mc = _sev(msg)
|
||||
body = f"{mc}{msg}\033[0m" if mc else msg
|
||||
return f"{lc}{ln}\033[0m \033[2m{src}\033[0m {body}"
|
||||
return line
|
||||
|
||||
|
||||
def _emit(out, buf):
|
||||
styled = _restyle(buf)
|
||||
if styled is not None:
|
||||
out.write(styled + "\r\n"); out.flush()
|
||||
|
||||
|
||||
def main():
|
||||
out = sys.stdout
|
||||
buf = ""
|
||||
pending_cr = False
|
||||
read = sys.stdin.buffer.read
|
||||
while True:
|
||||
try:
|
||||
ch = read(1)
|
||||
except Exception:
|
||||
break
|
||||
if not ch:
|
||||
break
|
||||
try:
|
||||
c = ch.decode("utf-8", "replace")
|
||||
if pending_cr:
|
||||
pending_cr = False
|
||||
if c == "\n":
|
||||
_emit(out, buf); buf = ""; continue
|
||||
out.write(buf + "\r"); out.flush(); buf = "" # bare \r: progress
|
||||
if c == "\r":
|
||||
pending_cr = True
|
||||
elif c == "\n":
|
||||
_emit(out, buf); buf = ""
|
||||
else:
|
||||
buf += c
|
||||
except Exception:
|
||||
try:
|
||||
out.write(buf); out.flush()
|
||||
except Exception:
|
||||
pass
|
||||
buf = ""; pending_cr = False
|
||||
if pending_cr:
|
||||
out.write(buf + "\r"); out.flush()
|
||||
elif buf:
|
||||
try:
|
||||
styled = _restyle(buf)
|
||||
if styled is not None:
|
||||
out.write(styled); out.flush()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
main()
|
||||
except Exception:
|
||||
try:
|
||||
import shutil
|
||||
shutil.copyfileobj(sys.stdin.buffer, sys.stdout.buffer)
|
||||
except Exception:
|
||||
pass
|
||||
32
tools/iqpilot/git-pretty.sh
Normal file
32
tools/iqpilot/git-pretty.sh
Normal file
@@ -0,0 +1,32 @@
|
||||
# IQ.Pilot pretty git wrappers (gpull/gfetch/gsync/greset/gbv). bash + zsh.
|
||||
# Source from your shell rc: source .../tools/iqpilot/git-pretty.sh
|
||||
if [ -n "${BASH_SOURCE:-}" ]; then
|
||||
_iq_gp_src="${BASH_SOURCE[0]}"
|
||||
elif [ -n "${ZSH_VERSION:-}" ]; then
|
||||
_iq_gp_src="${(%):-%x}"
|
||||
else
|
||||
_iq_gp_src="$0"
|
||||
fi
|
||||
_IQ_GIT_PRETTY="$(cd "$(dirname "$_iq_gp_src")" 2>/dev/null && pwd)/git_pretty.py"
|
||||
unset _iq_gp_src
|
||||
|
||||
if [ -z "${_IQ_GP_PY:-}" ]; then
|
||||
if command -v python3 >/dev/null 2>&1; then _IQ_GP_PY=python3
|
||||
elif [ -x /usr/bin/python3 ]; then _IQ_GP_PY=/usr/bin/python3
|
||||
else _IQ_GP_PY=python; fi
|
||||
fi
|
||||
|
||||
_iq_git_pretty() {
|
||||
command git "$@" 2>&1 | "$_IQ_GP_PY" "$_IQ_GIT_PRETTY"
|
||||
if [ -n "${ZSH_VERSION:-}" ]; then
|
||||
return ${pipestatus[1]}
|
||||
else
|
||||
return ${PIPESTATUS[0]}
|
||||
fi
|
||||
}
|
||||
|
||||
gpull() { _iq_git_pretty -c color.ui=always pull --progress "$@"; }
|
||||
gfetch() { _iq_git_pretty -c color.ui=always fetch --progress "$@"; }
|
||||
gsync() { _iq_git_pretty -c color.ui=always submodule update --init --recursive --progress "$@"; }
|
||||
greset() { _iq_git_pretty -c color.ui=always reset "$@"; }
|
||||
gbv() { _iq_git_pretty -c color.branch=always branch -v "$@"; }
|
||||
123
tools/iqpilot/git_pretty.py
Normal file
123
tools/iqpilot/git_pretty.py
Normal file
@@ -0,0 +1,123 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import re
|
||||
import sys
|
||||
|
||||
_GRAD = {
|
||||
"PULL": ((95, 240, 150), (40, 200, 120)),
|
||||
"FETCH": ((95, 205, 255), (130, 110, 250)),
|
||||
"SUBMOD": ((200, 160, 255), (150, 110, 250)),
|
||||
"RESET": ((255, 190, 90), (230, 80, 70)),
|
||||
"BRANCH": ((95, 215, 255), (70, 130, 245)),
|
||||
}
|
||||
_TARGET_RGB = (150, 152, 178)
|
||||
_GREEN = (120, 210, 130)
|
||||
_DIM = "\033[2;38;5;246m"
|
||||
_RST = "\033[0m"
|
||||
|
||||
|
||||
def _mode():
|
||||
if not sys.stdout.isatty() or os.environ.get("NO_COLOR"):
|
||||
return None
|
||||
return "true" if os.environ.get("COLORTERM", "").lower() in ("truecolor", "24bit") else "256"
|
||||
|
||||
|
||||
def _fg(rgb, mode):
|
||||
r, g, b = rgb
|
||||
if mode == "true":
|
||||
return f"\033[38;2;{r};{g};{b}m"
|
||||
if abs(r - g) < 12 and abs(g - b) < 12 and abs(r - b) < 12:
|
||||
idx = 232 + min(23, round((r + g + b) / 3 / 255 * 23))
|
||||
else:
|
||||
idx = 16 + 36 * round(r / 255 * 5) + 6 * round(g / 255 * 5) + round(b / 255 * 5)
|
||||
return f"\033[38;5;{idx}m"
|
||||
|
||||
|
||||
def _grad(word, label, mode):
|
||||
start, end = _GRAD.get(label, _GRAD["FETCH"])
|
||||
n = max(1, len(word) - 1)
|
||||
out = [f"\033[1m{_fg(tuple(int(s + (e - s) * i / n) for s, e in zip(start, end)), mode)}{ch}"
|
||||
for i, ch in enumerate(word)]
|
||||
return "".join(out) + _RST
|
||||
|
||||
|
||||
def _label(label, body, mode):
|
||||
pad = " " * max(0, 8 - len(label))
|
||||
return f"{pad}{_grad(label, label, mode)} {_fg(_TARGET_RGB, mode)}{body}{_RST}"
|
||||
|
||||
|
||||
def _restyle(line, mode):
|
||||
s = line.rstrip("\n")
|
||||
raw = re.sub(r"\033\[[0-9;]*m", "", s) # match against de-colored text
|
||||
|
||||
if raw == "Already up to date.":
|
||||
return f"{_fg(_GREEN, mode)}✓ already up to date{_RST}"
|
||||
m = re.match(r"Updating ([0-9a-f]+\.\.[0-9a-f]+)$", raw)
|
||||
if m:
|
||||
return _label("PULL", m.group(1), mode)
|
||||
if raw == "Fast-forward":
|
||||
return f"{_DIM}fast-forward{_RST}"
|
||||
m = re.match(r"HEAD is now at ([0-9a-f]+) (.*)$", raw)
|
||||
if m:
|
||||
return _label("RESET", f"{m.group(1)} {m.group(2)}", mode)
|
||||
m = re.match(r"Submodule path '(.+)': checked out '([0-9a-f]+)'$", raw)
|
||||
if m:
|
||||
return _label("SUBMOD", f"{m.group(1)} @ {m.group(2)[:9]}", mode)
|
||||
m = re.match(r"Submodule '(.+)' \((.+)\) registered for path '(.+)'$", raw)
|
||||
if m:
|
||||
return _label("SUBMOD", f"{m.group(3)} (registered)", mode)
|
||||
m = re.match(r"From (.+)$", raw)
|
||||
if m:
|
||||
return _label("FETCH", m.group(1), mode)
|
||||
m = re.match(r"\s*\*?\s*\[new (?:branch|tag)\]\s+(\S+)\s+->\s+(\S+)$", raw)
|
||||
if m:
|
||||
return _label("FETCH", f"new {m.group(1)} → {m.group(2)}", mode)
|
||||
m = re.match(r"\s*\*\s+(?:branch|tag)\s+(\S+)\s+->\s+(\S+)$", raw)
|
||||
if m:
|
||||
return _label("FETCH", f"{m.group(1)} → {m.group(2)}", mode)
|
||||
m = re.match(r"\s*([0-9a-f]+\.\.[0-9a-f]+)\s+(\S+)\s+->\s+(\S+)$", raw)
|
||||
if m:
|
||||
return _label("FETCH", f"{m.group(1)} {m.group(2)} → {m.group(3)}", mode)
|
||||
m = re.match(r"([* ]) +(\S+) +([0-9a-f]{7,})( .*)?$", raw)
|
||||
if m:
|
||||
cur, name, sha, msg = m.groups()
|
||||
star = f"{_fg(_GREEN, mode)}●{_RST} " if cur == "*" else " "
|
||||
return f"{star}{_grad(name, 'BRANCH', mode)} {_fg(_TARGET_RGB, mode)}{sha}{(msg or '')}{_RST}"
|
||||
return s
|
||||
|
||||
|
||||
def main():
|
||||
mode = _mode()
|
||||
out = sys.stdout
|
||||
if mode is None: # not a tty / NO_COLOR: passthrough
|
||||
for chunk in iter(lambda: sys.stdin.buffer.read(4096), b""):
|
||||
out.buffer.write(chunk)
|
||||
out.buffer.flush()
|
||||
return
|
||||
|
||||
buf = ""
|
||||
stream = sys.stdin
|
||||
while True:
|
||||
ch = stream.read(1)
|
||||
if not ch:
|
||||
break
|
||||
if ch == "\r": # progress fragment: emit live, untouched
|
||||
out.write(buf + "\r")
|
||||
out.flush()
|
||||
buf = ""
|
||||
elif ch == "\n":
|
||||
out.write(_restyle(buf, mode) + "\n")
|
||||
out.flush()
|
||||
buf = ""
|
||||
else:
|
||||
buf += ch
|
||||
if buf:
|
||||
out.write(_restyle(buf, mode))
|
||||
out.flush()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
main()
|
||||
except (BrokenPipeError, KeyboardInterrupt):
|
||||
pass
|
||||
753
tools/iqpilot/mici_preview.py
Executable file
753
tools/iqpilot/mici_preview.py
Executable file
@@ -0,0 +1,753 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
╔══════════════════════════════════════════════════════════════════════════╗
|
||||
║ IQ.Pilot MICI UI Preview Tool ║
|
||||
║ ────────────────────────────────────────────────────────────────────── ║
|
||||
║ Renders any MICI layout/widget at 536×240 on your Mac desktop so you ║
|
||||
║ can visually inspect and iterate without a physical comma 4. ║
|
||||
║ ║
|
||||
║ MODES ║
|
||||
║ screenshot — render N frames, save PNG(s), open in Preview ║
|
||||
║ video — render N seconds to MP4 via ffmpeg, open in QuickTime ║
|
||||
║ live — interactive window, hot-reload on file-save ║
|
||||
║ ║
|
||||
║ USAGE ║
|
||||
║ python tools/iqpilot/mici_preview.py [OPTIONS] ║
|
||||
║ ║
|
||||
║ OPTIONS ║
|
||||
║ --panel PANEL Panel to render: steering, visuals, display, ║
|
||||
║ software, cruise, trips, osm, models, toggles, ║
|
||||
║ device, developer, home, settings (default: steering)║
|
||||
║ --mode MODE screenshot | video | live (default: screenshot) ║
|
||||
║ --frames N Frames to settle before screenshot (default: 90) ║
|
||||
║ --shots N Number of screenshots to take (default: 1) ║
|
||||
║ --duration S Video duration in seconds (default: 4) ║
|
||||
║ --fps N Render FPS (default: 60) ║
|
||||
║ --scale F Window scale multiplier (2.0 = 1072×480 window) ║
|
||||
║ (default: 2.5) ║
|
||||
║ --out PATH Output file/dir (default: /tmp/mici_preview/) ║
|
||||
║ --open Open output file(s) after capture (default: True) ║
|
||||
║ --mock Use mock UI state (no real params needed) ║
|
||||
║ ║
|
||||
║ EXAMPLES ║
|
||||
║ # Screenshot the steering panel (scaled 2.5×, opens in Preview) ║
|
||||
║ python tools/iqpilot/mici_preview.py --panel steering ║
|
||||
║ ║
|
||||
║ # 4-second video of the neon glow animation ║
|
||||
║ python tools/iqpilot/mici_preview.py --panel steering --mode video ║
|
||||
║ ║
|
||||
║ # Live interactive window with hot-reload ║
|
||||
║ python tools/iqpilot/mici_preview.py --panel visuals --mode live ║
|
||||
║ ║
|
||||
║ # Multiple panels in one go (screenshots) ║
|
||||
║ python tools/iqpilot/mici_preview.py --panel all ║
|
||||
╚══════════════════════════════════════════════════════════════════════════╝
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import importlib
|
||||
import os
|
||||
import subprocess
|
||||
import sys
|
||||
import time
|
||||
import queue
|
||||
import threading
|
||||
from pathlib import Path
|
||||
|
||||
# ── Project root on sys.path ──────────────────────────────────────────────
|
||||
ROOT = Path(__file__).resolve().parents[2]
|
||||
sys.path.insert(0, str(ROOT))
|
||||
|
||||
# Force MICI mode (no BIG UI)
|
||||
os.environ.setdefault("BIG", "0")
|
||||
os.environ.setdefault("IQPILOT_UI", "1")
|
||||
# Use offscreen mode for screenshot/video (no FPS cap → fast capture)
|
||||
# Live mode overrides this below.
|
||||
|
||||
import pyray as rl # noqa: E402 — must come after env setup
|
||||
|
||||
# ── MICI canvas dimensions ─────────────────────────────────────────────────
|
||||
MICI_W = 536
|
||||
MICI_H = 240
|
||||
|
||||
# ── Output directory ───────────────────────────────────────────────────────
|
||||
DEFAULT_OUT = Path("/tmp/mici_preview")
|
||||
|
||||
# ── Color palette for the preview chrome ──────────────────────────────────
|
||||
CHROME_BG = rl.Color(18, 18, 22, 255) # dark bg behind the MICI canvas
|
||||
LABEL_COLOR = rl.Color(160, 160, 160, 200) # panel label text
|
||||
|
||||
# ── All available panels ───────────────────────────────────────────────────
|
||||
ALL_PANELS = [
|
||||
"steering", "visuals", "display", "software", "cruise",
|
||||
"trips", "osm", "models", "toggles", "device", "developer", "home",
|
||||
]
|
||||
|
||||
|
||||
# ─────────────────────────────────────────────────────────────────────────────
|
||||
# Mock UI state so we can run without a live comma process
|
||||
# ─────────────────────────────────────────────────────────────────────────────
|
||||
|
||||
def _patch_mock_state():
|
||||
"""Replace real ui_state and Params with lightweight mocks."""
|
||||
import types
|
||||
|
||||
# ── Shared in-memory store ────────────────────────────────────────────────
|
||||
_store: dict = {}
|
||||
|
||||
class MockParams:
|
||||
"""
|
||||
Drop-in Params mock that also supports `Params | None` type expressions
|
||||
at module import time by implementing __or__ / __ror__ on the class itself.
|
||||
"""
|
||||
# Allow `Params | None` as a runtime type-union expression
|
||||
def __class_getitem__(cls, item):
|
||||
return cls
|
||||
def __or__(cls, other):
|
||||
import types as _t
|
||||
return _t.UnionType if hasattr(_t, 'UnionType') else object
|
||||
__ror__ = __or__
|
||||
|
||||
def __init__(self, _=None, **__):
|
||||
pass # ignore any constructor args (real Params accepts path kwarg)
|
||||
|
||||
def get(self, key, default=None, return_default=False):
|
||||
val = _store.get(key, default)
|
||||
# Real Params.get() returns str or None; convert bools → str
|
||||
if isinstance(val, bool):
|
||||
return str(int(val))
|
||||
return val
|
||||
|
||||
def get_bool(self, key, default=False):
|
||||
val = _store.get(key, default)
|
||||
if isinstance(val, str):
|
||||
return val not in ('', '0', 'false', 'False', 'None')
|
||||
return bool(val)
|
||||
|
||||
def put(self, key, val):
|
||||
_store[key] = val
|
||||
|
||||
def put_bool(self, key, val):
|
||||
_store[key] = bool(val)
|
||||
|
||||
def put_nonblocking(self, key, val):
|
||||
_store[key] = val
|
||||
|
||||
# Seed some sensible defaults so widgets render realistically
|
||||
mp = MockParams()
|
||||
# ── Steering ────────────────────────────────────────────────────────────
|
||||
mp.put("AolEnabled", False)
|
||||
mp.put("AolSteeringMode", 0) # 0=remain active, 1=pause, 2=disengage
|
||||
mp.put("AolMainCruiseAllowed", True)
|
||||
mp.put("AolUnifiedEngagementMode", False)
|
||||
mp.put("NeuralNetworkFeedForward", False)
|
||||
mp.put("IQLaneChangeTimer", 0) # nudge
|
||||
mp.put("IQLaneChangeBsmDelay", False)
|
||||
|
||||
# ── Visuals (correct param keys matching visuals.py) ─────────────────────
|
||||
mp.put("IQBlindSpotAlerts", True)
|
||||
mp.put("IQSteerEffortArc", True)
|
||||
mp.put("IQRoadNameOverlay", True)
|
||||
mp.put("IQBlinkerIndicators", True)
|
||||
mp.put("IQAccelMeter", False)
|
||||
mp.put("IQLeadReadouts", 0) # 0=off
|
||||
mp.put("IQDevUIInfo", 0) # 0=off
|
||||
mp.put("AlphaLongitudinalEnabled", False) # real param; gates ChevronInfo
|
||||
|
||||
# ── Display ───────────────────────────────────────────────────────────────
|
||||
mp.put("OnroadScreenOffBrightness", 0) # 0=auto
|
||||
mp.put("OnroadScreenOffTimer", 60) # 1m
|
||||
mp.put("InteractivityTimeout", 0) # default
|
||||
|
||||
# ── Software ──────────────────────────────────────────────────────────────
|
||||
mp.put("DisableUpdates", False)
|
||||
mp.put("GitBranch", "master-mici")
|
||||
mp.put("Version", "IQ.Pilot 0.9.5-mici")
|
||||
|
||||
# ── Models ────────────────────────────────────────────────────────────────
|
||||
mp.put("IQLiveSteerDelay", False)
|
||||
mp.put("IQLaneTurnDesire", False)
|
||||
mp.put("IQLaneTurnValue", "19.0")
|
||||
|
||||
# ── Cruise ────────────────────────────────────────────────────────────────
|
||||
mp.put("ExperimentalMode", False)
|
||||
mp.put("IQDynamicMode", False)
|
||||
mp.put("LongitudinalPersonality", 1) # 0=aggressive,1=standard,2=relaxed,3=stock
|
||||
mp.put("IQSpeedAssistMode", 0) # 0=off
|
||||
|
||||
# ── Misc / system ─────────────────────────────────────────────────────────
|
||||
mp.put("IsMetric", False)
|
||||
mp.put("UIAccentColor", "#00FFF5") # default neon cyan
|
||||
|
||||
# Inject mock into common.params
|
||||
# IMPORTANT: inject the CLASS (not an instance) so that `Params | None`
|
||||
# type-union expressions in downstream modules work at import time.
|
||||
mock_module = types.ModuleType("openpilot.common.params")
|
||||
mock_module.Params = MockParams
|
||||
sys.modules["openpilot.common.params"] = mock_module
|
||||
|
||||
# ── Mock ui_state (both the iqpilot layer and the top-level selfdrive layer) ─
|
||||
class MockCP:
|
||||
enableBsm = True
|
||||
openpilotLongitudinalControl = True
|
||||
alphaLongitudinalAvailable = False
|
||||
|
||||
# Minimal mock for ui_state.sm — returns empty/default objects for any key
|
||||
class _MockBundle:
|
||||
internalName = "mock-model"
|
||||
displayName = "Mock Model"
|
||||
index = 0
|
||||
status = None # not downloading
|
||||
models = []
|
||||
overrides = []
|
||||
|
||||
class _MockModelManager:
|
||||
availableBundles = []
|
||||
activeBundle = _MockBundle()
|
||||
selectedBundle = None # None = not downloading
|
||||
|
||||
class _MockSM:
|
||||
"""Minimal SubMaster-like dict that returns sensible defaults."""
|
||||
_data = {
|
||||
"iqModelManager": _MockModelManager(),
|
||||
}
|
||||
def __getitem__(self, key):
|
||||
return self._data.get(key, type("Empty", (), {"enabled": False})())
|
||||
@property
|
||||
def updated(self):
|
||||
return type("U", (), {"__getitem__": lambda s, k: False})()
|
||||
@property
|
||||
def alive(self):
|
||||
return type("A", (), {"__getitem__": lambda s, k: True})()
|
||||
@property
|
||||
def valid(self):
|
||||
return type("V", (), {"__getitem__": lambda s, k: True})()
|
||||
@property
|
||||
def frame(self): return 0
|
||||
def __contains__(self, key): return True
|
||||
|
||||
class MockUIState:
|
||||
CP = MockCP()
|
||||
params = mp
|
||||
sm = _MockSM()
|
||||
started = False
|
||||
ignition = False
|
||||
is_metric = False
|
||||
has_longitudinal_control = True
|
||||
always_on_dm = False
|
||||
recording_audio = False
|
||||
personality = 1 # standard
|
||||
custom_interactive_timeout = 0
|
||||
light_sensor = -1.0
|
||||
is_release = False
|
||||
# IQ-specific extras
|
||||
aol_enabled = False
|
||||
aol_state = 0
|
||||
|
||||
def is_offroad(self): return True
|
||||
def is_onroad(self): return False
|
||||
def add_offroad_transition_callback(self, cb): pass
|
||||
def add_engaged_transition_callback(self, cb): pass
|
||||
def update_params(self): pass
|
||||
|
||||
@property
|
||||
def engaged(self): return False
|
||||
|
||||
_mock_ui_state = MockUIState()
|
||||
|
||||
# openpilot.selfdrive.ui.ui_state (base openpilot layer — hosts the IQ UI state classes/enums)
|
||||
from enum import Enum, IntEnum
|
||||
class _UIStatus(Enum):
|
||||
DISENGAGED = "disengaged"
|
||||
ENGAGED = "engaged"
|
||||
OVERRIDE = "override"
|
||||
LAT_ONLY = "lat_only"
|
||||
LONG_ONLY = "long_only"
|
||||
|
||||
class _OnroadTimerStatus(Enum):
|
||||
NONE = 0
|
||||
PAUSE = 1
|
||||
RESUME = 2
|
||||
|
||||
class _OnroadBrightness(IntEnum):
|
||||
AUTO = 0
|
||||
AUTO_DARK = 1
|
||||
|
||||
mock_base_ui_mod = types.ModuleType("openpilot.selfdrive.ui.ui_state")
|
||||
mock_base_ui_mod.ui_state = _mock_ui_state
|
||||
mock_base_ui_mod.UIStatus = _UIStatus
|
||||
mock_base_ui_mod.OnroadTimerStatus = _OnroadTimerStatus
|
||||
mock_base_ui_mod.OnroadBrightness = _OnroadBrightness
|
||||
mock_base_ui_mod.device = type("MockDevice", (), {"awake": True})()
|
||||
sys.modules["openpilot.selfdrive.ui.ui_state"] = mock_base_ui_mod
|
||||
|
||||
return mp
|
||||
|
||||
|
||||
# ─────────────────────────────────────────────────────────────────────────────
|
||||
# Panel loader
|
||||
# ─────────────────────────────────────────────────────────────────────────────
|
||||
|
||||
def load_panel(name: str):
|
||||
"""
|
||||
Instantiate a MICI panel/layout by name.
|
||||
Returns a Widget instance with set_rect() already called.
|
||||
"""
|
||||
rect = rl.Rectangle(0, 0, MICI_W, MICI_H)
|
||||
|
||||
layouts = {
|
||||
"steering": ("openpilot.iqpilot.ui.mici.layouts.steering", "SteeringLayoutMici"),
|
||||
"visuals": ("openpilot.iqpilot.ui.mici.layouts.visuals", "VisualsLayoutMici"),
|
||||
"display": ("openpilot.iqpilot.ui.mici.layouts.display", "DisplayLayoutMici"),
|
||||
"software": ("openpilot.iqpilot.ui.mici.layouts.software", "SoftwareLayoutMici"),
|
||||
"cruise": ("openpilot.iqpilot.ui.mici.layouts.cruise", "CruiseLayoutMici"),
|
||||
"trips": ("openpilot.iqpilot.ui.mici.layouts.trips", "TripsLayoutMici"),
|
||||
"osm": ("openpilot.iqpilot.ui.mici.layouts.osm", "OSMLayoutMici"),
|
||||
"models": ("openpilot.iqpilot.ui.mici.layouts.models", "ModelsLayoutMici"),
|
||||
"toggles": ("openpilot.selfdrive.ui.mici.layouts.settings.toggles", "TogglesLayoutMici"),
|
||||
"device": ("openpilot.selfdrive.ui.mici.layouts.settings.device", "DeviceLayoutMici"),
|
||||
"developer":("openpilot.selfdrive.ui.mici.layouts.settings.developer","DeveloperLayoutMici"),
|
||||
"home": ("openpilot.selfdrive.ui.mici.layouts.home", "MiciHomeLayout"),
|
||||
"settings": ("openpilot.iqpilot.ui.mici.layouts.settings", "IQMiciSettingsLayout"),
|
||||
}
|
||||
|
||||
if name not in layouts:
|
||||
raise ValueError(f"Unknown panel '{name}'. Choose from: {', '.join(layouts)}")
|
||||
|
||||
# All IQ.Pilot MICI layout panels accept an optional back_callback
|
||||
NEEDS_BACK_CB = {
|
||||
"steering", "visuals", "display", "software", "cruise",
|
||||
"trips", "osm", "models", "toggles", "device", "developer",
|
||||
}
|
||||
|
||||
mod_path, cls_name = layouts[name]
|
||||
mod = importlib.import_module(mod_path)
|
||||
cls = getattr(mod, cls_name)
|
||||
if name in NEEDS_BACK_CB:
|
||||
widget = cls(back_callback=lambda: None)
|
||||
else:
|
||||
widget = cls()
|
||||
widget.set_rect(rect)
|
||||
widget.show_event()
|
||||
return widget
|
||||
|
||||
|
||||
# ─────────────────────────────────────────────────────────────────────────────
|
||||
# Rendering helpers
|
||||
# ─────────────────────────────────────────────────────────────────────────────
|
||||
|
||||
def _draw_chrome(panel_name: str, scale: float, canvas_x: int, canvas_y: int):
|
||||
"""Draw the preview window chrome: background, label, dimension hint."""
|
||||
# Nothing to draw outside the canvas — the window IS the canvas (+ padding)
|
||||
pass
|
||||
|
||||
|
||||
def _render_frame(widget, render_tex: rl.RenderTexture, canvas_x: int, canvas_y: int, scale: float, panel_name: str):
|
||||
"""
|
||||
Render one frame:
|
||||
1. Draw the MICI widget into render_tex (536×240 offscreen)
|
||||
2. Blit the texture into the window at the correct position + scale
|
||||
3. Draw chrome overlays (panel label, grid, etc.)
|
||||
"""
|
||||
# Draw into the MICI-sized render texture
|
||||
rl.begin_texture_mode(render_tex)
|
||||
rl.clear_background(rl.Color(0, 0, 0, 255))
|
||||
widget.render(rl.Rectangle(0, 0, MICI_W, MICI_H))
|
||||
rl.end_texture_mode()
|
||||
|
||||
# Blit to screen (flip Y because OpenGL textures are upside-down)
|
||||
src = rl.Rectangle(0, 0, MICI_W, -MICI_H) # negative H = flip
|
||||
dst = rl.Rectangle(canvas_x, canvas_y, MICI_W * scale, MICI_H * scale)
|
||||
rl.draw_texture_pro(render_tex.texture, src, dst, rl.Vector2(0, 0), 0, rl.WHITE)
|
||||
|
||||
# Panel name label bottom-left
|
||||
rl.draw_text(panel_name.upper(), canvas_x + 6, canvas_y + int(MICI_H * scale) + 6,
|
||||
14, rl.Color(120, 120, 120, 180))
|
||||
|
||||
# Dimension hint bottom-right
|
||||
hint = f"{MICI_W}×{MICI_H} (×{scale:.1f})"
|
||||
hint_w = rl.measure_text(hint, 12)
|
||||
win_w = rl.get_screen_width()
|
||||
rl.draw_text(hint, win_w - hint_w - 8, canvas_y + int(MICI_H * scale) + 6,
|
||||
12, rl.Color(80, 80, 80, 160))
|
||||
|
||||
|
||||
# ─────────────────────────────────────────────────────────────────────────────
|
||||
# Screenshot mode
|
||||
# ─────────────────────────────────────────────────────────────────────────────
|
||||
|
||||
def run_screenshot(panel_name: str, args, out_dir: Path) -> list[Path]:
|
||||
"""
|
||||
Render `args.frames` frames (so animations settle), then take `args.shots`
|
||||
screenshots 0.5s apart. Returns list of saved PNG paths.
|
||||
|
||||
Uses gui_app.init_window() so fonts are loaded correctly, and the SCALE
|
||||
env var (set to args.scale in main()) controls window size.
|
||||
"""
|
||||
from openpilot.system.ui.lib.application import gui_app
|
||||
|
||||
gui_app.init_window(f"MICI Preview — {panel_name}", fps=args.fps)
|
||||
|
||||
# Offscreen MICI-resolution render texture
|
||||
render_tex = rl.load_render_texture(MICI_W, MICI_H)
|
||||
|
||||
# Load widget AFTER window/fonts are initialized
|
||||
widget = load_panel(panel_name)
|
||||
|
||||
saved: list[Path] = []
|
||||
frame = 0
|
||||
shots_taken = 0
|
||||
next_shot_frame = args.frames # first shot after settle
|
||||
|
||||
# Display at native scaled window coords (0,0 → screen size)
|
||||
win_w = rl.get_screen_width()
|
||||
win_h = rl.get_screen_height()
|
||||
|
||||
while not rl.window_should_close() and shots_taken < args.shots:
|
||||
rl.begin_drawing()
|
||||
rl.clear_background(CHROME_BG)
|
||||
|
||||
# Render widget into the MICI-sized texture, then blit full-window
|
||||
rl.begin_texture_mode(render_tex)
|
||||
rl.clear_background(rl.BLACK)
|
||||
widget.render(rl.Rectangle(0, 0, MICI_W, MICI_H))
|
||||
rl.end_texture_mode()
|
||||
|
||||
# Blit the render texture to fill the whole window (Y-flipped)
|
||||
src = rl.Rectangle(0, 0, MICI_W, -MICI_H)
|
||||
dst = rl.Rectangle(0, 0, win_w, win_h)
|
||||
rl.draw_texture_pro(render_tex.texture, src, dst, rl.Vector2(0, 0), 0, rl.WHITE)
|
||||
|
||||
# Settle progress bar
|
||||
if frame < args.frames:
|
||||
pct = frame / args.frames
|
||||
rl.draw_rectangle(0, win_h - 3, int(win_w * pct), 3, rl.Color(0, 255, 245, 140))
|
||||
rl.draw_text(f"settling {frame}/{args.frames}", 6, 6, 11, rl.Color(100, 100, 100, 160))
|
||||
|
||||
rl.end_drawing()
|
||||
frame += 1
|
||||
|
||||
if frame == next_shot_frame:
|
||||
# Read from render texture (native MICI res, no padding to crop)
|
||||
img = rl.load_image_from_texture(render_tex.texture)
|
||||
rl.image_flip_vertical(img)
|
||||
# Save at 2× native for clarity on Retina displays
|
||||
rl.image_resize(img, MICI_W * 2, MICI_H * 2)
|
||||
out_dir.mkdir(parents=True, exist_ok=True)
|
||||
ts = int(time.time() * 1000)
|
||||
path = out_dir / f"mici_{panel_name}_{shots_taken + 1:02d}_{ts}.png"
|
||||
rl.export_image(img, str(path))
|
||||
rl.unload_image(img)
|
||||
saved.append(path)
|
||||
print(f" 📸 saved: {path}")
|
||||
shots_taken += 1
|
||||
next_shot_frame += int(args.fps * 0.5) # 0.5s between shots
|
||||
|
||||
rl.unload_render_texture(render_tex)
|
||||
rl.close_window()
|
||||
return saved
|
||||
|
||||
|
||||
# ─────────────────────────────────────────────────────────────────────────────
|
||||
# Video mode
|
||||
# ─────────────────────────────────────────────────────────────────────────────
|
||||
|
||||
def run_video(panel_name: str, args, out_dir: Path) -> Path:
|
||||
"""
|
||||
Render args.duration seconds at args.fps, pipe raw RGBA frames to ffmpeg → MP4.
|
||||
Returns the output MP4 path.
|
||||
"""
|
||||
from openpilot.system.ui.lib.application import gui_app
|
||||
|
||||
out_dir.mkdir(parents=True, exist_ok=True)
|
||||
ts = int(time.time())
|
||||
mp4_path = out_dir / f"mici_{panel_name}_{ts}.mp4"
|
||||
|
||||
# Launch ffmpeg to accept raw RGBA frames at MICI native resolution
|
||||
ffmpeg = subprocess.Popen([
|
||||
"ffmpeg", "-v", "warning", "-nostats",
|
||||
"-f", "rawvideo", "-pix_fmt", "rgba",
|
||||
"-s", f"{MICI_W}x{MICI_H}",
|
||||
"-r", str(args.fps),
|
||||
"-i", "pipe:0",
|
||||
"-vf", "vflip,format=yuv420p",
|
||||
"-c:v", "libx264", "-preset", "fast", "-crf", "18",
|
||||
"-y", str(mp4_path),
|
||||
], stdin=subprocess.PIPE)
|
||||
|
||||
# Write-queue so rendering doesn't block on ffmpeg
|
||||
frame_queue: queue.Queue = queue.Queue(maxsize=args.fps * 2)
|
||||
stop_event = threading.Event()
|
||||
|
||||
def _writer():
|
||||
while not stop_event.is_set() or not frame_queue.empty():
|
||||
try:
|
||||
data = frame_queue.get(timeout=0.1)
|
||||
ffmpeg.stdin.write(data)
|
||||
except queue.Empty:
|
||||
pass
|
||||
ffmpeg.stdin.close()
|
||||
|
||||
writer_thread = threading.Thread(target=_writer, daemon=True)
|
||||
writer_thread.start()
|
||||
|
||||
gui_app.init_window(f"MICI Preview (recording) — {panel_name}", fps=args.fps)
|
||||
render_tex = rl.load_render_texture(MICI_W, MICI_H)
|
||||
widget = load_panel(panel_name)
|
||||
|
||||
win_w = rl.get_screen_width()
|
||||
win_h = rl.get_screen_height()
|
||||
total_frames = int(args.fps * args.duration)
|
||||
frame = 0
|
||||
settle = min(args.frames, total_frames // 4)
|
||||
|
||||
print(f" 🎬 recording {args.duration}s at {args.fps}fps → {mp4_path.name}")
|
||||
|
||||
while not rl.window_should_close() and frame < total_frames:
|
||||
rl.begin_drawing()
|
||||
rl.clear_background(CHROME_BG)
|
||||
|
||||
rl.begin_texture_mode(render_tex)
|
||||
rl.clear_background(rl.BLACK)
|
||||
widget.render(rl.Rectangle(0, 0, MICI_W, MICI_H))
|
||||
rl.end_texture_mode()
|
||||
|
||||
src = rl.Rectangle(0, 0, MICI_W, -MICI_H)
|
||||
dst = rl.Rectangle(0, 0, win_w, win_h)
|
||||
rl.draw_texture_pro(render_tex.texture, src, dst, rl.Vector2(0, 0), 0, rl.WHITE)
|
||||
|
||||
# REC progress bar
|
||||
pct = frame / total_frames
|
||||
rl.draw_rectangle(0, win_h - 3, int(win_w * pct), 3, rl.Color(255, 80, 80, 180))
|
||||
rl.draw_text(f"REC {frame}/{total_frames}", 6, 6, 11, rl.Color(255, 80, 80, 200))
|
||||
|
||||
rl.end_drawing()
|
||||
|
||||
# Queue raw RGBA from MICI-res texture for ffmpeg
|
||||
if frame >= settle:
|
||||
import ctypes
|
||||
img = rl.load_image_from_texture(render_tex.texture)
|
||||
colors = rl.load_image_colors(img)
|
||||
raw = bytes(ctypes.string_at(colors, MICI_W * MICI_H * 4))
|
||||
rl.unload_image_colors(colors)
|
||||
rl.unload_image(img)
|
||||
try:
|
||||
frame_queue.put(raw, timeout=1.0)
|
||||
except queue.Full:
|
||||
pass
|
||||
|
||||
frame += 1
|
||||
|
||||
rl.unload_render_texture(render_tex)
|
||||
rl.close_window()
|
||||
|
||||
stop_event.set()
|
||||
writer_thread.join(timeout=10)
|
||||
ffmpeg.wait(timeout=15)
|
||||
|
||||
print(f" ✅ video saved: {mp4_path}")
|
||||
return mp4_path
|
||||
|
||||
|
||||
# ─────────────────────────────────────────────────────────────────────────────
|
||||
# Live / hot-reload mode
|
||||
# ─────────────────────────────────────────────────────────────────────────────
|
||||
|
||||
def run_live(panel_name: str, args, mock_params):
|
||||
"""
|
||||
Interactive window with hot-reload.
|
||||
Watches the source file of the selected panel; re-imports it on save.
|
||||
Press S to take a screenshot, R to force reload, Q/Esc to quit.
|
||||
"""
|
||||
from openpilot.system.ui.lib.application import gui_app
|
||||
|
||||
gui_app.init_window(f"MICI Live — {panel_name} [S=shot R=reload Q=quit]", fps=args.fps)
|
||||
render_tex = rl.load_render_texture(MICI_W, MICI_H)
|
||||
win_w = rl.get_screen_width()
|
||||
win_h = rl.get_screen_height()
|
||||
|
||||
widget = load_panel(panel_name)
|
||||
last_mtime: dict[str, float] = {}
|
||||
|
||||
def _watch_paths() -> list[Path]:
|
||||
"""Files to watch for changes (panel module + shared theme/button)."""
|
||||
mods = [
|
||||
ROOT / "selfdrive/ui/iqpilot/mici/layouts" / f"{panel_name}.py",
|
||||
ROOT / "selfdrive/ui/iqpilot/theme.py",
|
||||
ROOT / "selfdrive/ui/mici/widgets/button.py",
|
||||
]
|
||||
return [p for p in mods if p.exists()]
|
||||
|
||||
def _needs_reload() -> bool:
|
||||
for p in _watch_paths():
|
||||
mtime = p.stat().st_mtime
|
||||
if last_mtime.get(str(p), 0) != mtime:
|
||||
last_mtime[str(p)] = mtime
|
||||
return True
|
||||
return False
|
||||
|
||||
def _reload():
|
||||
nonlocal widget
|
||||
print(" 🔄 reloading...")
|
||||
# Invalidate cached modules so importlib picks up changes
|
||||
prefix = "openpilot.iqpilot.ui.mici.layouts"
|
||||
for key in list(sys.modules.keys()):
|
||||
if key.startswith(prefix) or key == "openpilot.iqpilot.ui.theme":
|
||||
del sys.modules[key]
|
||||
try:
|
||||
widget = load_panel(panel_name)
|
||||
print(" ✅ reloaded OK")
|
||||
except Exception as e:
|
||||
print(f" ❌ reload error: {e}")
|
||||
|
||||
# Seed mtimes
|
||||
for p in _watch_paths():
|
||||
last_mtime[str(p)] = p.stat().st_mtime
|
||||
|
||||
out_dir = Path(args.out)
|
||||
shot_count = 0
|
||||
|
||||
while not rl.window_should_close():
|
||||
# Check for file changes
|
||||
if _needs_reload():
|
||||
_reload()
|
||||
|
||||
# Key bindings
|
||||
if rl.is_key_pressed(rl.KeyboardKey.KEY_Q) or rl.is_key_pressed(rl.KeyboardKey.KEY_ESCAPE):
|
||||
break
|
||||
if rl.is_key_pressed(rl.KeyboardKey.KEY_R):
|
||||
_reload()
|
||||
if rl.is_key_pressed(rl.KeyboardKey.KEY_S):
|
||||
# Take screenshot from render texture (native MICI res)
|
||||
img = rl.load_image_from_texture(render_tex.texture)
|
||||
rl.image_flip_vertical(img)
|
||||
rl.image_resize(img, MICI_W * 2, MICI_H * 2)
|
||||
out_dir.mkdir(parents=True, exist_ok=True)
|
||||
ts = int(time.time() * 1000)
|
||||
path = out_dir / f"mici_{panel_name}_live_{shot_count:03d}_{ts}.png"
|
||||
rl.export_image(img, str(path))
|
||||
rl.unload_image(img)
|
||||
shot_count += 1
|
||||
print(f" 📸 screenshot saved: {path}")
|
||||
if args.open:
|
||||
subprocess.Popen(["open", str(path)])
|
||||
|
||||
rl.begin_drawing()
|
||||
rl.clear_background(CHROME_BG)
|
||||
|
||||
# Render widget into MICI-sized texture, blit to full window
|
||||
rl.begin_texture_mode(render_tex)
|
||||
rl.clear_background(rl.BLACK)
|
||||
widget.render(rl.Rectangle(0, 0, MICI_W, MICI_H))
|
||||
rl.end_texture_mode()
|
||||
|
||||
src = rl.Rectangle(0, 0, MICI_W, -MICI_H)
|
||||
dst = rl.Rectangle(0, 0, win_w, win_h)
|
||||
rl.draw_texture_pro(render_tex.texture, src, dst, rl.Vector2(0, 0), 0, rl.WHITE)
|
||||
|
||||
# Status hint overlay
|
||||
watch_files = _watch_paths()
|
||||
hint = f"watching {len(watch_files)} file(s) | S=shot R=reload Q=quit"
|
||||
rl.draw_text(hint, 6, win_h - 18, 11, rl.Color(80, 80, 80, 160))
|
||||
|
||||
rl.end_drawing()
|
||||
|
||||
rl.unload_render_texture(render_tex)
|
||||
rl.close_window()
|
||||
|
||||
|
||||
# ─────────────────────────────────────────────────────────────────────────────
|
||||
# Entry point
|
||||
# ─────────────────────────────────────────────────────────────────────────────
|
||||
|
||||
def parse_args():
|
||||
p = argparse.ArgumentParser(
|
||||
description="IQ.Pilot MICI UI Preview Tool",
|
||||
formatter_class=argparse.RawDescriptionHelpFormatter,
|
||||
epilog=__doc__,
|
||||
)
|
||||
p.add_argument("--panel", default="steering",
|
||||
help="Panel name or 'all'")
|
||||
p.add_argument("--mode", default="screenshot",
|
||||
choices=["screenshot", "video", "live"],
|
||||
help="Capture mode")
|
||||
p.add_argument("--frames", type=int, default=90,
|
||||
help="Settle frames before screenshot")
|
||||
p.add_argument("--shots", type=int, default=1,
|
||||
help="Number of screenshots")
|
||||
p.add_argument("--duration", type=float, default=4.0,
|
||||
help="Video duration in seconds")
|
||||
p.add_argument("--fps", type=int, default=60,
|
||||
help="Render FPS")
|
||||
p.add_argument("--scale", type=float, default=2.5,
|
||||
help="Window scale multiplier (1.0 = native 536×240)")
|
||||
p.add_argument("--out", default=str(DEFAULT_OUT),
|
||||
help="Output directory")
|
||||
p.add_argument("--open", action="store_true", default=True,
|
||||
help="Open output after capture (macOS)")
|
||||
p.add_argument("--no-open", dest="open", action="store_false")
|
||||
p.add_argument("--mock", action="store_true", default=True,
|
||||
help="Use mock UI state (no live process needed)")
|
||||
p.add_argument("--no-mock", dest="mock", action="store_false")
|
||||
p.add_argument("--accent", default=None,
|
||||
help="Override neon accent color, e.g. '#FF6B00'")
|
||||
return p.parse_args()
|
||||
|
||||
|
||||
def main():
|
||||
args = parse_args()
|
||||
out_dir = Path(args.out)
|
||||
out_dir.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
# Set SCALE env var BEFORE gui_app module is imported (it reads it at import time)
|
||||
os.environ["SCALE"] = str(args.scale)
|
||||
|
||||
# Apply mock state early (before any widget imports)
|
||||
mock_params = None
|
||||
if args.mock:
|
||||
print(" 🎭 using mock UI state (--no-mock to use live params)")
|
||||
mock_params = _patch_mock_state()
|
||||
|
||||
# Override accent color if requested
|
||||
if args.accent:
|
||||
if mock_params:
|
||||
mock_params.put("UIAccentColor", args.accent)
|
||||
print(f" 🎨 accent color: {args.accent}")
|
||||
|
||||
panels = ALL_PANELS if args.panel == "all" else [args.panel]
|
||||
mode = args.mode
|
||||
|
||||
if mode == "live" and len(panels) > 1:
|
||||
print(" ⚠️ live mode only supports a single panel. Using first panel.")
|
||||
panels = panels[:1]
|
||||
|
||||
all_outputs: list[Path] = []
|
||||
|
||||
for panel_name in panels:
|
||||
print(f"\n ▶ {mode.upper()} — panel: {panel_name}")
|
||||
|
||||
try:
|
||||
if mode == "live":
|
||||
run_live(panel_name, args, mock_params)
|
||||
elif mode == "screenshot":
|
||||
saved = run_screenshot(panel_name, args, out_dir)
|
||||
all_outputs.extend(saved)
|
||||
elif mode == "video":
|
||||
mp4 = run_video(panel_name, args, out_dir)
|
||||
all_outputs.append(mp4)
|
||||
except Exception as e:
|
||||
print(f" ❌ failed for panel '{panel_name}': {e}")
|
||||
import traceback; traceback.print_exc()
|
||||
if rl.is_window_ready():
|
||||
rl.close_window()
|
||||
|
||||
# Open all outputs at once
|
||||
if args.open and all_outputs:
|
||||
print(f"\n 📂 opening {len(all_outputs)} output(s)...")
|
||||
subprocess.Popen(["open"] + [str(p) for p in all_outputs])
|
||||
|
||||
print("\n ✨ done\n")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
53
tools/joystick/README.md
Normal file
53
tools/joystick/README.md
Normal file
@@ -0,0 +1,53 @@
|
||||
# Joystick
|
||||
|
||||
**Hardware needed**: device running openpilot, laptop, joystick (optional)
|
||||
|
||||
With joystick_control, you can connect your laptop to your comma device over the network and debug controls using a joystick or keyboard.
|
||||
joystick_control uses [inputs](https://pypi.org/project/inputs) which supports many common gamepads and joysticks.
|
||||
|
||||
## Usage
|
||||
|
||||
The car must be off, and openpilot must be offroad before starting `joystick_control`.
|
||||
|
||||
### Using a keyboard
|
||||
|
||||
SSH into your comma device and start joystick_control with the following command:
|
||||
|
||||
```shell
|
||||
tools/joystick/joystick_control.py --keyboard
|
||||
```
|
||||
|
||||
The available buttons and axes will print showing their key mappings. In general, the WASD keys control gas and brakes and steering torque in 5% increments.
|
||||
|
||||
### Joystick on your comma three
|
||||
|
||||
Plug the joystick into your comma three aux USB-C port. Then, SSH into the device and start `joystick_control.py`.
|
||||
|
||||
### Joystick on your laptop
|
||||
|
||||
In order to use a joystick over the network, we need to run joystick_control locally from your laptop and have it send `testJoystick` packets over the network to the comma device.
|
||||
|
||||
1. Connect a joystick to your PC.
|
||||
2. Connect your laptop to your comma device's hotspot and open a new SSH shell. Since joystick_control is being run on your laptop, we need to write a parameter to let controlsd know to start in joystick debug mode:
|
||||
```shell
|
||||
# on your comma device
|
||||
echo -n "1" > /data/params/d/JoystickDebugMode
|
||||
```
|
||||
3. Run bridge with your laptop's IP address. This republishes the `testJoystick` packets sent from your laptop so that openpilot can receive them:
|
||||
```shell
|
||||
# on your comma device
|
||||
cereal/messaging/bridge {LAPTOP_IP} testJoystick
|
||||
```
|
||||
4. Start joystick_control on your laptop in ZMQ mode.
|
||||
```shell
|
||||
# on your laptop
|
||||
export ZMQ=1
|
||||
tools/joystick/joystick_control.py
|
||||
```
|
||||
|
||||
---
|
||||
Now start your car and openpilot should go into joystick mode with an alert on startup! The status of the axes will display on the alert, while button statuses print in the shell.
|
||||
|
||||
Make sure the conditions are met in the panda to allow controls (e.g. cruise control engaged). You can also make a modification to the panda code to always allow controls.
|
||||
|
||||
|
||||
272
tools/joystick/joystick_control.py
Executable file
272
tools/joystick/joystick_control.py
Executable file
@@ -0,0 +1,272 @@
|
||||
#!/usr/bin/env python3
|
||||
import argparse
|
||||
import os
|
||||
import signal
|
||||
import socket
|
||||
import struct
|
||||
import threading
|
||||
import time
|
||||
|
||||
import numpy as np
|
||||
from inputs import UnpluggedError, get_gamepad
|
||||
|
||||
from cereal import messaging
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import Ratekeeper
|
||||
from openpilot.system.hardware import HARDWARE
|
||||
from openpilot.tools.lib.kbhit import KBHit
|
||||
|
||||
REMOTE_PORT_DEFAULT = 8765
|
||||
REMOTE_TIMEOUT_S = 0.25
|
||||
REMOTE_PUBLISH_HZ = 30
|
||||
LOCAL_PUBLISH_HZ = 100
|
||||
|
||||
|
||||
class Keyboard:
|
||||
def __init__(self):
|
||||
self.kb = KBHit()
|
||||
self.axis_increment = 0.05 # 5% of full actuation each key press
|
||||
self.axes_map = {'w': 'gb', 's': 'gb',
|
||||
'a': 'steer', 'd': 'steer'}
|
||||
self.axes_values = {'gb': 0., 'steer': 0.}
|
||||
self.axes_order = ['gb', 'steer']
|
||||
self.cancel = False
|
||||
self.idle_sleep_s = 0.0
|
||||
|
||||
def update(self):
|
||||
key = self.kb.getch().lower()
|
||||
self.cancel = False
|
||||
if key == 'r':
|
||||
self.axes_values = dict.fromkeys(self.axes_values, 0.)
|
||||
elif key == 'c':
|
||||
self.cancel = True
|
||||
elif key in self.axes_map:
|
||||
axis = self.axes_map[key]
|
||||
incr = self.axis_increment if key in ['w', 'a'] else -self.axis_increment
|
||||
self.axes_values[axis] = float(np.clip(self.axes_values[axis] + incr, -1, 1))
|
||||
else:
|
||||
return False
|
||||
return True
|
||||
|
||||
def get_buttons(self):
|
||||
return [False, self.cancel]
|
||||
|
||||
|
||||
class Joystick:
|
||||
def __init__(self):
|
||||
# This class supports a PlayStation 5 DualSense controller on the comma 3X
|
||||
# Using both analog sticks: left stick Y for gas/brake, right stick X for steering
|
||||
self.cancel_button = 'BTN_NORTH' # BTN_NORTH=X/triangle
|
||||
if HARDWARE.get_device_type() == 'pc':
|
||||
accel_axis = 'ABS_Y' # Left stick Y-axis
|
||||
steer_axis = 'ABS_RX' # Right stick X-axis
|
||||
self.flip_map = {} # No flipping needed
|
||||
else:
|
||||
accel_axis = 'ABS_Y' # Left stick Y-axis
|
||||
steer_axis = 'ABS_Z' # Right stick X-axis
|
||||
self.flip_map = {} # No flipping needed
|
||||
|
||||
self.min_axis_value = {accel_axis: 0., steer_axis: 0.}
|
||||
self.max_axis_value = {accel_axis: 255., steer_axis: 255.}
|
||||
self.axes_values = {accel_axis: 0., steer_axis: 0.}
|
||||
self.axes_order = [accel_axis, steer_axis]
|
||||
self.cancel = False
|
||||
self.idle_sleep_s = 0.0
|
||||
|
||||
def update(self):
|
||||
try:
|
||||
joystick_event = get_gamepad()[0]
|
||||
except (OSError, UnpluggedError):
|
||||
self.axes_values = dict.fromkeys(self.axes_values, 0.)
|
||||
return False
|
||||
|
||||
event = (joystick_event.code, joystick_event.state)
|
||||
|
||||
# flip left trigger to negative accel
|
||||
if event[0] in self.flip_map:
|
||||
event = (self.flip_map[event[0]], -event[1])
|
||||
|
||||
if event[0] == self.cancel_button:
|
||||
if event[1] == 1:
|
||||
self.cancel = True
|
||||
elif event[1] == 0: # state 0 is falling edge
|
||||
self.cancel = False
|
||||
elif event[0] in self.axes_values:
|
||||
self.max_axis_value[event[0]] = max(event[1], self.max_axis_value[event[0]])
|
||||
self.min_axis_value[event[0]] = min(event[1], self.min_axis_value[event[0]])
|
||||
|
||||
norm = -float(np.interp(event[1], [self.min_axis_value[event[0]], self.max_axis_value[event[0]]], [-1., 1.]))
|
||||
norm = norm if abs(norm) > 0.03 else 0. # center can be noisy, deadzone of 3%
|
||||
self.axes_values[event[0]] = norm
|
||||
else:
|
||||
return False
|
||||
return True
|
||||
|
||||
def get_buttons(self):
|
||||
return [False, self.cancel]
|
||||
|
||||
|
||||
class RemoteJoystick:
|
||||
def __init__(self, host: str, port: int):
|
||||
self.addr = (host, port)
|
||||
self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
self.socket.bind(self.addr)
|
||||
self.socket.settimeout(0.1)
|
||||
|
||||
self.axes_values = {'gb': 0.0, 'steer': 0.0}
|
||||
self.axes_order = ['gb', 'steer']
|
||||
self.buttons = [False, False]
|
||||
self.last_update = 0.0
|
||||
self.authenticated = False
|
||||
self.client_addr = None
|
||||
self.idle_sleep_s = 0.01
|
||||
|
||||
def _clamp(self, value: float) -> float:
|
||||
return float(np.clip(value, -1.0, 1.0))
|
||||
|
||||
def _handle_timeout(self, now: float) -> None:
|
||||
if self.authenticated and (now - self.last_update) > REMOTE_TIMEOUT_S:
|
||||
self.axes_values = {'gb': 0.0, 'steer': 0.0}
|
||||
self.buttons = [False, False]
|
||||
|
||||
def _send_auth_ok(self, addr) -> None:
|
||||
try:
|
||||
self.socket.sendto(bytes([1]), addr)
|
||||
except OSError:
|
||||
pass
|
||||
|
||||
def update(self):
|
||||
now = time.monotonic()
|
||||
try:
|
||||
data, addr = self.socket.recvfrom(64)
|
||||
except socket.timeout:
|
||||
self._handle_timeout(now)
|
||||
return False
|
||||
except OSError:
|
||||
return False
|
||||
|
||||
if not data:
|
||||
return False
|
||||
|
||||
msg_type = data[0]
|
||||
if msg_type == 0:
|
||||
self.client_addr = addr
|
||||
self.authenticated = True
|
||||
self._send_auth_ok(addr)
|
||||
return True
|
||||
|
||||
if msg_type == 2:
|
||||
try:
|
||||
payload = data[1:].decode("utf-8", errors="strict").strip()
|
||||
steer_s, accel_s, engage_s, disengage_s = payload.split(",", 3)
|
||||
steer = float(steer_s)
|
||||
accel = float(accel_s)
|
||||
engage = engage_s == "1"
|
||||
disengage = disengage_s == "1"
|
||||
except (UnicodeDecodeError, ValueError):
|
||||
return False
|
||||
|
||||
self.axes_values['steer'] = self._clamp(steer)
|
||||
self.axes_values['gb'] = self._clamp(accel)
|
||||
self.buttons = [engage, disengage]
|
||||
self.last_update = now
|
||||
return True
|
||||
|
||||
if msg_type != 1 or len(data) < 9:
|
||||
return False
|
||||
|
||||
steer, accel = struct.unpack_from("<ff", data, 1)
|
||||
engage = bool(data[9]) if len(data) > 9 else False
|
||||
disengage = bool(data[10]) if len(data) > 10 else False
|
||||
|
||||
self.axes_values['steer'] = self._clamp(steer)
|
||||
self.axes_values['gb'] = self._clamp(accel)
|
||||
self.buttons = [engage, disengage]
|
||||
self.last_update = now
|
||||
return True
|
||||
|
||||
def get_buttons(self):
|
||||
return self.buttons
|
||||
|
||||
|
||||
def send_thread(joystick, show_values: bool):
|
||||
pm = messaging.PubMaster(['testJoystick'])
|
||||
|
||||
publish_hz = REMOTE_PUBLISH_HZ if isinstance(joystick, RemoteJoystick) else LOCAL_PUBLISH_HZ
|
||||
rk = Ratekeeper(publish_hz, print_delay_threshold=None)
|
||||
|
||||
while True:
|
||||
if show_values and rk.frame % 20 == 0:
|
||||
print('\n' + ', '.join(f'{name}: {round(v, 3)}' for name, v in joystick.axes_values.items()))
|
||||
|
||||
joystick_msg = messaging.new_message('testJoystick')
|
||||
joystick_msg.valid = True
|
||||
joystick_msg.testJoystick.axes = [joystick.axes_values[ax] for ax in joystick.axes_order]
|
||||
joystick_msg.testJoystick.buttons = joystick.get_buttons()
|
||||
|
||||
pm.send('testJoystick', joystick_msg)
|
||||
|
||||
rk.keep_time()
|
||||
|
||||
|
||||
def joystick_control_thread(joystick, show_values: bool):
|
||||
Params().put_bool('JoystickDebugMode', True)
|
||||
try:
|
||||
threading.Thread(target=send_thread, args=(joystick, show_values), daemon=True).start()
|
||||
while True:
|
||||
updated = joystick.update()
|
||||
if not updated and joystick.idle_sleep_s > 0:
|
||||
time.sleep(joystick.idle_sleep_s)
|
||||
finally:
|
||||
Params().put_bool('JoystickDebugMode', False)
|
||||
|
||||
|
||||
def main():
|
||||
joystick_control_thread(Joystick(), True)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
parser = argparse.ArgumentParser(description='Publishes events from your joystick to control your car.\n' +
|
||||
'openpilot must be offroad before starting joystick_control. This tool supports ' +
|
||||
'a PlayStation 5 DualSense controller on the comma 3X.',
|
||||
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
|
||||
parser.add_argument('--keyboard', action='store_true', help='Use your keyboard instead of a joystick')
|
||||
parser.add_argument('--remote', action='store_true', help='Listen for UDP joystick input')
|
||||
parser.add_argument('--listen', type=int, default=REMOTE_PORT_DEFAULT, help='UDP port for remote joystick input')
|
||||
parser.add_argument('--listen-address', default='0.0.0.0', help='UDP address to bind for remote input')
|
||||
args = parser.parse_args()
|
||||
|
||||
if not Params().get_bool("IsOffroad") and "ZMQ" not in os.environ:
|
||||
print("The car must be off before running joystick_control.")
|
||||
exit()
|
||||
|
||||
if args.remote and args.keyboard:
|
||||
print("Choose only one input mode.")
|
||||
exit()
|
||||
|
||||
print()
|
||||
if args.remote:
|
||||
print(f'Listening for remote joystick on {args.listen_address}:{args.listen}')
|
||||
elif args.keyboard:
|
||||
print('Gas/brake control: `W` and `S` keys')
|
||||
print('Steering control: `A` and `D` keys')
|
||||
print('Buttons')
|
||||
print('- `R`: Resets axes')
|
||||
print('- `C`: Cancel cruise control')
|
||||
else:
|
||||
print('Using joystick, make sure to run cereal/messaging/bridge on your device if running over the network!')
|
||||
print('If not running on a comma device, the mapping may need to be adjusted.')
|
||||
|
||||
def handle_exit(signum, _frame):
|
||||
Params().put_bool('JoystickDebugMode', False)
|
||||
raise SystemExit
|
||||
|
||||
signal.signal(signal.SIGINT, handle_exit)
|
||||
signal.signal(signal.SIGTERM, handle_exit)
|
||||
|
||||
if args.remote:
|
||||
joystick = RemoteJoystick(args.listen_address, int(args.listen))
|
||||
joystick_control_thread(joystick, False)
|
||||
else:
|
||||
joystick = Keyboard() if args.keyboard else Joystick()
|
||||
joystick_control_thread(joystick, True)
|
||||
139
tools/joystick/joystickd.py
Executable file
139
tools/joystick/joystickd.py
Executable file
@@ -0,0 +1,139 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import math
|
||||
import numpy as np
|
||||
|
||||
from cereal import messaging, car, custom
|
||||
from iqdbc.car.vehicle_model import VehicleModel
|
||||
from openpilot.common.realtime import DT_CTRL, Ratekeeper
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
|
||||
LongCtrlState = car.CarControl.Actuators.LongControlState
|
||||
MAX_LAT_ACCEL = 5.0
|
||||
MAX_STEERING_ANGLE_DEG = 500.0
|
||||
ACCEL_RELEASE_THRESHOLD = 0.01
|
||||
DECEL_REQUEST_THRESHOLD = -0.02
|
||||
STOPPING_HOLD_SPEED_MARGIN = 0.3
|
||||
STOPPING_SPEED = 0.25
|
||||
|
||||
|
||||
def get_lateral_joystick_outputs(CP: car.CarParams, VM: VehicleModel, v_ego: float, roll: float, steer_axis: float) -> tuple[float, float, float]:
|
||||
steer_axis = float(np.clip(steer_axis, -1, 1))
|
||||
steering_angle_deg = steer_axis * MAX_STEERING_ANGLE_DEG
|
||||
curvature = -VM.calc_curvature(math.radians(steering_angle_deg), v_ego, roll)
|
||||
|
||||
if CP.steerControlType in (car.CarParams.SteerControlType.angle, car.CarParams.SteerControlType.curvatureDEPRECATED):
|
||||
return 0.0, steering_angle_deg, curvature
|
||||
|
||||
max_curvature = MAX_LAT_ACCEL / max(v_ego ** 2, 5)
|
||||
max_angle = min(math.degrees(VM.get_steer_from_curvature(max_curvature, v_ego, roll)), MAX_STEERING_ANGLE_DEG)
|
||||
return steer_axis, steer_axis * max_angle, steer_axis * -max_curvature
|
||||
|
||||
|
||||
def joystickd_thread():
|
||||
params = Params()
|
||||
cloudlog.info("joystickd is waiting for CarParams")
|
||||
CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
|
||||
CP_IQ = messaging.log_from_bytes(params.get("IQCarParams", block=True), custom.IQCarParams)
|
||||
VM = VehicleModel(CP)
|
||||
|
||||
sm = messaging.SubMaster(['carState', 'onroadEvents', 'liveParameters', 'selfdriveState', 'iqState', 'testJoystick'], frequency=1. / DT_CTRL)
|
||||
pm = messaging.PubMaster(['carControl', 'controlsState'])
|
||||
|
||||
# Stop-hold behavior for joystick long control:
|
||||
# - enter hold only when user requested decel and we are near/at stop
|
||||
# - neutral input does not request decel while rolling
|
||||
# - release hold on positive accel request
|
||||
decel_intent_latched = False
|
||||
stop_hold_latched = False
|
||||
|
||||
rk = Ratekeeper(100, print_delay_threshold=None)
|
||||
while 1:
|
||||
sm.update(0)
|
||||
|
||||
cc_msg = messaging.new_message('carControl')
|
||||
cc_msg.valid = True
|
||||
CC = cc_msg.carControl
|
||||
ss = sm['selfdriveState']
|
||||
ss_iq = sm['iqState']
|
||||
aol_enabled = bool(getattr(ss_iq.aol, 'enabled', False))
|
||||
aol_active = bool(getattr(ss_iq.aol, 'active', False))
|
||||
joystick_angle_lat_active = aol_active or (
|
||||
aol_enabled and CP.steerControlType == car.CarParams.SteerControlType.angle
|
||||
)
|
||||
|
||||
CC.enabled = bool(ss.enabled or aol_enabled)
|
||||
CC.latActive = bool(ss.active or joystick_angle_lat_active) and not sm['carState'].steerFaultTemporary and not sm['carState'].steerFaultPermanent
|
||||
long_through_override = CP_IQ.longActiveWithGasOverride and CP.openpilotLongitudinalControl
|
||||
override_longitudinal = any(e.overrideLongitudinal for e in sm['onroadEvents'])
|
||||
CC.longActive = bool(ss.enabled) and (not override_longitudinal or long_through_override) and CP.openpilotLongitudinalControl
|
||||
CC.cruiseControl.cancel = sm['carState'].cruiseState.enabled and (not CC.enabled or not CP.pcmCruise)
|
||||
CC.hudControl.leadDistanceBars = 2
|
||||
|
||||
actuators = CC.actuators
|
||||
|
||||
# reset joystick if it hasn't been received in a while
|
||||
should_reset_joystick = sm.recv_frame['testJoystick'] == 0 or (sm.frame - sm.recv_frame['testJoystick'])*DT_CTRL > 0.2
|
||||
|
||||
if not should_reset_joystick:
|
||||
joystick_axes = sm['testJoystick'].axes
|
||||
else:
|
||||
joystick_axes = [0.0, 0.0]
|
||||
|
||||
if CC.longActive:
|
||||
accel_cmd = float(np.clip(joystick_axes[0], -1, 1))
|
||||
actuators.accel = 4.0 * accel_cmd
|
||||
|
||||
positive_accel_requested = accel_cmd > ACCEL_RELEASE_THRESHOLD
|
||||
negative_accel_requested = accel_cmd < DECEL_REQUEST_THRESHOLD
|
||||
near_stop = sm['carState'].standstill or sm['carState'].vEgo <= (STOPPING_SPEED + STOPPING_HOLD_SPEED_MARGIN)
|
||||
|
||||
if positive_accel_requested:
|
||||
stop_hold_latched = False
|
||||
decel_intent_latched = False
|
||||
elif negative_accel_requested:
|
||||
decel_intent_latched = True
|
||||
|
||||
if decel_intent_latched and near_stop and not positive_accel_requested:
|
||||
stop_hold_latched = True
|
||||
|
||||
# If we are moving again and driver is not asking for decel, clear stale hold state.
|
||||
if stop_hold_latched and sm['carState'].vEgo > (STOPPING_SPEED + STOPPING_HOLD_SPEED_MARGIN) and not negative_accel_requested:
|
||||
stop_hold_latched = False
|
||||
decel_intent_latched = False
|
||||
|
||||
actuators.longControlState = LongCtrlState.stopping if stop_hold_latched else LongCtrlState.pid
|
||||
CC.cruiseControl.resume = positive_accel_requested
|
||||
else:
|
||||
decel_intent_latched = False
|
||||
stop_hold_latched = False
|
||||
|
||||
if CC.latActive:
|
||||
torque, steering_angle_deg, curvature = get_lateral_joystick_outputs(CP, VM, sm['carState'].vEgo, sm['liveParameters'].roll, joystick_axes[1])
|
||||
actuators.torque = torque
|
||||
actuators.steeringAngleDeg = steering_angle_deg
|
||||
actuators.curvature = curvature
|
||||
|
||||
pm.send('carControl', cc_msg)
|
||||
|
||||
cs_msg = messaging.new_message('controlsState')
|
||||
cs_msg.valid = True
|
||||
controlsState = cs_msg.controlsState
|
||||
controlsState.lateralControlState.init('debugState')
|
||||
|
||||
lp = sm['liveParameters']
|
||||
steer_angle_without_offset = math.radians(sm['carState'].steeringAngleDeg - lp.angleOffsetDeg)
|
||||
controlsState.curvature = -VM.calc_curvature(steer_angle_without_offset, sm['carState'].vEgo, lp.roll)
|
||||
|
||||
pm.send('controlsState', cs_msg)
|
||||
|
||||
rk.keep_time()
|
||||
|
||||
|
||||
def main():
|
||||
joystickd_thread()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
30
tools/joystick/tests/test_joystickd.py
Normal file
30
tools/joystick/tests/test_joystickd.py
Normal file
@@ -0,0 +1,30 @@
|
||||
from cereal import car
|
||||
|
||||
from openpilot.tools.joystick.joystickd import get_lateral_joystick_outputs
|
||||
|
||||
|
||||
class StubVehicleModel:
|
||||
def get_steer_from_curvature(self, curvature: float, v_ego: float, roll: float) -> float:
|
||||
return curvature
|
||||
|
||||
|
||||
def test_angle_cars_use_angle_outputs():
|
||||
CP = car.CarParams.new_message()
|
||||
CP.steerControlType = car.CarParams.SteerControlType.angle
|
||||
|
||||
torque, steering_angle_deg, curvature = get_lateral_joystick_outputs(CP, StubVehicleModel(), 20.0, 0.0, 0.5)
|
||||
|
||||
assert torque == 0.0
|
||||
assert steering_angle_deg != 0.0
|
||||
assert curvature < 0.0
|
||||
|
||||
|
||||
def test_torque_cars_keep_torque_outputs():
|
||||
CP = car.CarParams.new_message()
|
||||
CP.steerControlType = car.CarParams.SteerControlType.torque
|
||||
|
||||
torque, steering_angle_deg, curvature = get_lateral_joystick_outputs(CP, StubVehicleModel(), 20.0, 0.0, 0.5)
|
||||
|
||||
assert torque == 0.5
|
||||
assert steering_angle_deg != 0.0
|
||||
assert curvature < 0.0
|
||||
1
tools/lateral_maneuvers/.gitignore
vendored
Normal file
1
tools/lateral_maneuvers/.gitignore
vendored
Normal file
@@ -0,0 +1 @@
|
||||
/lateral_reports/
|
||||
54
tools/lateral_maneuvers/README.md
Normal file
54
tools/lateral_maneuvers/README.md
Normal file
@@ -0,0 +1,54 @@
|
||||
# Lateral Maneuvers Testing Tool
|
||||
|
||||
> [!WARNING]
|
||||
> Use caution when using this tool.
|
||||
|
||||
Test your vehicle's lateral control tuning with this tool. The tool will test the vehicle's ability to follow a few lateral maneuvers and includes a tool to generate a report from the route.
|
||||
|
||||
## Instructions
|
||||
|
||||
1. Check out a development branch such as `master-mici` on your device. The toggle is hidden on release branches.
|
||||
2. The full maneuver suite runs at 20 and 30 mph.
|
||||
3. Enable "Lateral Maneuver Mode" in Settings > Developer on the device while offroad. Alternatively, set the parameter manually:
|
||||
|
||||
```sh
|
||||
echo -n 1 > /data/params/d/LateralManeuverMode
|
||||
```
|
||||
|
||||
4. Turn your vehicle back on. You will see "Lateral Maneuver Mode".
|
||||
|
||||
5. Ensure the area ahead is clear, as IQ.Pilot will command lateral acceleration steps in this mode. Once you are ready, set ACC manually to the target speed shown on screen and let IQ.Pilot stabilize lateral. After 2 seconds of steady straight driving on a road under 250 m radius and under 6.8° of roll, the maneuver will begin automatically. IQ.Pilot lateral control stays engaged between maneuvers normally while waiting for the next maneuver's readiness conditions. The maneuver will be aborted and repeated if speed is out of range, the steering wheel or gas is touched, or IQ.Pilot disengages.
|
||||
|
||||
6. When the testing is complete, you'll see an alert that says "Maneuvers Finished." Complete the route by pulling over and turning off the vehicle.
|
||||
|
||||
7. Locate the route(s) — they will stand out with lots of orange intervals in their timeline. Ensure "All logs" show as "uploaded."
|
||||
|
||||
8. Gather the route ID and then run the report generator. The file will be exported to the same directory:
|
||||
|
||||
```sh
|
||||
$ python tools/lateral_maneuvers/generate_report.py 98395b7c5b27882e/000001cc--5a73bde686
|
||||
|
||||
processing report for KIA_EV6
|
||||
plotting maneuver: step right 20mph, runs: 3
|
||||
plotting maneuver: step left 20mph, runs: 3
|
||||
plotting maneuver: sine 0.5Hz 20mph, runs: 3
|
||||
plotting maneuver: step right 30mph, runs: 3
|
||||
|
||||
Opening report: tools/lateral_maneuvers/lateral_reports/KIA_EV6_98395b7c5b27882e_000001cc--5a73bde686.html
|
||||
```
|
||||
|
||||
The IQ.Pilot `generate_report.py` also takes a path to a local `rlog.zst` or a directory of them, supports
|
||||
auto-detection of lateral sweeps in any route without `alertDebug` markers (pass `--auto`), and ranks the
|
||||
top-N highest-peak sweeps by speed/peak filters. See `generate_report.py --help`.
|
||||
|
||||
## Testing the tooling without a car
|
||||
|
||||
`sim_maneuvers.py` runs `lateral_maneuversd` as a real process against a synthetic steering rack and writes an
|
||||
rlog that `generate_report.py` reads. Use it to verify the daemon and the report generator after changing either:
|
||||
|
||||
```sh
|
||||
$ python tools/lateral_maneuvers/sim_maneuvers.py --out /tmp/lat/rlog.zst
|
||||
$ python tools/lateral_maneuvers/generate_report.py /tmp/lat/rlog.zst
|
||||
```
|
||||
|
||||
The full suite takes about 5 minutes of wall clock; `--max-maneuvers N` stops early.
|
||||
261
tools/lateral_maneuvers/generate_report.py
Normal file
261
tools/lateral_maneuvers/generate_report.py
Normal file
@@ -0,0 +1,261 @@
|
||||
#!/usr/bin/env python3
|
||||
import argparse
|
||||
import base64
|
||||
import io
|
||||
import math
|
||||
import numpy as np
|
||||
import os
|
||||
import webbrowser
|
||||
from collections import defaultdict
|
||||
from pathlib import Path
|
||||
import matplotlib.pyplot as plt
|
||||
from openpilot.common.utils import tabulate
|
||||
|
||||
from cereal import car
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_torque import LP_FILTER_CUTOFF_HZ
|
||||
from openpilot.tools.lib.logreader import LogReader
|
||||
from openpilot.system.hardware.hw import Paths
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.tools.longitudinal_maneuvers.generate_report import format_car_params
|
||||
|
||||
ANGLE_CONTROL = (car.CarParams.SteerControlType.angle, car.CarParams.SteerControlType.curvatureDEPRECATED)
|
||||
|
||||
|
||||
def lat_accel(curvature, v):
|
||||
return curvature * max(v, 1.0) ** 2
|
||||
|
||||
|
||||
def report(platform, route, _description, CP, ID, maneuvers):
|
||||
output_path = Path(__file__).resolve().parent / "lateral_reports"
|
||||
output_fn = output_path / f"{platform}_{route.replace('/', '_').replace('|', '_')}.html"
|
||||
output_path.mkdir(exist_ok=True)
|
||||
target_cross_times = defaultdict(list)
|
||||
|
||||
builder = [
|
||||
"<style>summary { cursor: pointer; }\n td, th { padding: 8px; } </style>\n",
|
||||
"<h1>Lateral maneuver report</h1>\n",
|
||||
f"<h3>{platform}</h3>\n",
|
||||
f"<h3>{route}</h3>\n",
|
||||
f"<h3>{ID.gitCommit}, {ID.gitBranch}, {ID.gitRemote}</h3>\n",
|
||||
]
|
||||
if _description is not None:
|
||||
builder.append(f"<h3>Description: {_description}</h3>\n")
|
||||
builder.append(f"<details><summary><h3 style='display: inline-block;'>CarParams</h3></summary><pre>{format_car_params(CP)}</pre></details>\n")
|
||||
builder.append('{ summary }') # to be replaced below
|
||||
for description, runs in maneuvers:
|
||||
# filter incomplete runs
|
||||
completed_runs = [msgs for msgs in runs
|
||||
if any(m.alertDebug.alertText1 == 'Complete' for m in msgs if m.which() == 'alertDebug')]
|
||||
print(f'plotting maneuver: {description}, runs: {len(completed_runs)}')
|
||||
if not completed_runs:
|
||||
continue
|
||||
builder.append("<div style='border-top: 1px solid #000; margin: 20px 0;'></div>\n")
|
||||
builder.append(f"<h2>{description}</h2>\n")
|
||||
for run, msgs in enumerate(completed_runs):
|
||||
last_active = max(m.logMonoTime for m in msgs if m.which() == 'lateralManeuverPlan' and m.valid)
|
||||
msgs = [m for m in msgs if m.logMonoTime <= last_active]
|
||||
t_carControl, carControl = zip(*[(m.logMonoTime, m.carControl) for m in msgs if m.which() == 'carControl'], strict=True)
|
||||
t_carState, carState = zip(*[(m.logMonoTime, m.carState) for m in msgs if m.which() == 'carState'], strict=True)
|
||||
t_controlsState, controlsState = zip(*[(m.logMonoTime, m.controlsState) for m in msgs if m.which() == 'controlsState'], strict=True)
|
||||
t_lateralPlan, lateralPlan = zip(*[(m.logMonoTime, m.lateralManeuverPlan) for m in msgs if m.which() == 'lateralManeuverPlan' and m.valid], strict=True)
|
||||
t_carOutput, carOutput = zip(*[(m.logMonoTime, m.carOutput) for m in msgs if m.which() == 'carOutput'], strict=True)
|
||||
|
||||
# make time relative seconds
|
||||
t_carControl = [(t - t_carControl[0]) / 1e9 for t in t_carControl]
|
||||
t_carState = [(t - t_carState[0]) / 1e9 for t in t_carState]
|
||||
t_controlsState = [(t - t_controlsState[0]) / 1e9 for t in t_controlsState]
|
||||
t_lateralPlan = [(t - t_lateralPlan[0]) / 1e9 for t in t_lateralPlan]
|
||||
t_carOutput = [(t - t_carOutput[0]) / 1e9 for t in t_carOutput]
|
||||
|
||||
# maneuver validity
|
||||
latActive = [m.latActive for m in carControl]
|
||||
maneuver_valid = all(latActive) and not any(cs.steeringPressed for cs in carState)
|
||||
|
||||
_open = 'open' if maneuver_valid else ''
|
||||
title = f'Run #{int(run)+1}' + (' <span style="color: red">(invalid maneuver!)</span>' if not maneuver_valid else '')
|
||||
|
||||
builder.append(f"<details {_open}><summary><h3 style='display: inline-block;'>{title}</h3></summary>\n")
|
||||
|
||||
baseline_accel = lat_accel(controlsState[0].curvature, carState[0].vEgo)
|
||||
v_ego = [m.vEgo for m in carState]
|
||||
cross_markers = []
|
||||
|
||||
if description.startswith(('sine', 'jitter')):
|
||||
amplitude = max(abs(lat_accel(lp.desiredCurvature, v) - baseline_accel)
|
||||
for lp, v in zip(lateralPlan, v_ego, strict=False))
|
||||
threshold = amplitude * 0.5
|
||||
builder.append('<h3 style="font-weight: normal">50% peak')
|
||||
for t, cs, v in zip(t_controlsState, controlsState, v_ego, strict=False):
|
||||
actual = lat_accel(cs.curvature, v) - baseline_accel
|
||||
if abs(actual) > threshold:
|
||||
builder.append(f', <strong>crossed in {t:.3f}s</strong>')
|
||||
cross_markers.append((t, actual + baseline_accel))
|
||||
if maneuver_valid:
|
||||
target_cross_times[description].append(t)
|
||||
break
|
||||
else:
|
||||
builder.append(', <strong>not crossed</strong>')
|
||||
builder.append('</h3>')
|
||||
if maneuver_valid:
|
||||
target_cross_times.setdefault(description, [])
|
||||
else:
|
||||
action_targets = [(0, lat_accel(lateralPlan[0].desiredCurvature, v_ego[0]) - baseline_accel)]
|
||||
for i in range(1, min(len(lateralPlan), len(v_ego))):
|
||||
if abs(lateralPlan[i].desiredCurvature - lateralPlan[i - 1].desiredCurvature) > 0.001:
|
||||
desired = lat_accel(lateralPlan[i].desiredCurvature, v_ego[i]) - baseline_accel
|
||||
action_targets.append((i, desired))
|
||||
|
||||
for j, (start_i, act_target) in enumerate(action_targets):
|
||||
start_time = t_lateralPlan[start_i]
|
||||
end_time = t_lateralPlan[action_targets[j + 1][0]] if j + 1 < len(action_targets) else t_controlsState[-1]
|
||||
|
||||
builder.append(f'<h3 style="font-weight: normal">aTarget: {round(act_target, 1)} m/s^2')
|
||||
prev_crossed = False
|
||||
for t, cs, v in zip(t_controlsState, controlsState, v_ego, strict=False):
|
||||
if not (start_time <= t <= end_time):
|
||||
continue
|
||||
actual_accel = lat_accel(cs.curvature, v) - baseline_accel
|
||||
crossed = (0 < act_target < actual_accel) or (0 > act_target > actual_accel)
|
||||
if crossed and prev_crossed:
|
||||
cross_time = t - start_time
|
||||
builder.append(f', <strong>crossed in {cross_time:.3f}s</strong>')
|
||||
cross_markers.append((t, act_target + baseline_accel))
|
||||
if maneuver_valid:
|
||||
target_cross_times[description].append(cross_time)
|
||||
break
|
||||
prev_crossed = crossed
|
||||
else:
|
||||
builder.append(', <strong>not crossed</strong>')
|
||||
builder.append('</h3>')
|
||||
if maneuver_valid:
|
||||
target_cross_times.setdefault(description, [])
|
||||
|
||||
plt.rcParams['font.size'] = 40
|
||||
fig = plt.figure(figsize=(30, 40))
|
||||
ax = fig.subplots(5, 1, sharex=True, gridspec_kw={'height_ratios': [5, 5, 3, 3, 3]})
|
||||
|
||||
ax[0].grid(linewidth=4)
|
||||
desired_label = 'lateralManeuverPlan.desiredCurvature * vEgo^2'
|
||||
desired_lat_accel = [lat_accel(m.desiredCurvature, v) for m, v in zip(lateralPlan, v_ego, strict=False)]
|
||||
if description.startswith(('sine', 'jitter')):
|
||||
ax[0].plot(t_lateralPlan[:len(desired_lat_accel)], desired_lat_accel, 'C1', label=desired_label, linewidth=6)
|
||||
else:
|
||||
t_desired = [t_lateralPlan[0]] + t_lateralPlan[:len(desired_lat_accel)]
|
||||
desired_lat_accel = [baseline_accel] + desired_lat_accel
|
||||
ax[0].step(t_desired, desired_lat_accel, 'C1', label=desired_label, linewidth=6, where='post')
|
||||
actual_lat_accel = [lat_accel(cs.curvature, v) for cs, v in zip(controlsState, v_ego, strict=False)]
|
||||
ax[0].plot(t_controlsState[:len(actual_lat_accel)], actual_lat_accel, 'g', label='controlsState.curvature * vEgo^2', linewidth=6)
|
||||
ax[0].set_ylabel('Lateral Accel (m/s^2)')
|
||||
for ct, cv in cross_markers:
|
||||
ax[0].plot(ct, cv, marker='o', markersize=50, markeredgewidth=7, markeredgecolor='black', markerfacecolor='None')
|
||||
ax[0].legend(prop={'size': 30})
|
||||
|
||||
ax[1].grid(linewidth=4)
|
||||
if CP.steerControlType in ANGLE_CONTROL:
|
||||
steer_field, steer_ylabel = 'steeringAngleDeg', 'Steer angle (deg)'
|
||||
else:
|
||||
steer_field, steer_ylabel = 'torque', 'Steer torque'
|
||||
ax[1].plot(t_carControl, [getattr(m.actuators, steer_field) for m in carControl], 'C1', label=f'carControl.actuators.{steer_field}', linewidth=6)
|
||||
ax[1].plot(t_carOutput, [getattr(m.actuatorsOutput, steer_field) for m in carOutput], 'g', label=f'carOutput.actuatorsOutput.{steer_field}', linewidth=6)
|
||||
ax[1].set_ylabel(steer_ylabel)
|
||||
ax[1].legend(prop={'size': 30})
|
||||
|
||||
ax[2].grid(linewidth=4)
|
||||
ax[2].plot(t_carState, [v * CV.MS_TO_MPH for v in v_ego], label='carState.vEgo', linewidth=6)
|
||||
ax[2].set_ylabel('Velocity (mph)')
|
||||
ax[2].yaxis.set_major_formatter(plt.FormatStrFormatter('%.1f'))
|
||||
ax[2].legend()
|
||||
|
||||
t_accel = np.array(t_controlsState[:len(actual_lat_accel)])
|
||||
raw_jerk = np.gradient(actual_lat_accel, t_accel)
|
||||
dt_avg = np.mean(np.diff(t_accel))
|
||||
jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), dt_avg)
|
||||
filtered_jerk = [jerk_filter.update(j) for j in raw_jerk]
|
||||
ax[3].grid(linewidth=4)
|
||||
ax[3].plot(t_accel, filtered_jerk, label='d/dt(controlsState.curvature * vEgo^2)', linewidth=6)
|
||||
ax[3].set_ylabel('Jerk (m/s^3)')
|
||||
ax[3].legend()
|
||||
|
||||
ax[4].grid(linewidth=4)
|
||||
ax[4].plot(t_carControl, [math.degrees(m.orientationNED[0]) if len(m.orientationNED) == 3 else 0.0 for m in carControl],
|
||||
label='carControl.orientationNED[0]', linewidth=6)
|
||||
ax[4].set_ylabel('Roll (deg)')
|
||||
ax[4].legend()
|
||||
|
||||
ax[-1].set_xlabel("Time (s)")
|
||||
fig.tight_layout()
|
||||
|
||||
buffer = io.BytesIO()
|
||||
fig.savefig(buffer, format='webp')
|
||||
plt.close(fig)
|
||||
buffer.seek(0)
|
||||
builder.append(f"<img src='data:image/webp;base64,{base64.b64encode(buffer.getvalue()).decode()}' style='width:100%; max-width:800px;'>\n")
|
||||
builder.append("</details>\n")
|
||||
|
||||
summary = ["<h2>Summary</h2>\n"]
|
||||
cols = ['maneuver', 'crossed', 'mean', 'min', 'max']
|
||||
table = []
|
||||
for description, times in target_cross_times.items():
|
||||
l = [description, len(times)]
|
||||
if len(times):
|
||||
l.extend([round(sum(times) / len(times), 2), round(min(times), 2), round(max(times), 2)])
|
||||
table.append(l)
|
||||
summary.append(tabulate(table, headers=cols, tablefmt='html', numalign='left') + '\n')
|
||||
|
||||
sum_idx = builder.index('{ summary }')
|
||||
builder[sum_idx:sum_idx + 1] = summary
|
||||
|
||||
with open(output_fn, "w") as f:
|
||||
f.write(''.join(builder))
|
||||
|
||||
print(f"\nOpening report: {output_fn}\n")
|
||||
webbrowser.open_new_tab(str(output_fn))
|
||||
|
||||
|
||||
def open_route(route: str) -> LogReader:
|
||||
if os.path.isdir(route):
|
||||
rlogs = sorted(str(p) for p in Path(route).glob("*rlog.zst"))
|
||||
if not rlogs:
|
||||
raise SystemExit(f"no *rlog.zst files in {route}")
|
||||
print(f"loading {len(rlogs)} rlogs from {route}")
|
||||
return LogReader(rlogs, only_union_types=True)
|
||||
if os.path.exists(route) or '/' in route or '|' in route:
|
||||
return LogReader(route, only_union_types=True)
|
||||
segs = [seg for seg in os.listdir(Paths.log_root()) if route in seg]
|
||||
return LogReader([os.path.join(Paths.log_root(), seg, 'rlog.zst') for seg in segs], only_union_types=True)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
parser = argparse.ArgumentParser(description='Generate lateral maneuver report from route')
|
||||
parser.add_argument('route', type=str, help='Route name, local rlog path, or directory of rlogs')
|
||||
parser.add_argument('description', type=str, nargs='?')
|
||||
|
||||
args = parser.parse_args()
|
||||
|
||||
lr = open_route(args.route)
|
||||
|
||||
CP = lr.first('carParams')
|
||||
ID = lr.first('initData')
|
||||
platform = CP.carFingerprint
|
||||
print('processing report for', platform)
|
||||
|
||||
maneuvers: list[tuple[str, list[list]]] = []
|
||||
active_prev = False
|
||||
description_prev = None
|
||||
|
||||
for msg in lr:
|
||||
if msg.which() == 'alertDebug':
|
||||
active = 'Active' in msg.alertDebug.alertText1 or msg.alertDebug.alertText1 == 'Complete'
|
||||
if active and not active_prev:
|
||||
if msg.alertDebug.alertText2 == description_prev:
|
||||
maneuvers[-1][1].append([])
|
||||
else:
|
||||
maneuvers.append((msg.alertDebug.alertText2, [[]]))
|
||||
description_prev = maneuvers[-1][0]
|
||||
active_prev = active
|
||||
|
||||
if active_prev:
|
||||
maneuvers[-1][1][-1].append(msg)
|
||||
|
||||
report(platform, args.route, args.description, CP, ID, maneuvers)
|
||||
213
tools/lateral_maneuvers/lateral_maneuversd.py
Normal file
213
tools/lateral_maneuvers/lateral_maneuversd.py
Normal file
@@ -0,0 +1,213 @@
|
||||
#!/usr/bin/env python3
|
||||
import numpy as np
|
||||
from dataclasses import dataclass
|
||||
|
||||
from cereal import messaging, car
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.realtime import DT_MDL, Ratekeeper
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import MIN_SPEED
|
||||
from openpilot.tools.longitudinal_maneuvers.maneuversd import Action, Maneuver as _Maneuver
|
||||
|
||||
# thresholds for starting maneuvers
|
||||
MAX_SPEED_DEV = 0.7 # deviation in m/s
|
||||
MAX_CURV = 0.004 # 250 m radius
|
||||
MAX_ROLL = 0.12 # 6.8°
|
||||
TIMER = 2.0 # sec stable conditions before starting maneuver
|
||||
|
||||
|
||||
@dataclass
|
||||
class Maneuver(_Maneuver):
|
||||
_baseline_curvature: float = 0.0
|
||||
|
||||
def get_accel(self, v_ego: float, lat_active: bool, curvature: float, roll: float) -> float:
|
||||
self._run_completed = False
|
||||
# only start maneuver on straight, flat roads
|
||||
ready = abs(v_ego - self.initial_speed) < MAX_SPEED_DEV and lat_active and abs(curvature) < MAX_CURV and abs(roll) < MAX_ROLL
|
||||
self._ready_cnt = (self._ready_cnt + 1) if ready else max(self._ready_cnt - 1, 0)
|
||||
|
||||
if self._ready_cnt > (TIMER / DT_MDL):
|
||||
if not self._active:
|
||||
self._baseline_curvature = curvature
|
||||
self._active = True
|
||||
|
||||
if not self._active:
|
||||
return 0.0
|
||||
|
||||
return self._step()
|
||||
|
||||
def reset(self):
|
||||
super().reset()
|
||||
self._ready_cnt = 0
|
||||
|
||||
|
||||
def _sine_action(amplitude, period, duration):
|
||||
t = np.linspace(0, duration, int(duration / DT_MDL) + 1)
|
||||
a = amplitude * np.sin(2 * np.pi * t / period)
|
||||
return Action(a.tolist(), t.tolist())
|
||||
|
||||
|
||||
MANEUVERS = [
|
||||
Maneuver(
|
||||
"step right 20mph",
|
||||
[Action([0.5], [1.0]), Action([-0.5], [1.5])],
|
||||
repeat=2,
|
||||
initial_speed=20. * CV.MPH_TO_MS,
|
||||
),
|
||||
Maneuver(
|
||||
"step left 20mph",
|
||||
[Action([-0.5], [1.0]), Action([0.5], [1.5])],
|
||||
repeat=2,
|
||||
initial_speed=20. * CV.MPH_TO_MS,
|
||||
),
|
||||
Maneuver(
|
||||
"sine 0.5Hz 20mph",
|
||||
[_sine_action(1.0, 2.0, 2.0), Action([0.0], [0.5])],
|
||||
repeat=2,
|
||||
initial_speed=20. * CV.MPH_TO_MS,
|
||||
),
|
||||
Maneuver(
|
||||
"jitter 20mph",
|
||||
[Action([-0.5 if i % 2 == 0 else 0.5], [0.1]) for i in range(10)],
|
||||
repeat=2,
|
||||
initial_speed=20. * CV.MPH_TO_MS,
|
||||
),
|
||||
Maneuver(
|
||||
"step right 30mph",
|
||||
[Action([0.5], [1.0]), Action([-0.5], [1.5])],
|
||||
repeat=2,
|
||||
initial_speed=30. * CV.MPH_TO_MS,
|
||||
),
|
||||
Maneuver(
|
||||
"step left 30mph",
|
||||
[Action([-0.5], [1.0]), Action([0.5], [1.5])],
|
||||
repeat=2,
|
||||
initial_speed=30. * CV.MPH_TO_MS,
|
||||
),
|
||||
Maneuver(
|
||||
"sine 0.5Hz 30mph",
|
||||
[_sine_action(1.0, 2.0, 2.0), Action([0.0], [0.5])],
|
||||
repeat=2,
|
||||
initial_speed=30. * CV.MPH_TO_MS,
|
||||
),
|
||||
Maneuver(
|
||||
"jitter 30mph",
|
||||
[Action([-0.5 if i % 2 == 0 else 0.5], [0.1]) for i in range(10)],
|
||||
repeat=2,
|
||||
initial_speed=30. * CV.MPH_TO_MS,
|
||||
),
|
||||
]
|
||||
|
||||
|
||||
def main():
|
||||
params = Params()
|
||||
cloudlog.info("lateral_maneuversd is waiting for CarParams")
|
||||
messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
|
||||
|
||||
# iqpilot: subscribe only to the services we actually read and drive timing with a
|
||||
# Ratekeeper instead of polling modelV2. msgq caps each topic at NUM_READERS (15) and
|
||||
# evicts ALL subscribers when exceeded; iqpilot runs many daemons, and unlike longitudinal
|
||||
# maneuver mode (which disables plannerd), lateral mode keeps plannerd running. Subscribing
|
||||
# to selfdriveState/modelV2 here (selfdriveState is unused; modelV2 was only a poll source)
|
||||
# tips those topics past 15 → eviction storm → UI/speed render drops to a few fps.
|
||||
sm = messaging.SubMaster(['carState', 'carControl', 'controlsState'])
|
||||
pm = messaging.PubMaster(['lateralManeuverPlan', 'alertDebug'])
|
||||
rk = Ratekeeper(int(1. / DT_MDL), print_delay_threshold=None) # 20 Hz, matches DT_MDL maneuver timing
|
||||
|
||||
maneuvers = iter(MANEUVERS)
|
||||
maneuver = None
|
||||
complete_cnt = 0
|
||||
aborted_cnt = 0
|
||||
abort_reason = ''
|
||||
display_holdoff = 0
|
||||
prev_text = ''
|
||||
|
||||
while True:
|
||||
sm.update(0)
|
||||
|
||||
if maneuver is None:
|
||||
maneuver = next(maneuvers, None)
|
||||
|
||||
alert_msg = messaging.new_message('alertDebug')
|
||||
alert_msg.valid = True
|
||||
|
||||
plan_send = messaging.new_message('lateralManeuverPlan')
|
||||
|
||||
accel = 0
|
||||
v_ego = max(sm['carState'].vEgo, 0)
|
||||
curvature = sm['controlsState'].desiredCurvature
|
||||
|
||||
if complete_cnt > 0:
|
||||
complete_cnt -= 1
|
||||
alert_msg.alertDebug.alertText1 = 'Completed'
|
||||
alert_msg.alertDebug.alertText2 = maneuver.description
|
||||
elif maneuver is not None:
|
||||
# any driver input aborts the maneuver
|
||||
CS = sm['carState']
|
||||
if CS.steeringPressed or CS.gasPressed:
|
||||
aborted_cnt = int(1.0 / DT_MDL)
|
||||
abort_reason = ('steering pressed' if CS.steeringPressed else 'gas pressed').ljust(20)
|
||||
aborted = aborted_cnt > 0
|
||||
speed_out_of_range = maneuver.active and abs(v_ego - maneuver.initial_speed) > MAX_SPEED_DEV
|
||||
if aborted or speed_out_of_range:
|
||||
maneuver.reset()
|
||||
|
||||
roll = sm['carControl'].orientationNED[0] if len(sm['carControl'].orientationNED) == 3 else 0.0
|
||||
accel = maneuver.get_accel(v_ego, sm['carControl'].latActive, curvature, roll)
|
||||
|
||||
if maneuver._run_completed:
|
||||
complete_cnt = int(1.0 / DT_MDL)
|
||||
alert_msg.alertDebug.alertText1 = 'Complete'
|
||||
alert_msg.alertDebug.alertText2 = maneuver.description
|
||||
elif maneuver.active:
|
||||
action_remaining = maneuver.actions[maneuver._action_index].time_bp[-1] - maneuver._action_frames * DT_MDL
|
||||
if maneuver.description.startswith('sine'):
|
||||
freq = maneuver.description.split()[1]
|
||||
alert_msg.alertDebug.alertText1 = f'Active sine {freq} {max(action_remaining, 0):.1f}s'
|
||||
else:
|
||||
alert_msg.alertDebug.alertText1 = f'Active {accel:+.1f}m/s² {max(action_remaining, 0):.1f}s'
|
||||
alert_msg.alertDebug.alertText2 = maneuver.description
|
||||
elif aborted_cnt > 0:
|
||||
aborted_cnt -= 1
|
||||
alert_msg.alertDebug.alertText1 = abort_reason
|
||||
elif not (abs(v_ego - maneuver.initial_speed) < MAX_SPEED_DEV and sm['carControl'].latActive):
|
||||
alert_msg.alertDebug.alertText1 = f'Set speed to {maneuver.initial_speed * CV.MS_TO_MPH:0.0f} mph'
|
||||
elif maneuver._ready_cnt > 0:
|
||||
ready_time = max(TIMER - maneuver._ready_cnt * DT_MDL, 0)
|
||||
alert_msg.alertDebug.alertText1 = f'Starting: {int(ready_time) + 1}'
|
||||
alert_msg.alertDebug.alertText2 = maneuver.description
|
||||
else:
|
||||
curv_ok = abs(curvature) < MAX_CURV
|
||||
reason = 'road not straight' if not curv_ok else 'road not flat'
|
||||
alert_msg.alertDebug.alertText1 = f'Waiting: {reason}'
|
||||
alert_msg.alertDebug.alertText2 = maneuver.description
|
||||
else:
|
||||
alert_msg.alertDebug.alertText1 = 'Maneuvers Finished'
|
||||
|
||||
# prevent flickering text
|
||||
setup = ('Set speed', 'Starting', 'Waiting')
|
||||
text = alert_msg.alertDebug.alertText1
|
||||
same = text == prev_text or (text.startswith('Starting') and prev_text.startswith('Starting'))
|
||||
if not same and text.startswith(setup) and prev_text.startswith(setup) and display_holdoff > 0:
|
||||
alert_msg.alertDebug.alertText1 = prev_text
|
||||
display_holdoff -= 1
|
||||
else:
|
||||
prev_text = text
|
||||
display_holdoff = int(0.5 / DT_MDL) if text.startswith(setup) else 0
|
||||
|
||||
pm.send('alertDebug', alert_msg)
|
||||
|
||||
plan_send.valid = maneuver is not None and maneuver.active and complete_cnt == 0
|
||||
if plan_send.valid:
|
||||
plan_send.lateralManeuverPlan.desiredCurvature = maneuver._baseline_curvature + accel / max(v_ego, MIN_SPEED) ** 2
|
||||
pm.send('lateralManeuverPlan', plan_send)
|
||||
|
||||
if maneuver is not None and maneuver.finished and complete_cnt == 0:
|
||||
maneuver = None
|
||||
|
||||
rk.keep_time()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
73
tools/lateral_maneuvers/sim_maneuvers.py
Executable file
73
tools/lateral_maneuvers/sim_maneuvers.py
Executable file
@@ -0,0 +1,73 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Run lateral_maneuversd against a synthetic lateral plant and write an rlog.
|
||||
|
||||
./tools/lateral_maneuvers/sim_maneuvers.py --out /tmp/lat_rlog.zst
|
||||
./tools/lateral_maneuvers/generate_report.py /tmp/lat_rlog.zst
|
||||
"""
|
||||
import argparse
|
||||
import re
|
||||
from pathlib import Path
|
||||
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.tools.lateral_maneuvers.lateral_maneuversd import MANEUVERS
|
||||
from openpilot.tools.longitudinal_maneuvers.sim_harness import ManeuverSim, Plant
|
||||
|
||||
CURV_TAU = 0.05 # controlsd curvature command tracking
|
||||
RACK_WN = 8.0 # steering rack + tire natural frequency (rad/s)
|
||||
RACK_ZETA = 0.7 # underdamped, so achieved curvature overshoots like a real rack
|
||||
CRUISE_ACCEL = 1.2
|
||||
|
||||
SET_SPEED_RE = re.compile(r"Set speed to (\d+) mph")
|
||||
|
||||
|
||||
class LateralPlant(Plant):
|
||||
PLAN = 'lateralManeuverPlan'
|
||||
|
||||
def __init__(self):
|
||||
super().__init__(v_ego=MANEUVERS[0].initial_speed)
|
||||
self.sim = None
|
||||
self._rack_rate = 0.0
|
||||
self.target_speed = MANEUVERS[0].initial_speed
|
||||
self._by_description = {m.description: m.initial_speed for m in MANEUVERS}
|
||||
|
||||
def _update_target(self):
|
||||
if self.sim is None:
|
||||
return
|
||||
speed = self._by_description.get(self.sim.alert2)
|
||||
if speed is None:
|
||||
match = SET_SPEED_RE.search(self.sim.alert1)
|
||||
speed = float(match.group(1)) * CV.MPH_TO_MS if match else None
|
||||
if speed is not None:
|
||||
self.target_speed = speed
|
||||
|
||||
def step(self, dt, plan):
|
||||
self._update_target()
|
||||
|
||||
err = self.target_speed - self.v_ego
|
||||
self.a_ego = max(min(err / 1.0, CRUISE_ACCEL), -CRUISE_ACCEL)
|
||||
self.v_ego = max(self.v_ego + self.a_ego * dt, 0.0)
|
||||
|
||||
desired_curvature = float(plan.desiredCurvature) if plan is not None else 0.0
|
||||
self.curvature += (dt / (CURV_TAU + dt)) * (desired_curvature - self.curvature)
|
||||
|
||||
self._rack_rate += dt * (RACK_WN ** 2 * (self.curvature - self.achieved_curvature) - 2 * RACK_ZETA * RACK_WN * self._rack_rate)
|
||||
self.achieved_curvature += dt * self._rack_rate
|
||||
self.lat_accel = self.achieved_curvature * max(self.v_ego, 1.0) ** 2
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--out", type=Path, default=Path("/tmp/lateral_maneuvers_sim/rlog.zst"))
|
||||
parser.add_argument("--max-maneuvers", type=int, default=0, help="stop after N maneuvers (0 = all)")
|
||||
parser.add_argument("--timeout", type=float, default=900.0)
|
||||
args = parser.parse_args()
|
||||
|
||||
sim = ManeuverSim("openpilot.tools.lateral_maneuvers.lateral_maneuversd", LateralPlant(),
|
||||
max_maneuvers=args.max_maneuvers, timeout=args.timeout)
|
||||
out = sim.run(args.out)
|
||||
print(f"\nmaneuvers seen: {sim.seen_maneuvers}")
|
||||
print(f"rlog: {out} ({out.stat().st_size / 1e6:.1f} MB)")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
59
tools/lib/README.md
Normal file
59
tools/lib/README.md
Normal file
@@ -0,0 +1,59 @@
|
||||
## LogReader
|
||||
|
||||
Route is a class for conveniently accessing all the [logs](/system/loggerd/) from your routes. The LogReader class reads the non-video logs, i.e. rlog.bz2 and qlog.bz2. There's also a matching FrameReader class for reading the videos.
|
||||
|
||||
```python
|
||||
from openpilot.tools.lib.route import Route
|
||||
from openpilot.tools.lib.logreader import LogReader
|
||||
|
||||
r = Route("a2a0ccea32023010|2023-07-27--13-01-19")
|
||||
|
||||
# get a list of paths for the route's rlog files
|
||||
print(r.log_paths())
|
||||
|
||||
# and road camera (fcamera.hevc) files
|
||||
print(r.camera_paths())
|
||||
|
||||
# setup a LogReader to read the route's first rlog
|
||||
lr = LogReader(r.log_paths()[0])
|
||||
|
||||
# print out all the messages in the log
|
||||
import codecs
|
||||
codecs.register_error("strict", codecs.backslashreplace_errors)
|
||||
for msg in lr:
|
||||
print(msg)
|
||||
|
||||
# setup a LogReader for the route's second qlog
|
||||
lr = LogReader(r.log_paths()[1])
|
||||
|
||||
# print all the steering angles values from the log
|
||||
for msg in lr:
|
||||
if msg.which() == "carState":
|
||||
print(msg.carState.steeringAngleDeg)
|
||||
```
|
||||
|
||||
### Segment Ranges
|
||||
|
||||
We also support a new format called a "segment range":
|
||||
|
||||
```
|
||||
344c5c15b34f2d8a / 2024-01-03--09-37-12 / 2:6 / q
|
||||
[ dongle id ] [ timestamp ] [ selector ] [ query type]
|
||||
```
|
||||
|
||||
you can specify which segments from a route to load
|
||||
|
||||
```python
|
||||
lr = LogReader("a2a0ccea32023010|2023-07-27--13-01-19/4") # 4th segment
|
||||
lr = LogReader("a2a0ccea32023010|2023-07-27--13-01-19/4:6") # 4th and 5th segment
|
||||
lr = LogReader("a2a0ccea32023010|2023-07-27--13-01-19/-1") # last segment
|
||||
lr = LogReader("a2a0ccea32023010|2023-07-27--13-01-19/:5") # first 5 segments
|
||||
lr = LogReader("a2a0ccea32023010|2023-07-27--13-01-19/1:") # all except first segment
|
||||
```
|
||||
|
||||
and can select which type of logs to grab
|
||||
|
||||
```python
|
||||
lr = LogReader("a2a0ccea32023010|2023-07-27--13-01-19/4/q") # get qlogs
|
||||
lr = LogReader("a2a0ccea32023010|2023-07-27--13-01-19/4/r") # get rlogs (default)
|
||||
```
|
||||
0
tools/lib/__init__.py
Normal file
0
tools/lib/__init__.py
Normal file
40
tools/lib/api.py
Normal file
40
tools/lib/api.py
Normal file
@@ -0,0 +1,40 @@
|
||||
import os
|
||||
import requests
|
||||
from requests.adapters import HTTPAdapter, Retry
|
||||
API_HOST = os.getenv('API_HOST', 'https://api-iqlabs.konn3kt.com')
|
||||
|
||||
# TODO: this should be merged into common.api
|
||||
|
||||
class CommaApi:
|
||||
def __init__(self, token=None):
|
||||
self.session = requests.Session()
|
||||
self.session.headers['User-agent'] = 'OpenpilotTools'
|
||||
if token:
|
||||
self.session.headers['Authorization'] = 'JWT ' + token
|
||||
|
||||
retries = Retry(total=5, backoff_factor=1, status_forcelist=[500, 502, 503, 504])
|
||||
self.session.mount('https://', HTTPAdapter(max_retries=retries))
|
||||
|
||||
def request(self, method, endpoint, **kwargs):
|
||||
with self.session.request(method, API_HOST + '/' + endpoint, **kwargs) as resp:
|
||||
resp_json = resp.json()
|
||||
if isinstance(resp_json, dict) and resp_json.get('error'):
|
||||
if resp.status_code in [401, 403]:
|
||||
raise UnauthorizedError('Unauthorized. Authenticate with tools/lib/auth.py')
|
||||
|
||||
e = APIError(str(resp.status_code) + ":" + resp_json.get('description', str(resp_json['error'])))
|
||||
e.status_code = resp.status_code
|
||||
raise e
|
||||
return resp_json
|
||||
|
||||
def get(self, endpoint, **kwargs):
|
||||
return self.request('GET', endpoint, **kwargs)
|
||||
|
||||
def post(self, endpoint, **kwargs):
|
||||
return self.request('POST', endpoint, **kwargs)
|
||||
|
||||
class APIError(Exception):
|
||||
pass
|
||||
|
||||
class UnauthorizedError(Exception):
|
||||
pass
|
||||
115
tools/lib/auth.py
Executable file
115
tools/lib/auth.py
Executable file
@@ -0,0 +1,115 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Usage::
|
||||
|
||||
usage: auth.py [-h] [{github,jwt}] [jwt]
|
||||
|
||||
Login to your konn3kt account
|
||||
|
||||
positional arguments:
|
||||
{github,jwt}
|
||||
jwt
|
||||
|
||||
optional arguments:
|
||||
-h, --help show this help message and exit
|
||||
|
||||
|
||||
Examples::
|
||||
|
||||
./auth.py # Log in with GitHub
|
||||
./auth.py jwt ey..hw # Log in with a pre-issued JWT (for CI)
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import sys
|
||||
import pprint
|
||||
import webbrowser
|
||||
from http.server import BaseHTTPRequestHandler, HTTPServer
|
||||
from typing import Any
|
||||
from urllib.parse import parse_qs, urlencode
|
||||
|
||||
from openpilot.tools.lib.api import APIError, CommaApi, UnauthorizedError
|
||||
from openpilot.tools.lib.auth_config import set_token, get_token
|
||||
|
||||
PORT = 3000
|
||||
|
||||
|
||||
class ClientRedirectServer(HTTPServer):
|
||||
query_params: dict[str, Any] = {}
|
||||
|
||||
|
||||
class ClientRedirectHandler(BaseHTTPRequestHandler):
|
||||
def do_GET(self):
|
||||
if '?' in self.path:
|
||||
query_parsed = parse_qs(self.path.split('?', 1)[1], keep_blank_values=True)
|
||||
if 'code' in query_parsed or 'error' in query_parsed:
|
||||
self.server.query_params = query_parsed
|
||||
|
||||
self.send_response(200)
|
||||
self.send_header('Content-type', 'text/plain')
|
||||
self.end_headers()
|
||||
self.wfile.write(b'Return to the CLI to continue')
|
||||
|
||||
def log_message(self, fmt, *fmt_args):
|
||||
sys.stderr.write(f"[auth callback] {self.address_string()} {fmt % fmt_args}\n")
|
||||
|
||||
|
||||
def auth_redirect_link(method):
|
||||
if method != 'github':
|
||||
raise NotImplementedError(f"no redirect implemented for method {method}")
|
||||
|
||||
params = {
|
||||
'client_id': 'Ov23lifjMafxJzFatvuB',
|
||||
'redirect_uri': 'https://api-iqlabs.konn3kt.com/v2/auth/h/redirect/',
|
||||
'state': f'service,localhost:{PORT}',
|
||||
'scope': 'read:user',
|
||||
}
|
||||
return 'https://github.com/login/oauth/authorize?' + urlencode(params)
|
||||
|
||||
|
||||
def login(method):
|
||||
oauth_uri = auth_redirect_link(method)
|
||||
|
||||
web_server = ClientRedirectServer(('localhost', PORT), ClientRedirectHandler)
|
||||
print(f'To sign in, use your browser and navigate to {oauth_uri}')
|
||||
webbrowser.open(oauth_uri, new=2)
|
||||
|
||||
while True:
|
||||
web_server.handle_request()
|
||||
if 'code' in web_server.query_params:
|
||||
break
|
||||
elif 'error' in web_server.query_params:
|
||||
print('Authentication Error: "{}". Description: "{}" '.format(
|
||||
web_server.query_params['error'],
|
||||
web_server.query_params.get('error_description')), file=sys.stderr)
|
||||
break
|
||||
|
||||
try:
|
||||
auth_resp = CommaApi().post('v2/auth/', data={'code': web_server.query_params['code'], 'provider': web_server.query_params['provider']})
|
||||
set_token(auth_resp['access_token'])
|
||||
except APIError as e:
|
||||
print(f'Authentication Error: {e}', file=sys.stderr)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
parser = argparse.ArgumentParser(description='Login to your konn3kt account')
|
||||
parser.add_argument('method', default='github', const='github', nargs='?', choices=['github', 'jwt'])
|
||||
parser.add_argument('jwt', nargs='?')
|
||||
|
||||
args = parser.parse_args()
|
||||
if args.method == 'jwt':
|
||||
if args.jwt is None:
|
||||
print("method JWT selected, but no JWT was provided")
|
||||
exit(1)
|
||||
|
||||
set_token(args.jwt)
|
||||
else:
|
||||
login(args.method)
|
||||
|
||||
try:
|
||||
me = CommaApi(token=get_token()).get('/v1/me')
|
||||
print("Authenticated!")
|
||||
pprint.pprint(me)
|
||||
except UnauthorizedError:
|
||||
print("Got invalid JWT")
|
||||
exit(1)
|
||||
29
tools/lib/auth_config.py
Normal file
29
tools/lib/auth_config.py
Normal file
@@ -0,0 +1,29 @@
|
||||
import json
|
||||
import os
|
||||
from openpilot.system.hardware.hw import Paths
|
||||
|
||||
|
||||
class MissingAuthConfigError(Exception):
|
||||
pass
|
||||
|
||||
|
||||
def get_token():
|
||||
try:
|
||||
with open(os.path.join(Paths.config_root(), 'auth.json')) as f:
|
||||
auth = json.load(f)
|
||||
return auth['access_token']
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
|
||||
def set_token(token):
|
||||
os.makedirs(Paths.config_root(), exist_ok=True)
|
||||
with open(os.path.join(Paths.config_root(), 'auth.json'), 'w') as f:
|
||||
json.dump({'access_token': token}, f)
|
||||
|
||||
|
||||
def clear_token():
|
||||
try:
|
||||
os.unlink(os.path.join(Paths.config_root(), 'auth.json'))
|
||||
except FileNotFoundError:
|
||||
pass
|
||||
73
tools/lib/azure_container.py
Normal file
73
tools/lib/azure_container.py
Normal file
@@ -0,0 +1,73 @@
|
||||
import os
|
||||
from datetime import datetime, timedelta, UTC
|
||||
from functools import lru_cache
|
||||
from pathlib import Path
|
||||
from typing import IO
|
||||
|
||||
|
||||
TOKEN_PATH = Path("/data/azure_token")
|
||||
|
||||
@lru_cache
|
||||
def get_azure_credential():
|
||||
if "AZURE_TOKEN" in os.environ:
|
||||
return os.environ["AZURE_TOKEN"]
|
||||
elif TOKEN_PATH.is_file():
|
||||
return TOKEN_PATH.read_text().strip()
|
||||
else:
|
||||
from azure.identity import AzureCliCredential
|
||||
return AzureCliCredential()
|
||||
|
||||
@lru_cache
|
||||
def get_container_sas(account_name: str, container_name: str):
|
||||
from azure.storage.blob import BlobServiceClient, ContainerSasPermissions, generate_container_sas
|
||||
start_time = datetime.now(UTC).replace(tzinfo=None)
|
||||
expiry_time = start_time + timedelta(hours=1)
|
||||
blob_service = BlobServiceClient(
|
||||
account_url=f"https://{account_name}.blob.core.windows.net",
|
||||
credential=get_azure_credential(),
|
||||
)
|
||||
return generate_container_sas(
|
||||
account_name,
|
||||
container_name,
|
||||
user_delegation_key=blob_service.get_user_delegation_key(start_time, expiry_time),
|
||||
permission=ContainerSasPermissions(read=True, write=True, list=True),
|
||||
expiry=expiry_time,
|
||||
)
|
||||
|
||||
class AzureContainer:
|
||||
def __init__(self, account, container):
|
||||
self.ACCOUNT = account
|
||||
self.CONTAINER = container
|
||||
|
||||
@property
|
||||
def ACCOUNT_URL(self) -> str:
|
||||
return f"https://{self.ACCOUNT}.blob.core.windows.net"
|
||||
|
||||
@property
|
||||
def BASE_URL(self) -> str:
|
||||
return f"{self.ACCOUNT_URL}/{self.CONTAINER}/"
|
||||
|
||||
def get_client_and_key(self):
|
||||
from azure.storage.blob import ContainerClient
|
||||
client = ContainerClient(self.ACCOUNT_URL, self.CONTAINER, credential=get_azure_credential())
|
||||
key = get_container_sas(self.ACCOUNT, self.CONTAINER)
|
||||
return client, key
|
||||
|
||||
def get_url(self, route_name: str, segment_num: str, filename: str) -> str:
|
||||
return self.BASE_URL + f"{route_name.replace('|', '/')}/{segment_num}/{filename}"
|
||||
|
||||
def upload_bytes(self, data: bytes | IO, blob_name: str, overwrite=False) -> str:
|
||||
from azure.storage.blob import BlobClient
|
||||
blob = BlobClient(
|
||||
account_url=self.ACCOUNT_URL,
|
||||
container_name=self.CONTAINER,
|
||||
blob_name=blob_name,
|
||||
credential=get_azure_credential(),
|
||||
overwrite=overwrite,
|
||||
)
|
||||
blob.upload_blob(data, overwrite=overwrite)
|
||||
return self.BASE_URL + blob_name
|
||||
|
||||
def upload_file(self, path: str | os.PathLike, blob_name: str, overwrite=False) -> str:
|
||||
with open(path, "rb") as f:
|
||||
return self.upload_bytes(f, blob_name, overwrite)
|
||||
57
tools/lib/bootlog.py
Normal file
57
tools/lib/bootlog.py
Normal file
@@ -0,0 +1,57 @@
|
||||
import functools
|
||||
import re
|
||||
|
||||
from openpilot.tools.lib.auth_config import get_token
|
||||
from openpilot.tools.lib.api import CommaApi
|
||||
from openpilot.tools.lib.helpers import RE
|
||||
|
||||
|
||||
@functools.total_ordering
|
||||
class Bootlog:
|
||||
def __init__(self, url: str):
|
||||
self._url = url
|
||||
|
||||
r = re.search(RE.BOOTLOG_NAME, url)
|
||||
if not r:
|
||||
raise Exception(f"Unable to parse: {url}")
|
||||
|
||||
self._id = r.group('log_id')
|
||||
self._dongle_id = r.group('dongle_id')
|
||||
|
||||
@property
|
||||
def url(self) -> str:
|
||||
return self._url
|
||||
|
||||
@property
|
||||
def dongle_id(self) -> str:
|
||||
return self._dongle_id
|
||||
|
||||
@property
|
||||
def id(self) -> str:
|
||||
return self._id
|
||||
|
||||
def __str__(self):
|
||||
return f"{self._dongle_id}/{self._id}"
|
||||
|
||||
def __eq__(self, b) -> bool:
|
||||
if not isinstance(b, Bootlog):
|
||||
return False
|
||||
return self.id == b.id
|
||||
|
||||
def __lt__(self, b) -> bool:
|
||||
if not isinstance(b, Bootlog):
|
||||
return False
|
||||
return self.id < b.id
|
||||
|
||||
def get_bootlog_from_id(bootlog_id: str) -> Bootlog | None:
|
||||
# TODO: implement an API endpoint for this
|
||||
bl = Bootlog(bootlog_id)
|
||||
for b in get_bootlogs(bl.dongle_id):
|
||||
if b == bl:
|
||||
return b
|
||||
return None
|
||||
|
||||
def get_bootlogs(dongle_id: str) -> list[Bootlog]:
|
||||
api = CommaApi(get_token())
|
||||
r = api.get(f'v1/devices/{dongle_id}/bootlogs')
|
||||
return [Bootlog(b) for b in r]
|
||||
14
tools/lib/cache.py
Normal file
14
tools/lib/cache.py
Normal file
@@ -0,0 +1,14 @@
|
||||
import os
|
||||
import urllib.parse
|
||||
|
||||
DEFAULT_CACHE_DIR = os.getenv("CACHE_ROOT", os.path.expanduser("~/.commacache"))
|
||||
|
||||
def cache_path_for_file_path(fn, cache_dir=DEFAULT_CACHE_DIR):
|
||||
dir_ = os.path.join(cache_dir, "local")
|
||||
os.makedirs(dir_, exist_ok=True)
|
||||
fn_parsed = urllib.parse.urlparse(fn)
|
||||
if fn_parsed.scheme == '':
|
||||
cache_fn = os.path.abspath(fn).replace("/", "_")
|
||||
else:
|
||||
cache_fn = f'{fn_parsed.hostname}_{fn_parsed.path.replace("/", "_")}'
|
||||
return os.path.join(dir_, cache_fn)
|
||||
91
tools/lib/comma_car_segments.py
Normal file
91
tools/lib/comma_car_segments.py
Normal file
@@ -0,0 +1,91 @@
|
||||
import os
|
||||
import requests
|
||||
|
||||
|
||||
# Forks with additional car support can fork the commaCarSegments repo on huggingface or host the LFS files themselves
|
||||
COMMA_CAR_SEGMENTS_REPO = os.environ.get("COMMA_CAR_SEGMENTS_REPO", "https://huggingface.co/datasets/commaai/commaCarSegments")
|
||||
COMMA_CAR_SEGMENTS_BRANCH = os.environ.get("COMMA_CAR_SEGMENTS_BRANCH", "main")
|
||||
COMMA_CAR_SEGMENTS_LFS_INSTANCE = os.environ.get("COMMA_CAR_SEGMENTS_LFS_INSTANCE", COMMA_CAR_SEGMENTS_REPO)
|
||||
|
||||
def get_comma_car_segments_database():
|
||||
from iqdbc.car.fingerprints import MIGRATION
|
||||
|
||||
database = requests.get(get_repo_raw_url("database.json")).json()
|
||||
|
||||
ret = {}
|
||||
for platform in database:
|
||||
# TODO: remove this when commaCarSegments is updated to remove selector
|
||||
ret[MIGRATION.get(platform, platform)] = [s.rstrip('/s') for s in database[platform]]
|
||||
|
||||
return ret
|
||||
|
||||
|
||||
# Helpers related to interfacing with the commaCarSegments repository, which contains a collection of public segments for users to perform validation on.
|
||||
|
||||
def parse_lfs_pointer(text):
|
||||
header, lfs_version = text.splitlines()[0].split(" ")
|
||||
assert header == "version"
|
||||
assert lfs_version == "https://git-lfs.github.com/spec/v1"
|
||||
|
||||
header, oid_raw = text.splitlines()[1].split(" ")
|
||||
assert header == "oid"
|
||||
header, oid = oid_raw.split(":")
|
||||
assert header == "sha256"
|
||||
|
||||
header, size = text.splitlines()[2].split(" ")
|
||||
assert header == "size"
|
||||
|
||||
return oid, size
|
||||
|
||||
def get_lfs_file_url(oid, size):
|
||||
data = {
|
||||
"operation": "download",
|
||||
"transfers": [ "basic" ],
|
||||
"objects": [
|
||||
{
|
||||
"oid": oid,
|
||||
"size": int(size)
|
||||
}
|
||||
],
|
||||
"hash_algo": "sha256"
|
||||
}
|
||||
|
||||
headers = {
|
||||
"Accept": "application/vnd.git-lfs+json",
|
||||
"Content-Type": "application/vnd.git-lfs+json"
|
||||
}
|
||||
|
||||
response = requests.post(f"{COMMA_CAR_SEGMENTS_LFS_INSTANCE}.git/info/lfs/objects/batch", json=data, headers=headers)
|
||||
|
||||
assert response.ok
|
||||
|
||||
obj = response.json()["objects"][0]
|
||||
|
||||
assert "error" not in obj, obj
|
||||
|
||||
return obj["actions"]["download"]["href"]
|
||||
|
||||
def get_repo_raw_url(path):
|
||||
if "huggingface" in COMMA_CAR_SEGMENTS_REPO:
|
||||
return f"{COMMA_CAR_SEGMENTS_REPO}/raw/{COMMA_CAR_SEGMENTS_BRANCH}/{path}"
|
||||
|
||||
def get_repo_url(path):
|
||||
# Automatically switch to LFS if we are requesting a file that is stored in LFS
|
||||
|
||||
response = requests.head(get_repo_raw_url(path))
|
||||
|
||||
if "text/plain" in response.headers.get("content-type"):
|
||||
# This is an LFS pointer, so download the raw data from lfs
|
||||
response = requests.get(get_repo_raw_url(path))
|
||||
assert response.status_code == 200
|
||||
oid, size = parse_lfs_pointer(response.text)
|
||||
|
||||
return get_lfs_file_url(oid, size)
|
||||
else:
|
||||
# File has not been uploaded to LFS yet
|
||||
# (either we are on a fork where the data hasn't been pushed to LFS yet, or the CI job to push hasn't finished)
|
||||
return get_repo_raw_url(path)
|
||||
|
||||
|
||||
def get_url(route, segment, file="rlog.zst"):
|
||||
return get_repo_url(f"segments/{route.replace('|', '/')}/{segment}/{file}")
|
||||
2
tools/lib/exceptions.py
Normal file
2
tools/lib/exceptions.py
Normal file
@@ -0,0 +1,2 @@
|
||||
class DataUnreadableError(Exception):
|
||||
pass
|
||||
57
tools/lib/file_sources.py
Executable file
57
tools/lib/file_sources.py
Executable file
@@ -0,0 +1,57 @@
|
||||
from collections.abc import Callable
|
||||
|
||||
from openpilot.tools.lib.comma_car_segments import get_url as get_comma_segments_url
|
||||
from openpilot.tools.lib.openpilotci import get_url
|
||||
from openpilot.tools.lib.filereader import DATA_ENDPOINT, file_exists, internal_source_available
|
||||
from openpilot.tools.lib.route import Route, SegmentRange, FileName
|
||||
|
||||
# When passed a tuple of file names, each source will return the first that exists (rlog.zst, rlog.bz2)
|
||||
FileNames = tuple[str, ...]
|
||||
Source = Callable[[SegmentRange, list[int], FileNames], dict[int, str]]
|
||||
|
||||
InternalUnavailableException = Exception("Internal source not available")
|
||||
|
||||
|
||||
def comma_api_source(sr: SegmentRange, seg_idxs: list[int], fns: FileNames) -> dict[int, str]:
|
||||
route = Route(sr.route_name)
|
||||
|
||||
# comma api will have already checked if the file exists
|
||||
if fns == FileName.RLOG:
|
||||
return {seg: route.log_paths()[seg] for seg in seg_idxs if route.log_paths()[seg] is not None}
|
||||
else:
|
||||
return {seg: route.qlog_paths()[seg] for seg in seg_idxs if route.qlog_paths()[seg] is not None}
|
||||
|
||||
|
||||
def internal_source(sr: SegmentRange, seg_idxs: list[int], fns: FileNames, endpoint_url: str = DATA_ENDPOINT) -> dict[int, str]:
|
||||
if not internal_source_available(endpoint_url):
|
||||
raise InternalUnavailableException
|
||||
|
||||
def get_internal_url(sr: SegmentRange, seg, file):
|
||||
return f"{endpoint_url.rstrip('/')}/{sr.dongle_id}/{sr.log_id}/{seg}/{file}"
|
||||
|
||||
return eval_source({seg: [get_internal_url(sr, seg, fn) for fn in fns] for seg in seg_idxs})
|
||||
|
||||
|
||||
def openpilotci_source(sr: SegmentRange, seg_idxs: list[int], fns: FileNames) -> dict[int, str]:
|
||||
return eval_source({seg: [get_url(sr.route_name, seg, fn) for fn in fns] for seg in seg_idxs})
|
||||
|
||||
|
||||
def comma_car_segments_source(sr: SegmentRange, seg_idxs: list[int], fns: FileNames) -> dict[int, str]:
|
||||
return eval_source({seg: get_comma_segments_url(sr.route_name, seg) for seg in seg_idxs})
|
||||
|
||||
|
||||
def eval_source(files: dict[int, list[str] | str]) -> dict[int, str]:
|
||||
# Returns valid file URLs given a list of possible file URLs for each segment (e.g. rlog.bz2, rlog.zst)
|
||||
valid_files: dict[int, str] = {}
|
||||
|
||||
for seg_idx, urls in files.items():
|
||||
if isinstance(urls, str):
|
||||
urls = [urls]
|
||||
|
||||
# Add first valid file URL
|
||||
for url in urls:
|
||||
if file_exists(url):
|
||||
valid_files[seg_idx] = url
|
||||
break
|
||||
|
||||
return valid_files
|
||||
58
tools/lib/filereader.py
Normal file
58
tools/lib/filereader.py
Normal file
@@ -0,0 +1,58 @@
|
||||
import os
|
||||
import io
|
||||
import posixpath
|
||||
import socket
|
||||
from functools import cache
|
||||
from openpilot.common.utils import retry
|
||||
from urllib.parse import urlparse
|
||||
|
||||
from openpilot.tools.lib.url_file import URLFile
|
||||
|
||||
DATA_ENDPOINT = os.getenv("DATA_ENDPOINT", "http://data-raw.comma.internal/")
|
||||
|
||||
|
||||
@cache
|
||||
@retry(delay=0.0)
|
||||
def internal_source_available(url: str) -> bool:
|
||||
if os.path.isdir(url):
|
||||
return True
|
||||
|
||||
try:
|
||||
hostname = urlparse(url).hostname
|
||||
port = urlparse(url).port or 80
|
||||
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
|
||||
s.settimeout(0.5)
|
||||
s.connect((hostname, port))
|
||||
return True
|
||||
except (socket.gaierror, ConnectionRefusedError):
|
||||
pass
|
||||
return False
|
||||
|
||||
|
||||
def resolve_name(fn):
|
||||
if fn.startswith("cd:/"):
|
||||
return posixpath.join(DATA_ENDPOINT, fn[4:])
|
||||
return fn
|
||||
|
||||
|
||||
@cache
|
||||
def file_exists(fn):
|
||||
fn = resolve_name(fn)
|
||||
if fn.startswith(("http://", "https://")):
|
||||
return URLFile(fn).get_length_online() != -1
|
||||
return os.path.exists(fn)
|
||||
|
||||
class DiskFile(io.BufferedReader):
|
||||
def get_multi_range(self, ranges: list[tuple[int, int]]) -> list[bytes]:
|
||||
parts = []
|
||||
for r in ranges:
|
||||
self.seek(r[0])
|
||||
parts.append(self.read(r[1] - r[0]))
|
||||
return parts
|
||||
|
||||
def FileReader(fn):
|
||||
fn = resolve_name(fn)
|
||||
if fn.startswith(("http://", "https://")):
|
||||
return URLFile(fn)
|
||||
else:
|
||||
return DiskFile(open(fn, "rb"))
|
||||
173
tools/lib/framereader.py
Normal file
173
tools/lib/framereader.py
Normal file
@@ -0,0 +1,173 @@
|
||||
import os
|
||||
import subprocess
|
||||
import json
|
||||
import logging
|
||||
from collections.abc import Iterator
|
||||
from collections import OrderedDict
|
||||
|
||||
import numpy as np
|
||||
from openpilot.tools.lib.filereader import FileReader, resolve_name
|
||||
from openpilot.tools.lib.exceptions import DataUnreadableError
|
||||
from openpilot.tools.lib.vidindex import hevc_index
|
||||
|
||||
logger = logging.getLogger("tools")
|
||||
|
||||
HEVC_SLICE_B = 0
|
||||
HEVC_SLICE_P = 1
|
||||
HEVC_SLICE_I = 2
|
||||
|
||||
class LRUCache:
|
||||
def __init__(self, capacity: int):
|
||||
self._cache: OrderedDict = OrderedDict()
|
||||
self.capacity = capacity
|
||||
|
||||
def __getitem__(self, key):
|
||||
self._cache.move_to_end(key)
|
||||
return self._cache[key]
|
||||
|
||||
def __setitem__(self, key, value):
|
||||
self._cache[key] = value
|
||||
if len(self._cache) > self.capacity:
|
||||
self._cache.popitem(last=False)
|
||||
|
||||
def __contains__(self, key):
|
||||
return key in self._cache
|
||||
|
||||
def assert_hvec(fn: str) -> None:
|
||||
with FileReader(fn) as f:
|
||||
header = f.read(4)
|
||||
if len(header) == 0:
|
||||
raise DataUnreadableError(f"{fn} is empty")
|
||||
elif header == b"\x00\x00\x00\x01":
|
||||
if 'hevc' not in fn:
|
||||
raise NotImplementedError(fn)
|
||||
|
||||
def decompress_video_data(rawdat, w, h, pix_fmt="rgb24", vid_fmt='hevc', hwaccel="auto", loglevel="info") -> np.ndarray:
|
||||
threads = os.getenv("FFMPEG_THREADS", "0")
|
||||
args = ["ffmpeg", "-v", loglevel,
|
||||
"-threads", threads,
|
||||
"-hwaccel", hwaccel,
|
||||
"-c:v", "hevc",
|
||||
"-vsync", "0",
|
||||
"-f", vid_fmt,
|
||||
"-flags2", "showall",
|
||||
"-i", "-",
|
||||
"-f", "rawvideo",
|
||||
"-pix_fmt", pix_fmt,
|
||||
"-"]
|
||||
dat = subprocess.check_output(args, input=rawdat)
|
||||
|
||||
ret: np.ndarray
|
||||
if pix_fmt == "rgb24":
|
||||
ret = np.frombuffer(dat, dtype=np.uint8).reshape(-1, h, w, 3)
|
||||
elif pix_fmt in ["nv12", "yuv420p"]:
|
||||
ret = np.frombuffer(dat, dtype=np.uint8).reshape(-1, (h*w*3//2))
|
||||
else:
|
||||
raise NotImplementedError(f"Unsupported pixel format: {pix_fmt}")
|
||||
return ret
|
||||
|
||||
def ffprobe(fn, fmt=None):
|
||||
fn = resolve_name(fn)
|
||||
cmd = ["ffprobe", "-v", "quiet", "-print_format", "json", "-show_format", "-show_streams"]
|
||||
if fmt:
|
||||
cmd += ["-f", fmt]
|
||||
cmd += ["-i", "-"]
|
||||
|
||||
try:
|
||||
with FileReader(fn) as f:
|
||||
ffprobe_output = subprocess.check_output(cmd, input=f.read(4096))
|
||||
except subprocess.CalledProcessError as e:
|
||||
raise DataUnreadableError(fn) from e
|
||||
return json.loads(ffprobe_output)
|
||||
|
||||
def get_index_data(fn: str, index_data: dict|None = None):
|
||||
if index_data is None:
|
||||
index_data = get_video_index(fn)
|
||||
if index_data is None:
|
||||
raise DataUnreadableError(f"Failed to index {fn!r}")
|
||||
stream = index_data["probe"]["streams"][0]
|
||||
return index_data["index"], index_data["global_prefix"], stream["width"], stream["height"]
|
||||
|
||||
def get_video_index(fn):
|
||||
assert_hvec(fn)
|
||||
frame_types, dat_len, prefix = hevc_index(fn)
|
||||
index = np.array(frame_types + [(0xFFFFFFFF, dat_len)], dtype=np.uint32)
|
||||
probe = ffprobe(fn, "hevc")
|
||||
return {
|
||||
'index': index,
|
||||
'global_prefix': prefix,
|
||||
'probe': probe
|
||||
}
|
||||
|
||||
class FfmpegDecoder:
|
||||
def __init__(self, fn: str, index_data: dict|None = None,
|
||||
pix_fmt: str = "rgb24", hwaccel="auto", loglevel="quiet"):
|
||||
self.fn = fn
|
||||
self.index, self.prefix, self.w, self.h = get_index_data(fn, index_data)
|
||||
self.frame_count = len(self.index) - 1 # sentinel row at the end
|
||||
self.iframes = np.where(self.index[:, 0] == HEVC_SLICE_I)[0]
|
||||
self.pix_fmt = pix_fmt
|
||||
self.loglevel, self.hwaccel = loglevel, hwaccel
|
||||
|
||||
def _gop_bounds(self, frame_idx: int):
|
||||
f_b = frame_idx
|
||||
while f_b > 0 and self.index[f_b, 0] != HEVC_SLICE_I:
|
||||
f_b -= 1
|
||||
f_e = frame_idx + 1
|
||||
while f_e < self.frame_count and self.index[f_e, 0] != HEVC_SLICE_I:
|
||||
f_e += 1
|
||||
return f_b, f_e, self.index[f_b, 1], self.index[f_e, 1]
|
||||
|
||||
def _decode_gop(self, raw: bytes) -> Iterator[np.ndarray]:
|
||||
yield from decompress_video_data(raw, self.w, self.h, pix_fmt=self.pix_fmt, hwaccel=self.hwaccel, loglevel=self.loglevel)
|
||||
|
||||
def get_gop_start(self, frame_idx: int):
|
||||
return self.iframes[np.searchsorted(self.iframes, frame_idx, side="right") - 1]
|
||||
|
||||
def get_iterator(self, start_fidx: int = 0, end_fidx: int|None = None,
|
||||
frame_skip: int = 1) -> Iterator[tuple[int, np.ndarray]]:
|
||||
end_fidx = end_fidx or self.frame_count
|
||||
fidx = start_fidx
|
||||
while fidx < end_fidx:
|
||||
f_b, f_e, off_b, off_e = self._gop_bounds(fidx)
|
||||
with FileReader(self.fn) as f:
|
||||
f.seek(off_b)
|
||||
raw = self.prefix + f.read(off_e - off_b)
|
||||
# number of frames to discard inside this GOP before the wanted one
|
||||
for i, frm in enumerate(decompress_video_data(raw, self.w, self.h, self.pix_fmt, hwaccel=self.hwaccel, loglevel=self.loglevel)):
|
||||
fidx = f_b + i
|
||||
if fidx >= end_fidx:
|
||||
return
|
||||
elif fidx >= start_fidx and (fidx - start_fidx) % frame_skip == 0:
|
||||
yield fidx, frm
|
||||
fidx += 1
|
||||
|
||||
def FrameIterator(fn: str, index_data: dict|None=None, pix_fmt: str = "rgb24",
|
||||
start_fidx:int=0, end_fidx=None, frame_skip:int=1, hwaccel="auto", loglevel="quiet") -> Iterator[np.ndarray]:
|
||||
dec = FfmpegDecoder(fn, pix_fmt=pix_fmt, index_data=index_data, hwaccel=hwaccel, loglevel=loglevel)
|
||||
for _, frame in dec.get_iterator(start_fidx=start_fidx, end_fidx=end_fidx, frame_skip=frame_skip):
|
||||
yield frame
|
||||
|
||||
class FrameReader:
|
||||
def __init__(self, fn: str, index_data: dict|None = None, cache_size: int = 30,
|
||||
pix_fmt: str = "rgb24", hwaccel="auto", loglevel="quiet"):
|
||||
self.decoder = FfmpegDecoder(fn, index_data=index_data, pix_fmt=pix_fmt, hwaccel=hwaccel, loglevel=loglevel)
|
||||
self.iframes = self.decoder.iframes
|
||||
self._cache: LRUCache = LRUCache(cache_size)
|
||||
self.w, self.h, self.frame_count, = self.decoder.w, self.decoder.h, self.decoder.frame_count
|
||||
self.pix_fmt = pix_fmt
|
||||
|
||||
self.it: Iterator[tuple[int, np.ndarray]] | None = None
|
||||
self.fidx = -1
|
||||
|
||||
def get(self, fidx:int):
|
||||
if fidx in self._cache: # If frame is cached, return it
|
||||
return self._cache[fidx]
|
||||
read_start = self.decoder.get_gop_start(fidx)
|
||||
if not self.it or fidx < self.fidx or read_start != self.decoder.get_gop_start(self.fidx): # If the frame is in a different GOP, reset the iterator
|
||||
self.it = self.decoder.get_iterator(read_start)
|
||||
self.fidx = -1
|
||||
while self.fidx < fidx:
|
||||
self.fidx, frame = next(self.it)
|
||||
self._cache[self.fidx] = frame
|
||||
return self._cache[fidx]
|
||||
113
tools/lib/github_utils.py
Normal file
113
tools/lib/github_utils.py
Normal file
@@ -0,0 +1,113 @@
|
||||
import base64
|
||||
import requests
|
||||
from http import HTTPMethod
|
||||
|
||||
class GithubUtils:
|
||||
def __init__(self, api_token, data_token, owner='commaai', api_repo='openpilot', data_repo='ci-artifacts'):
|
||||
self.OWNER = owner
|
||||
self.API_REPO = api_repo
|
||||
self.DATA_REPO = data_repo
|
||||
self.API_TOKEN = api_token
|
||||
self.DATA_TOKEN = data_token
|
||||
|
||||
@property
|
||||
def API_ROUTE(self):
|
||||
return f"https://api.github.com/repos/{self.OWNER}/{self.API_REPO}"
|
||||
|
||||
@property
|
||||
def DATA_ROUTE(self):
|
||||
return f"https://api.github.com/repos/{self.OWNER}/{self.DATA_REPO}"
|
||||
|
||||
def api_call(self, path, data="", method=HTTPMethod.GET, accept="", data_call=False, raise_on_failure=True):
|
||||
token = self.DATA_TOKEN if data_call else self.API_TOKEN
|
||||
if token:
|
||||
headers = {"Authorization": f"Bearer {self.DATA_TOKEN if data_call else self.API_TOKEN}", \
|
||||
"Accept": f"application/vnd.github{accept}+json"}
|
||||
else:
|
||||
headers = {}
|
||||
path = f'{self.DATA_ROUTE if data_call else self.API_ROUTE}/{path}'
|
||||
r = requests.request(method, path, headers=headers, data=data)
|
||||
if not r.ok and raise_on_failure:
|
||||
raise Exception(f"Call to {path} failed with {r.status_code}")
|
||||
else:
|
||||
return r
|
||||
|
||||
def upload_file(self, bucket, path, file_name):
|
||||
with open(path, "rb") as f:
|
||||
encoded = base64.b64encode(f.read()).decode()
|
||||
|
||||
# check if file already exists
|
||||
sha = self.get_file_sha(bucket, file_name)
|
||||
sha = f'"sha":"{sha}",' if sha else ''
|
||||
|
||||
data = f'{{"message":"uploading {file_name}", \
|
||||
"branch":"{bucket}", \
|
||||
"committer":{{"name":"Vehicle Researcher", "email": "user@comma.ai"}}, \
|
||||
{sha} \
|
||||
"content":"{encoded}"}}'
|
||||
github_path = f"contents/{file_name}"
|
||||
self.api_call(github_path, data=data, method=HTTPMethod.PUT, data_call=True)
|
||||
|
||||
def upload_files(self, bucket, files):
|
||||
self.create_bucket(bucket)
|
||||
for file_name,path in files:
|
||||
self.upload_file(bucket, path, file_name)
|
||||
|
||||
def create_bucket(self, bucket):
|
||||
if self.get_bucket_sha(bucket):
|
||||
return
|
||||
master_sha = self.get_bucket_sha('master')
|
||||
github_path = "git/refs"
|
||||
data = f'{{"ref":"refs/heads/{bucket}", "sha":"{master_sha}"}}'
|
||||
self.api_call(github_path, data=data, method=HTTPMethod.POST, data_call=True)
|
||||
|
||||
def get_bucket_sha(self, bucket):
|
||||
github_path = f"git/refs/heads/{bucket}"
|
||||
r = self.api_call(github_path, data_call=True, raise_on_failure=False)
|
||||
return r.json()['object']['sha'] if r.ok else None
|
||||
|
||||
def get_file_url(self, bucket, file_name):
|
||||
github_path = f"contents/{file_name}?ref={bucket}"
|
||||
r = self.api_call(github_path, data_call=True)
|
||||
return r.json()['download_url']
|
||||
|
||||
def get_file_sha(self, bucket, file_name):
|
||||
github_path = f"contents/{file_name}?ref={bucket}"
|
||||
r = self.api_call(github_path, data_call=True, raise_on_failure=False)
|
||||
return r.json()['sha'] if r.ok else None
|
||||
|
||||
def get_pr_number(self, pr_branch):
|
||||
github_path = f"commits/{pr_branch}/pulls"
|
||||
r = self.api_call(github_path)
|
||||
return r.json()[0]['number']
|
||||
|
||||
def get_bucket_link(self, bucket):
|
||||
return f'https://raw.githubusercontent.com/{self.OWNER}/{self.DATA_REPO}/refs/heads/{bucket}'
|
||||
|
||||
def comment_on_pr(self, comment, pr_branch, commenter="", overwrite=False):
|
||||
pr_number = self.get_pr_number(pr_branch)
|
||||
data = f'{{"body": "{comment}"}}'
|
||||
github_path = f'issues/{pr_number}/comments'
|
||||
if overwrite:
|
||||
r = self.api_call(github_path)
|
||||
comments = [x['id'] for x in r.json() if x['user']['login'] == commenter]
|
||||
if comments:
|
||||
github_path = f'issues/comments/{comments[0]}'
|
||||
self.api_call(github_path, data=data, method=HTTPMethod.PATCH)
|
||||
return
|
||||
|
||||
self.api_call(github_path, data=data, method=HTTPMethod.POST)
|
||||
|
||||
# upload files to github and comment them on the pr
|
||||
def comment_images_on_pr(self, title, commenter, pr_branch, bucket, images):
|
||||
self.upload_files(bucket, images)
|
||||
table = [f'<details><summary>{title}</summary><table>']
|
||||
for i,f in enumerate(images):
|
||||
if not (i % 2):
|
||||
table.append('<tr>')
|
||||
table.append(f'<td><img src=\\"https://raw.githubusercontent.com/{self.OWNER}/{self.DATA_REPO}/{bucket}/{f[0]}\\"></td>')
|
||||
if (i % 2):
|
||||
table.append('</tr>')
|
||||
table.append('</table></details>')
|
||||
table = ''.join(table)
|
||||
self.comment_on_pr(table, commenter, pr_branch)
|
||||
17
tools/lib/helpers.py
Normal file
17
tools/lib/helpers.py
Normal file
@@ -0,0 +1,17 @@
|
||||
# regex patterns
|
||||
class RE:
|
||||
DONGLE_ID = r'(?P<dongle_id>[a-f0-9]{16})'
|
||||
TIMESTAMP = r'(?P<timestamp>[0-9]{4}-[0-9]{2}-[0-9]{2}--[0-9]{2}-[0-9]{2}-[0-9]{2})'
|
||||
LOG_ID_V2 = r'(?P<count>[a-f0-9]{8})--(?P<uid>[a-z0-9]{10})'
|
||||
LOG_ID = fr'(?P<log_id>(?:{TIMESTAMP}|{LOG_ID_V2}))'
|
||||
ROUTE_NAME = fr'(?P<route_name>{DONGLE_ID}[|_/]{LOG_ID})'
|
||||
SEGMENT_NAME = fr'{ROUTE_NAME}(?:--|/)(?P<segment_num>[0-9]+)'
|
||||
|
||||
INDEX = r'-?[0-9]+'
|
||||
SLICE = fr'(?P<start>{INDEX})?:?(?P<end>{INDEX})?:?(?P<step>{INDEX})?'
|
||||
SEGMENT_RANGE = fr'{ROUTE_NAME}(?:(--|/)(?P<slice>({SLICE})))?(?:/(?P<selector>([qra])))?'
|
||||
|
||||
BOOTLOG_NAME = ROUTE_NAME
|
||||
|
||||
EXPLORER_FILE = fr'^(?P<segment_name>{SEGMENT_NAME})--(?P<file_name>[a-z]+\.[a-z0-9]+)$'
|
||||
OP_SEGMENT_DIR = fr'^(?P<segment_name>{SEGMENT_NAME})$'
|
||||
81
tools/lib/kbhit.py
Executable file
81
tools/lib/kbhit.py
Executable file
@@ -0,0 +1,81 @@
|
||||
#!/usr/bin/env python3
|
||||
import sys
|
||||
import termios
|
||||
import atexit
|
||||
from select import select
|
||||
|
||||
|
||||
class KBHit:
|
||||
def __init__(self) -> None:
|
||||
''' Creates a KBHit object that you can call to do various keyboard things.
|
||||
'''
|
||||
|
||||
self.stdin_fd = sys.stdin.fileno()
|
||||
self.set_kbhit_terminal()
|
||||
|
||||
def set_kbhit_terminal(self) -> None:
|
||||
''' Save old terminal settings for closure, remove ICANON & ECHO flags.
|
||||
'''
|
||||
|
||||
# Save the terminal settings
|
||||
self.old_term = termios.tcgetattr(self.stdin_fd)
|
||||
self.new_term = self.old_term.copy()
|
||||
|
||||
# New terminal setting unbuffered
|
||||
self.new_term[3] &= ~(termios.ICANON | termios.ECHO)
|
||||
termios.tcsetattr(self.stdin_fd, termios.TCSAFLUSH, self.new_term)
|
||||
|
||||
# Support normal-terminal reset at exit
|
||||
atexit.register(self.set_normal_term)
|
||||
|
||||
def set_normal_term(self) -> None:
|
||||
''' Resets to normal terminal. On Windows this is a no-op.
|
||||
'''
|
||||
|
||||
termios.tcsetattr(self.stdin_fd, termios.TCSAFLUSH, self.old_term)
|
||||
|
||||
@staticmethod
|
||||
def getch() -> str:
|
||||
''' Returns a keyboard character after kbhit() has been called.
|
||||
Should not be called in the same program as getarrow().
|
||||
'''
|
||||
return sys.stdin.read(1)
|
||||
|
||||
@staticmethod
|
||||
def getarrow() -> int:
|
||||
''' Returns an arrow-key code after kbhit() has been called. Codes are
|
||||
0 : up
|
||||
1 : right
|
||||
2 : down
|
||||
3 : left
|
||||
Should not be called in the same program as getch().
|
||||
'''
|
||||
|
||||
c = sys.stdin.read(3)[2]
|
||||
vals = [65, 67, 66, 68]
|
||||
|
||||
return vals.index(ord(c))
|
||||
|
||||
@staticmethod
|
||||
def kbhit():
|
||||
''' Returns True if keyboard character was hit, False otherwise.
|
||||
'''
|
||||
return select([sys.stdin], [], [], 0)[0] != []
|
||||
|
||||
|
||||
# Test
|
||||
if __name__ == "__main__":
|
||||
|
||||
kb = KBHit()
|
||||
|
||||
print('Hit any key, or ESC to exit')
|
||||
|
||||
while True:
|
||||
|
||||
if kb.kbhit():
|
||||
c = kb.getch()
|
||||
if c == '\x1b': # ESC
|
||||
break
|
||||
print(c)
|
||||
|
||||
kb.set_normal_term()
|
||||
30
tools/lib/live_logreader.py
Normal file
30
tools/lib/live_logreader.py
Normal file
@@ -0,0 +1,30 @@
|
||||
import os
|
||||
from cereal import log as capnp_log, messaging
|
||||
from cereal.services import SERVICE_LIST
|
||||
|
||||
from openpilot.tools.lib.logreader import LogIterable, RawLogIterable
|
||||
|
||||
|
||||
ALL_SERVICES = list(SERVICE_LIST.keys())
|
||||
|
||||
def raw_live_logreader(services: list[str] = ALL_SERVICES, addr: str = '127.0.0.1') -> RawLogIterable:
|
||||
if addr != "127.0.0.1":
|
||||
os.environ["ZMQ"] = "1"
|
||||
messaging.reset_context()
|
||||
|
||||
poller = messaging.Poller()
|
||||
|
||||
for m in services:
|
||||
messaging.sub_sock(m, poller, addr=addr)
|
||||
|
||||
while True:
|
||||
polld = poller.poll(100)
|
||||
for sock in polld:
|
||||
msg = sock.receive()
|
||||
yield msg
|
||||
|
||||
|
||||
def live_logreader(services: list[str] = ALL_SERVICES, addr: str = '127.0.0.1') -> LogIterable:
|
||||
for m in raw_live_logreader(services, addr):
|
||||
with capnp_log.Event.from_bytes(m) as evt:
|
||||
yield evt
|
||||
84
tools/lib/log_time_series.py
Normal file
84
tools/lib/log_time_series.py
Normal file
@@ -0,0 +1,84 @@
|
||||
import numpy as np
|
||||
|
||||
|
||||
def flatten_type_dict(d, sep="/", prefix=None):
|
||||
res = {}
|
||||
if isinstance(d, dict):
|
||||
for key, val in d.items():
|
||||
if prefix is None:
|
||||
res.update(flatten_type_dict(val, prefix=key))
|
||||
else:
|
||||
res.update(flatten_type_dict(val, prefix=prefix + sep + key))
|
||||
return res
|
||||
elif isinstance(d, list):
|
||||
return {prefix: np.array(d)}
|
||||
else:
|
||||
return {prefix: d}
|
||||
|
||||
|
||||
def get_message_dict(message, typ):
|
||||
valid = message.valid
|
||||
message = message._get(typ)
|
||||
if not hasattr(message, 'to_dict') or typ in ('qcomGnss', 'ubloxGnss'):
|
||||
# TODO: support these
|
||||
#print("skipping", typ)
|
||||
return
|
||||
|
||||
msg_dict = message.to_dict(verbose=True)
|
||||
msg_dict = flatten_type_dict(msg_dict)
|
||||
msg_dict['_valid'] = valid
|
||||
return msg_dict
|
||||
|
||||
|
||||
def append_dict(path, t, d, values):
|
||||
if path not in values:
|
||||
group = {}
|
||||
group["t"] = []
|
||||
for k in d:
|
||||
group[k] = []
|
||||
values[path] = group
|
||||
else:
|
||||
group = values[path]
|
||||
|
||||
group["t"].append(t)
|
||||
for k, v in d.items():
|
||||
group[k].append(v)
|
||||
|
||||
|
||||
def potentially_ragged_array(arr, dtype=None, **kwargs):
|
||||
# TODO: is there a better way to detect inhomogeneous shapes?
|
||||
try:
|
||||
return np.array(arr, dtype=dtype, **kwargs)
|
||||
except ValueError:
|
||||
return np.array(arr, dtype=object, **kwargs)
|
||||
|
||||
def msgs_to_time_series(msgs):
|
||||
"""
|
||||
Convert an iterable of canonical capnp messages into a dictionary of time series.
|
||||
Each time series has a value with key "t" which consists of monotonically increasing timestamps
|
||||
in seconds.
|
||||
"""
|
||||
values = {}
|
||||
for msg in msgs:
|
||||
typ = msg.which()
|
||||
|
||||
tm = msg.logMonoTime / 1.0e9
|
||||
msg_dict = get_message_dict(msg, typ)
|
||||
if msg_dict is not None:
|
||||
append_dict(typ, tm, msg_dict, values)
|
||||
|
||||
# Sort values by time.
|
||||
for group in values.values():
|
||||
order = np.argsort(group["t"])
|
||||
for name, group_values in group.items():
|
||||
group[name] = potentially_ragged_array(group_values)[order]
|
||||
|
||||
return values
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
import sys
|
||||
from openpilot.tools.lib.logreader import LogReader
|
||||
m = msgs_to_time_series(LogReader(sys.argv[1]))
|
||||
print(m['driverCameraState']['t'])
|
||||
print(np.diff(m['driverCameraState']['timestampSof']))
|
||||
315
tools/lib/logreader.py
Executable file
315
tools/lib/logreader.py
Executable file
@@ -0,0 +1,315 @@
|
||||
#!/usr/bin/env python3
|
||||
import bz2
|
||||
from functools import partial
|
||||
import multiprocessing
|
||||
import capnp
|
||||
import enum
|
||||
import os
|
||||
import pathlib
|
||||
import sys
|
||||
import tqdm
|
||||
import urllib.parse
|
||||
import warnings
|
||||
import zstandard as zstd
|
||||
|
||||
from collections.abc import Iterable, Iterator
|
||||
from urllib.parse import parse_qs, urlparse
|
||||
|
||||
from cereal import log as capnp_log
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.tools.lib.filereader import FileReader
|
||||
from openpilot.tools.lib.file_sources import comma_api_source, internal_source, openpilotci_source, comma_car_segments_source, Source
|
||||
from openpilot.tools.lib.route import SegmentRange, FileName
|
||||
from openpilot.tools.lib.log_time_series import msgs_to_time_series
|
||||
|
||||
LogMessage = type[capnp._DynamicStructReader]
|
||||
LogIterable = Iterable[LogMessage]
|
||||
RawLogIterable = Iterable[bytes]
|
||||
|
||||
|
||||
def save_log(dest, log_msgs, compress=True):
|
||||
dat = b"".join(msg.as_builder().to_bytes() for msg in log_msgs)
|
||||
|
||||
if compress and dest.endswith(".bz2"):
|
||||
dat = bz2.compress(dat)
|
||||
elif compress and dest.endswith(".zst"):
|
||||
dat = zstd.compress(dat, 10)
|
||||
|
||||
with open(dest, "wb") as f:
|
||||
f.write(dat)
|
||||
|
||||
|
||||
def decompress_stream(data: bytes):
|
||||
dctx = zstd.ZstdDecompressor()
|
||||
decompressed_data = b""
|
||||
|
||||
with dctx.stream_reader(data) as reader:
|
||||
decompressed_data = reader.read()
|
||||
|
||||
return decompressed_data
|
||||
|
||||
|
||||
class CachedEventReader:
|
||||
__slots__ = ('_evt', '_enum')
|
||||
|
||||
def __init__(self, evt: capnp._DynamicStructReader, _enum: str | None = None):
|
||||
"""All capnp attribute accesses are expensive, and which() is often called multiple times"""
|
||||
self._evt = evt
|
||||
self._enum: str | None = _enum
|
||||
|
||||
# fast pickle support
|
||||
def __reduce__(self):
|
||||
return CachedEventReader._reducer, (self._evt.as_builder().to_bytes(), self._enum)
|
||||
|
||||
@staticmethod
|
||||
def _reducer(data: bytes, _enum: str | None = None):
|
||||
with capnp_log.Event.from_bytes(data) as evt:
|
||||
return CachedEventReader(evt, _enum)
|
||||
|
||||
def __repr__(self):
|
||||
return self._evt.__repr__()
|
||||
|
||||
def __str__(self):
|
||||
return self._evt.__str__()
|
||||
|
||||
def __dir__(self):
|
||||
return dir(self._evt)
|
||||
|
||||
def which(self) -> str:
|
||||
if self._enum is None:
|
||||
self._enum = self._evt.which()
|
||||
return self._enum
|
||||
|
||||
def __getattr__(self, name: str):
|
||||
if name.startswith("__") and name.endswith("__"):
|
||||
return getattr(self, name)
|
||||
return getattr(self._evt, name)
|
||||
|
||||
|
||||
class _LogFileReader:
|
||||
def __init__(self, fn, only_union_types=False, sort_by_time=False, dat=None):
|
||||
self.data_version = None
|
||||
self._only_union_types = only_union_types
|
||||
|
||||
ext = None
|
||||
if not dat:
|
||||
_, ext = os.path.splitext(urllib.parse.urlparse(fn).path)
|
||||
if ext not in ('', '.bz2', '.zst'):
|
||||
# old rlogs weren't compressed
|
||||
raise ValueError(f"unknown extension {ext}")
|
||||
|
||||
with FileReader(fn) as f:
|
||||
dat = f.read()
|
||||
|
||||
if ext == ".bz2" or dat.startswith(b'BZh9'):
|
||||
dat = bz2.decompress(dat)
|
||||
elif ext == ".zst" or dat.startswith(b'\x28\xB5\x2F\xFD'):
|
||||
# https://github.com/facebook/zstd/blob/dev/doc/zstd_compression_format.md#zstandard-frames
|
||||
dat = decompress_stream(dat)
|
||||
|
||||
ents = capnp_log.Event.read_multiple_bytes(dat)
|
||||
|
||||
self._ents = []
|
||||
try:
|
||||
for e in ents:
|
||||
self._ents.append(CachedEventReader(e))
|
||||
except capnp.KjException:
|
||||
warnings.warn("Corrupted events detected", RuntimeWarning, stacklevel=1)
|
||||
|
||||
if sort_by_time:
|
||||
self._ents.sort(key=lambda x: x.logMonoTime)
|
||||
|
||||
def __iter__(self) -> Iterator[capnp._DynamicStructReader]:
|
||||
for ent in self._ents:
|
||||
if self._only_union_types:
|
||||
try:
|
||||
ent.which()
|
||||
yield ent
|
||||
except capnp.lib.capnp.KjException:
|
||||
pass
|
||||
else:
|
||||
yield ent
|
||||
|
||||
|
||||
class ReadMode(enum.StrEnum):
|
||||
RLOG = "r" # only read rlogs
|
||||
QLOG = "q" # only read qlogs
|
||||
AUTO = "a" # default to rlogs, fallback to qlogs
|
||||
AUTO_INTERACTIVE = "i" # default to rlogs, fallback to qlogs with a prompt from the user
|
||||
|
||||
|
||||
class LogsUnavailable(Exception):
|
||||
pass
|
||||
|
||||
|
||||
def direct_source(file_or_url: str) -> list[str]:
|
||||
return [file_or_url]
|
||||
|
||||
|
||||
# TODO this should apply to camera files as well
|
||||
def auto_source(identifier: str, sources: list[Source], default_mode: ReadMode) -> list[str]:
|
||||
exceptions = {}
|
||||
|
||||
sr = SegmentRange(identifier)
|
||||
needed_seg_idxs = sr.seg_idxs
|
||||
|
||||
mode = default_mode if sr.selector is None else ReadMode(sr.selector)
|
||||
if mode == ReadMode.QLOG:
|
||||
try_fns = [FileName.QLOG]
|
||||
else:
|
||||
try_fns = [FileName.RLOG]
|
||||
|
||||
# If selector allows it, fallback to qlogs
|
||||
if mode in (ReadMode.AUTO, ReadMode.AUTO_INTERACTIVE):
|
||||
try_fns.append(FileName.QLOG)
|
||||
|
||||
# Build a dict of valid files as we evaluate each source. May contain mix of rlogs, qlogs, and None.
|
||||
# This function only returns when we've sourced all files, or throws an exception
|
||||
valid_files: dict[int, str] = {}
|
||||
for fn in try_fns:
|
||||
for source in sources:
|
||||
try:
|
||||
files = source(sr, needed_seg_idxs, fn)
|
||||
|
||||
# Build a dict of valid files
|
||||
valid_files |= files
|
||||
|
||||
# Don't check for segment files that have already been found
|
||||
needed_seg_idxs = [idx for idx in needed_seg_idxs if idx not in valid_files]
|
||||
|
||||
# We've found all files, return them
|
||||
if len(needed_seg_idxs) == 0:
|
||||
return list(valid_files.values())
|
||||
else:
|
||||
raise FileNotFoundError(f"Did not find {fn} for seg idxs {needed_seg_idxs} of {sr.route_name}")
|
||||
|
||||
except Exception as e:
|
||||
exceptions[source.__name__] = e
|
||||
|
||||
if fn == try_fns[0]:
|
||||
missing_logs = len(needed_seg_idxs)
|
||||
if mode == ReadMode.AUTO:
|
||||
cloudlog.warning(f"{missing_logs}/{len(sr.seg_idxs)} rlogs were not found, falling back to qlogs for those segments...")
|
||||
elif mode == ReadMode.AUTO_INTERACTIVE:
|
||||
if input(f"{missing_logs}/{len(sr.seg_idxs)} rlogs were not found, would you like to fallback to qlogs for those segments? (y/N) ").lower() != "y":
|
||||
break
|
||||
|
||||
missing_logs = len(needed_seg_idxs)
|
||||
raise LogsUnavailable(f"{missing_logs}/{len(sr.seg_idxs)} logs were not found, please ensure all logs " +
|
||||
"are uploaded. You can fall back to qlogs with '/a' selector at the end of the route name.\n\n" +
|
||||
"Exceptions for sources:\n - " + "\n - ".join([f"{k}: {repr(v)}" for k, v in exceptions.items()]))
|
||||
|
||||
|
||||
def parse_indirect(identifier: str) -> str:
|
||||
if "useradmin.comma.ai" in identifier:
|
||||
query = parse_qs(urlparse(identifier).query)
|
||||
identifier = query["onebox"][0]
|
||||
elif "connect.comma.ai" in identifier or "konn3kt.com" in identifier:
|
||||
path = urlparse(identifier).path.strip("/").split("/")
|
||||
if path and path[0] == "connectdata":
|
||||
# signed data URL from the API host (api-*.konn3kt.com/connectdata/...), not a share link
|
||||
return identifier
|
||||
path = ['/'.join(path[:2]), *path[2:]] # recombine log id
|
||||
|
||||
identifier = path[0]
|
||||
if len(path) > 2:
|
||||
# convert url with seconds to segments
|
||||
start, end = int(path[1]) // 60, int(path[2]) // 60 + 1
|
||||
identifier = f"{identifier}/{start}:{end}"
|
||||
|
||||
# add selector if it exists
|
||||
if len(path) > 3:
|
||||
identifier += f"/{path[3]}"
|
||||
else:
|
||||
# add selector if it exists
|
||||
identifier = "/".join(path)
|
||||
|
||||
return identifier
|
||||
|
||||
|
||||
def parse_direct(identifier: str):
|
||||
if identifier.startswith(("http://", "https://", "cd:/")) or pathlib.Path(identifier).exists():
|
||||
return identifier
|
||||
return None
|
||||
|
||||
|
||||
class LogReader:
|
||||
def _parse_identifier(self, identifier: str) -> list[str]:
|
||||
# useradmin, etc.
|
||||
identifier = parse_indirect(identifier)
|
||||
|
||||
# direct url or file
|
||||
direct_parsed = parse_direct(identifier)
|
||||
if direct_parsed is not None:
|
||||
return direct_source(identifier)
|
||||
|
||||
identifiers = auto_source(identifier, self.sources, self.default_mode)
|
||||
return identifiers
|
||||
|
||||
def __init__(self, identifier: str | list[str], default_mode: ReadMode = ReadMode.RLOG,
|
||||
sources: list[Source] | None = None, sort_by_time=False, only_union_types=False):
|
||||
if sources is None:
|
||||
sources = [internal_source, comma_api_source, openpilotci_source, comma_car_segments_source]
|
||||
|
||||
self.default_mode = default_mode
|
||||
self.sources = sources
|
||||
self.identifier = identifier
|
||||
if isinstance(identifier, str):
|
||||
self.identifier = [identifier]
|
||||
|
||||
self.sort_by_time = sort_by_time
|
||||
self.only_union_types = only_union_types
|
||||
|
||||
self.__lrs: dict[int, _LogFileReader] = {}
|
||||
self.reset()
|
||||
|
||||
def _get_lr(self, i):
|
||||
if i not in self.__lrs:
|
||||
self.__lrs[i] = _LogFileReader(self.logreader_identifiers[i], sort_by_time=self.sort_by_time, only_union_types=self.only_union_types)
|
||||
return self.__lrs[i]
|
||||
|
||||
def __iter__(self):
|
||||
for i in range(len(self.logreader_identifiers)):
|
||||
yield from self._get_lr(i)
|
||||
|
||||
def _run_on_segment(self, func, i):
|
||||
return func(self._get_lr(i))
|
||||
|
||||
def run_across_segments(self, num_processes, func, disable_tqdm=False, desc=None):
|
||||
with multiprocessing.Pool(num_processes) as pool:
|
||||
ret = []
|
||||
num_segs = len(self.logreader_identifiers)
|
||||
for p in tqdm.tqdm(pool.imap(partial(self._run_on_segment, func), range(num_segs)), total=num_segs, disable=disable_tqdm, desc=desc):
|
||||
ret.extend(p)
|
||||
return ret
|
||||
|
||||
def reset(self):
|
||||
self.logreader_identifiers = []
|
||||
for identifier in self.identifier:
|
||||
self.logreader_identifiers.extend(self._parse_identifier(identifier))
|
||||
|
||||
@staticmethod
|
||||
def from_bytes(dat):
|
||||
return _LogFileReader("", dat=dat)
|
||||
|
||||
def filter(self, msg_type: str):
|
||||
return (getattr(m, m.which()) for m in filter(lambda m: m.which() == msg_type, self))
|
||||
|
||||
def first(self, msg_type: str):
|
||||
return next(self.filter(msg_type), None)
|
||||
|
||||
@property
|
||||
def time_series(self):
|
||||
return msgs_to_time_series(self)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
import codecs
|
||||
|
||||
# capnproto <= 0.8.0 throws errors converting byte data to string
|
||||
# below line catches those errors and replaces the bytes with \x__
|
||||
codecs.register_error("strict", codecs.backslashreplace_errors)
|
||||
log_path = sys.argv[1]
|
||||
lr = LogReader(log_path, sort_by_time=True)
|
||||
for msg in lr:
|
||||
print(msg)
|
||||
12
tools/lib/openpilotci.py
Normal file
12
tools/lib/openpilotci.py
Normal file
@@ -0,0 +1,12 @@
|
||||
from openpilot.tools.lib.openpilotcontainers import OpenpilotCIContainer
|
||||
|
||||
def get_url(*args, **kwargs):
|
||||
return OpenpilotCIContainer.get_url(*args, **kwargs)
|
||||
|
||||
def upload_file(*args, **kwargs):
|
||||
return OpenpilotCIContainer.upload_file(*args, **kwargs)
|
||||
|
||||
def upload_bytes(*args, **kwargs):
|
||||
return OpenpilotCIContainer.upload_bytes(*args, **kwargs)
|
||||
|
||||
BASE_URL = OpenpilotCIContainer.BASE_URL
|
||||
6
tools/lib/openpilotcontainers.py
Executable file
6
tools/lib/openpilotcontainers.py
Executable file
@@ -0,0 +1,6 @@
|
||||
#!/usr/bin/env python3
|
||||
from openpilot.tools.lib.azure_container import AzureContainer
|
||||
|
||||
OpenpilotCIContainer = AzureContainer("commadataci", "openpilotci")
|
||||
DataCIContainer = AzureContainer("commadataci", "commadataci")
|
||||
DataProdContainer = AzureContainer("commadata2", "commadata2")
|
||||
369
tools/lib/route.py
Normal file
369
tools/lib/route.py
Normal file
@@ -0,0 +1,369 @@
|
||||
import os
|
||||
import re
|
||||
import requests
|
||||
from functools import cache
|
||||
from urllib.parse import urlparse
|
||||
from collections import defaultdict
|
||||
from itertools import chain
|
||||
|
||||
from openpilot.tools.lib.auth_config import get_token
|
||||
from openpilot.tools.lib.api import APIError, CommaApi
|
||||
from openpilot.tools.lib.helpers import RE
|
||||
|
||||
|
||||
class FileName:
|
||||
RLOG = ("rlog.zst", "rlog.bz2")
|
||||
QLOG = ("qlog.zst", "qlog.bz2")
|
||||
QCAMERA = ('qcamera.ts',)
|
||||
FCAMERA = ('fcamera.hevc',)
|
||||
ECAMERA = ('ecamera.hevc',)
|
||||
DCAMERA = ('dcamera.hevc',)
|
||||
BOOTLOG = ('bootlog.zst', 'bootlog.bz2')
|
||||
|
||||
|
||||
class Route:
|
||||
def __init__(self, name, data_dir=None):
|
||||
self._name = RouteName(name)
|
||||
self.files = None
|
||||
if data_dir is not None:
|
||||
self._segments = self._get_segments_local(data_dir)
|
||||
else:
|
||||
self._segments = self._get_segments_remote()
|
||||
self.max_seg_number = self._segments[-1].name.segment_num
|
||||
|
||||
@property
|
||||
def name(self):
|
||||
return self._name
|
||||
|
||||
@property
|
||||
def segments(self):
|
||||
return self._segments
|
||||
|
||||
def log_paths(self):
|
||||
log_path_by_seg_num = {s.name.segment_num: s.log_path for s in self._segments}
|
||||
return [log_path_by_seg_num.get(i, None) for i in range(self.max_seg_number + 1)]
|
||||
|
||||
def qlog_paths(self):
|
||||
qlog_path_by_seg_num = {s.name.segment_num: s.qlog_path for s in self._segments}
|
||||
return [qlog_path_by_seg_num.get(i, None) for i in range(self.max_seg_number + 1)]
|
||||
|
||||
def camera_paths(self):
|
||||
camera_path_by_seg_num = {s.name.segment_num: s.camera_path for s in self._segments}
|
||||
return [camera_path_by_seg_num.get(i, None) for i in range(self.max_seg_number + 1)]
|
||||
|
||||
def dcamera_paths(self):
|
||||
dcamera_path_by_seg_num = {s.name.segment_num: s.dcamera_path for s in self._segments}
|
||||
return [dcamera_path_by_seg_num.get(i, None) for i in range(self.max_seg_number + 1)]
|
||||
|
||||
def ecamera_paths(self):
|
||||
ecamera_path_by_seg_num = {s.name.segment_num: s.ecamera_path for s in self._segments}
|
||||
return [ecamera_path_by_seg_num.get(i, None) for i in range(self.max_seg_number + 1)]
|
||||
|
||||
def qcamera_paths(self):
|
||||
qcamera_path_by_seg_num = {s.name.segment_num: s.qcamera_path for s in self._segments}
|
||||
return [qcamera_path_by_seg_num.get(i, None) for i in range(self.max_seg_number + 1)]
|
||||
|
||||
# TODO: refactor this, it's super repetitive
|
||||
def _get_segments_remote(self):
|
||||
api = CommaApi(get_token())
|
||||
route_files = api.get('v1/route/' + self.name.canonical_name + '/files')
|
||||
self.files = [f['url'] if isinstance(f, dict) else f
|
||||
for f in chain.from_iterable(route_files.values())]
|
||||
|
||||
segments = {}
|
||||
for url in self.files:
|
||||
_, dongle_id, time_str, segment_num, fn = urlparse(url).path.rsplit('/', maxsplit=4)
|
||||
segment_name = f'{dongle_id}|{time_str}--{segment_num}'
|
||||
if segments.get(segment_name):
|
||||
segments[segment_name] = Segment(
|
||||
segment_name,
|
||||
url if fn in FileName.RLOG else segments[segment_name].log_path,
|
||||
url if fn in FileName.QLOG else segments[segment_name].qlog_path,
|
||||
url if fn in FileName.FCAMERA else segments[segment_name].camera_path,
|
||||
url if fn in FileName.DCAMERA else segments[segment_name].dcamera_path,
|
||||
url if fn in FileName.ECAMERA else segments[segment_name].ecamera_path,
|
||||
url if fn in FileName.QCAMERA else segments[segment_name].qcamera_path,
|
||||
)
|
||||
else:
|
||||
segments[segment_name] = Segment(
|
||||
segment_name,
|
||||
url if fn in FileName.RLOG else None,
|
||||
url if fn in FileName.QLOG else None,
|
||||
url if fn in FileName.FCAMERA else None,
|
||||
url if fn in FileName.DCAMERA else None,
|
||||
url if fn in FileName.ECAMERA else None,
|
||||
url if fn in FileName.QCAMERA else None,
|
||||
)
|
||||
|
||||
return sorted(segments.values(), key=lambda seg: seg.name.segment_num)
|
||||
|
||||
def _get_segments_local(self, data_dir):
|
||||
files = os.listdir(data_dir)
|
||||
segment_files = defaultdict(list)
|
||||
|
||||
for f in files:
|
||||
fullpath = os.path.join(data_dir, f)
|
||||
explorer_match = re.match(RE.EXPLORER_FILE, f)
|
||||
op_match = re.match(RE.OP_SEGMENT_DIR, f)
|
||||
|
||||
if explorer_match:
|
||||
segment_name = explorer_match.group('segment_name')
|
||||
fn = explorer_match.group('file_name')
|
||||
if segment_name.replace('_', '|').startswith(self.name.canonical_name):
|
||||
segment_files[segment_name].append((fullpath, fn))
|
||||
elif op_match and os.path.isdir(fullpath):
|
||||
segment_name = op_match.group('segment_name')
|
||||
if segment_name.startswith(self.name.canonical_name):
|
||||
for seg_f in os.listdir(fullpath):
|
||||
segment_files[segment_name].append((os.path.join(fullpath, seg_f), seg_f))
|
||||
elif f == self.name.canonical_name:
|
||||
for seg_num in os.listdir(fullpath):
|
||||
if not seg_num.isdigit():
|
||||
continue
|
||||
|
||||
segment_name = f'{self.name.canonical_name}--{seg_num}'
|
||||
for seg_f in os.listdir(os.path.join(fullpath, seg_num)):
|
||||
segment_files[segment_name].append((os.path.join(fullpath, seg_num, seg_f), seg_f))
|
||||
|
||||
segments = []
|
||||
for segment, files in segment_files.items():
|
||||
|
||||
try:
|
||||
log_path = next(path for path, filename in files if filename in FileName.RLOG)
|
||||
except StopIteration:
|
||||
log_path = None
|
||||
|
||||
try:
|
||||
qlog_path = next(path for path, filename in files if filename in FileName.QLOG)
|
||||
except StopIteration:
|
||||
qlog_path = None
|
||||
|
||||
try:
|
||||
camera_path = next(path for path, filename in files if filename in FileName.FCAMERA)
|
||||
except StopIteration:
|
||||
camera_path = None
|
||||
|
||||
try:
|
||||
dcamera_path = next(path for path, filename in files if filename in FileName.DCAMERA)
|
||||
except StopIteration:
|
||||
dcamera_path = None
|
||||
|
||||
try:
|
||||
ecamera_path = next(path for path, filename in files if filename in FileName.ECAMERA)
|
||||
except StopIteration:
|
||||
ecamera_path = None
|
||||
|
||||
try:
|
||||
qcamera_path = next(path for path, filename in files if filename in FileName.QCAMERA)
|
||||
except StopIteration:
|
||||
qcamera_path = None
|
||||
|
||||
segments.append(Segment(segment, log_path, qlog_path, camera_path, dcamera_path, ecamera_path, qcamera_path))
|
||||
|
||||
if len(segments) == 0:
|
||||
raise ValueError(f'Could not find segments for route {self.name.canonical_name} in data directory {data_dir}')
|
||||
return sorted(segments, key=lambda seg: seg.name.segment_num)
|
||||
|
||||
|
||||
class Segment:
|
||||
def __init__(self, name, log_path, qlog_path, camera_path, dcamera_path, ecamera_path, qcamera_path):
|
||||
self._events = None
|
||||
self._name = SegmentName(name)
|
||||
self.log_path = log_path
|
||||
self.qlog_path = qlog_path
|
||||
self.camera_path = camera_path
|
||||
self.dcamera_path = dcamera_path
|
||||
self.ecamera_path = ecamera_path
|
||||
self.qcamera_path = qcamera_path
|
||||
|
||||
@property
|
||||
def name(self):
|
||||
return self._name
|
||||
|
||||
@staticmethod
|
||||
@cache
|
||||
def _get_route_metadata(route_name: str):
|
||||
api = CommaApi(get_token())
|
||||
return api.get(f'v1/route/{route_name}')
|
||||
|
||||
@property
|
||||
def url(self):
|
||||
route_name = self._name.route_name.canonical_name
|
||||
metadata = self._get_route_metadata(route_name)
|
||||
return f'{metadata["url"]}/{self._name.segment_num}'
|
||||
|
||||
@property
|
||||
def events(self):
|
||||
if not self._events:
|
||||
try:
|
||||
resp = requests.get(f'{self.url}/events.json')
|
||||
resp.raise_for_status()
|
||||
self._events = resp.json()
|
||||
except Exception as e:
|
||||
raise APIError(f'error getting events for segment {self._name}') from e
|
||||
return self._events
|
||||
|
||||
|
||||
class RouteName:
|
||||
def __init__(self, name_str: str):
|
||||
self._name_str = name_str
|
||||
delim = next(c for c in self._name_str if c in ("|", "/"))
|
||||
self._dongle_id, self._time_str = self._name_str.split(delim)
|
||||
|
||||
assert len(self._dongle_id) == 16, self._name_str
|
||||
assert len(self._time_str) == 20, self._name_str
|
||||
self._canonical_name = f"{self._dongle_id}|{self._time_str}"
|
||||
|
||||
@property
|
||||
def canonical_name(self) -> str: return self._canonical_name
|
||||
|
||||
@property
|
||||
def dongle_id(self) -> str: return self._dongle_id
|
||||
|
||||
@property
|
||||
def log_id(self) -> str: return self._time_str
|
||||
|
||||
@property
|
||||
def time_str(self) -> str: return self._time_str
|
||||
|
||||
@property
|
||||
def azure_prefix(self):
|
||||
return f'{self.dongle_id}/{self.log_id}'
|
||||
|
||||
def __str__(self) -> str: return self._canonical_name
|
||||
|
||||
|
||||
class SegmentName:
|
||||
# TODO: add constructor that takes dongle_id, time_str, segment_num and then create instances
|
||||
# of this class instead of manually constructing a segment name (use canonical_name prop instead)
|
||||
def __init__(self, name_str: str, allow_route_name=False):
|
||||
data_dir_path_separator_index = name_str.rsplit("|", 1)[0].rfind("/")
|
||||
use_data_dir = (data_dir_path_separator_index != -1) and ("|" in name_str)
|
||||
self._name_str = name_str[data_dir_path_separator_index + 1:] if use_data_dir else name_str
|
||||
self._data_dir = name_str[:data_dir_path_separator_index] if use_data_dir else None
|
||||
|
||||
seg_num_delim = "--" if self._name_str.count("--") == 2 else "/"
|
||||
name_parts = self._name_str.rsplit(seg_num_delim, 1)
|
||||
if allow_route_name and len(name_parts) == 1:
|
||||
name_parts.append("-1") # no segment number
|
||||
self._route_name = RouteName(name_parts[0])
|
||||
self._num = int(name_parts[1])
|
||||
self._canonical_name = f"{self._route_name._dongle_id}|{self._route_name._time_str}--{self._num}"
|
||||
|
||||
@property
|
||||
def canonical_name(self) -> str: return self._canonical_name
|
||||
|
||||
# TODO should only use one name
|
||||
@property
|
||||
def data_name(self) -> str: return f"{self._route_name.canonical_name}/{self._num}"
|
||||
|
||||
@property
|
||||
def azure_prefix(self):
|
||||
return f'{self.dongle_id}/{self.log_id}/{self._num}'
|
||||
|
||||
@property
|
||||
def dongle_id(self) -> str: return self._route_name.dongle_id
|
||||
|
||||
@property
|
||||
def time_str(self) -> str: return self._route_name.time_str
|
||||
|
||||
@property
|
||||
def log_id(self) -> str: return self._route_name.time_str
|
||||
|
||||
@property
|
||||
def segment_num(self) -> int: return self._num
|
||||
|
||||
@property
|
||||
def route_name(self) -> RouteName: return self._route_name
|
||||
|
||||
@property
|
||||
def data_dir(self) -> str | None: return self._data_dir
|
||||
|
||||
def __str__(self) -> str: return self._canonical_name
|
||||
|
||||
@staticmethod
|
||||
def from_file_name(file_name):
|
||||
# ??????/xxxxxxxxxxxxxxxx|1111-11-11-11--11-11-11/1/rlog.bz2
|
||||
dongle_id, route_name, segment_num = file_name.replace('|', '/').split('/')[-4:-1]
|
||||
return SegmentName(dongle_id + "|" + route_name + "--" + segment_num)
|
||||
|
||||
@staticmethod
|
||||
def from_device_key(dongle_id, key):
|
||||
# 2018-05-07--18-56-13--5/rlog.bz2
|
||||
segment_name = key.split('/')[0]
|
||||
return SegmentName(dongle_id + "|" + segment_name)
|
||||
|
||||
@staticmethod
|
||||
def from_file_key(key):
|
||||
# 38c52c217150700f/2018-05-07--18-56-13/5/rlog.bz2
|
||||
az_prefix = '/'.join(key.split('/')[:3])
|
||||
return SegmentName.from_azure_prefix(az_prefix)
|
||||
|
||||
@staticmethod
|
||||
def from_azure_prefix(prefix):
|
||||
# xxxxxxxx/1111-11-11-11--11-11-11/0
|
||||
dongle_id, route_name, segment_num = prefix.split("/")
|
||||
return SegmentName(dongle_id + "|" + route_name + "--" + segment_num)
|
||||
|
||||
|
||||
@cache
|
||||
def get_max_seg_number_cached(sr: 'SegmentRange') -> int:
|
||||
try:
|
||||
api = CommaApi(get_token())
|
||||
max_seg_number = api.get("/v1/route/" + sr.route_name.replace("/", "|"))["maxqlog"]
|
||||
assert isinstance(max_seg_number, int)
|
||||
return max_seg_number
|
||||
except Exception as e:
|
||||
raise Exception("unable to get max_segment_number. ensure you have access to this route or the route is public.") from e
|
||||
|
||||
|
||||
class SegmentRange:
|
||||
def __init__(self, segment_range: str):
|
||||
m = re.fullmatch(RE.SEGMENT_RANGE, segment_range)
|
||||
assert m is not None, f"Segment range is not valid {segment_range}"
|
||||
self.m = m
|
||||
|
||||
@property
|
||||
def route_name(self) -> str:
|
||||
return self.m.group("route_name")
|
||||
|
||||
@property
|
||||
def dongle_id(self) -> str:
|
||||
return self.m.group("dongle_id")
|
||||
|
||||
@property
|
||||
def log_id(self) -> str:
|
||||
return self.m.group("log_id")
|
||||
|
||||
@property
|
||||
def slice(self) -> str:
|
||||
return self.m.group("slice") or ""
|
||||
|
||||
@property
|
||||
def selector(self) -> str | None:
|
||||
return self.m.group("selector")
|
||||
|
||||
@property
|
||||
def seg_idxs(self) -> list[int]:
|
||||
m = re.fullmatch(RE.SLICE, self.slice)
|
||||
assert m is not None, f"Invalid slice: {self.slice}"
|
||||
start, end, step = (None if s is None else int(s) for s in m.groups())
|
||||
|
||||
# one segment specified
|
||||
if start is not None and end is None and ':' not in self.slice:
|
||||
if start < 0:
|
||||
start += get_max_seg_number_cached(self) + 1
|
||||
return [start]
|
||||
|
||||
s = slice(start, end, step)
|
||||
# no specified end or using relative indexing, need number of segments
|
||||
if end is None or end < 0 or (start is not None and start < 0):
|
||||
return list(range(get_max_seg_number_cached(self) + 1))[s]
|
||||
else:
|
||||
return list(range(end + 1))[s]
|
||||
|
||||
def __str__(self) -> str:
|
||||
return f"{self.dongle_id}/{self.log_id}" + (f"/{self.slice}" if self.slice else "") + (f"/{self.selector}" if self.selector else "")
|
||||
|
||||
def __repr__(self) -> str:
|
||||
return self.__str__()
|
||||
26
tools/lib/sanitizer.py
Normal file
26
tools/lib/sanitizer.py
Normal file
@@ -0,0 +1,26 @@
|
||||
# Utilities for sanitizing routes of only essential data for testing car ports and doing validation.
|
||||
|
||||
from openpilot.tools.lib.logreader import LogIterable, LogMessage
|
||||
|
||||
|
||||
def sanitize_vin(vin: str):
|
||||
# (last 6 digits of vin are serial number https://en.wikipedia.org/wiki/Vehicle_identification_number)
|
||||
VIN_SENSITIVE = 6
|
||||
return vin[:-VIN_SENSITIVE] + "X" * VIN_SENSITIVE
|
||||
|
||||
|
||||
def sanitize_msg(msg: LogMessage) -> LogMessage:
|
||||
if msg.which() == "carParams":
|
||||
msg = msg.as_builder()
|
||||
msg.carParams.carVin = sanitize_vin(msg.carParams.carVin)
|
||||
msg = msg.as_reader()
|
||||
return msg
|
||||
|
||||
|
||||
PRESERVE_SERVICES = ["can", "carParams", "pandaStates", "pandaStateDEPRECATED"]
|
||||
|
||||
|
||||
def sanitize(lr: LogIterable) -> LogIterable:
|
||||
filtered = filter(lambda msg: msg.which() in PRESERVE_SERVICES, lr)
|
||||
sanitized = map(sanitize_msg, filtered)
|
||||
return sanitized
|
||||
0
tools/lib/tests/__init__.py
Normal file
0
tools/lib/tests/__init__.py
Normal file
167
tools/lib/tests/test_caching.py
Normal file
167
tools/lib/tests/test_caching.py
Normal file
@@ -0,0 +1,167 @@
|
||||
import http.server
|
||||
import os
|
||||
import shutil
|
||||
import socket
|
||||
import tempfile
|
||||
import pytest
|
||||
|
||||
from openpilot.selfdrive.test.helpers import http_server_context
|
||||
from openpilot.system.hardware.hw import Paths
|
||||
from openpilot.tools.lib.url_file import URLFile, prune_cache
|
||||
import openpilot.tools.lib.url_file as url_file_module
|
||||
|
||||
|
||||
class CachingTestRequestHandler(http.server.BaseHTTPRequestHandler):
|
||||
FILE_EXISTS = True
|
||||
|
||||
def do_GET(self):
|
||||
if self.FILE_EXISTS:
|
||||
self.send_response(206 if "Range" in self.headers else 200, b'1234')
|
||||
else:
|
||||
self.send_response(404)
|
||||
self.end_headers()
|
||||
|
||||
def do_HEAD(self):
|
||||
if self.FILE_EXISTS:
|
||||
self.send_response(200)
|
||||
self.send_header("Content-Length", "4")
|
||||
else:
|
||||
self.send_response(404)
|
||||
self.end_headers()
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def host():
|
||||
with http_server_context(handler=CachingTestRequestHandler) as (host, port):
|
||||
yield f"http://{host}:{port}"
|
||||
|
||||
class TestFileDownload:
|
||||
|
||||
def test_pipeline_defaults(self, host):
|
||||
# TODO: parameterize the defaults so we don't rely on hard-coded values in xx
|
||||
|
||||
assert URLFile.pool_manager().pools._maxsize == 10# PoolManager num_pools param
|
||||
pool_manager_defaults = {
|
||||
"maxsize": 100,
|
||||
"socket_options": [(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1),],
|
||||
}
|
||||
for k, v in pool_manager_defaults.items():
|
||||
assert URLFile.pool_manager().connection_pool_kw.get(k) == v
|
||||
|
||||
retry_defaults = {
|
||||
"total": 5,
|
||||
"backoff_factor": 0.5,
|
||||
"status_forcelist": [409, 429, 503, 504],
|
||||
}
|
||||
for k, v in retry_defaults.items():
|
||||
assert getattr(URLFile.pool_manager().connection_pool_kw["retries"], k) == v
|
||||
|
||||
# ensure caching on by default and cache dir gets created
|
||||
os.environ.pop("DISABLE_FILEREADER_CACHE", None)
|
||||
if os.path.exists(Paths.download_cache_root()):
|
||||
shutil.rmtree(Paths.download_cache_root())
|
||||
URLFile(f"{host}/test.txt").get_length()
|
||||
URLFile(f"{host}/test.txt").read()
|
||||
assert os.path.exists(Paths.download_cache_root())
|
||||
|
||||
def compare_loads(self, url, start=0, length=None):
|
||||
"""Compares range between cached and non cached version"""
|
||||
file_cached = URLFile(url, cache=True)
|
||||
file_downloaded = URLFile(url, cache=False)
|
||||
|
||||
file_cached.seek(start)
|
||||
file_downloaded.seek(start)
|
||||
|
||||
assert file_cached.get_length() == file_downloaded.get_length()
|
||||
assert length + start if length is not None else 0 <= file_downloaded.get_length()
|
||||
|
||||
response_cached = file_cached.read(ll=length)
|
||||
response_downloaded = file_downloaded.read(ll=length)
|
||||
|
||||
assert response_cached == response_downloaded
|
||||
|
||||
# Now test with cache in place
|
||||
file_cached = URLFile(url, cache=True)
|
||||
file_cached.seek(start)
|
||||
response_cached = file_cached.read(ll=length)
|
||||
|
||||
assert file_cached.get_length() == file_downloaded.get_length()
|
||||
assert response_cached == response_downloaded
|
||||
|
||||
def test_small_file(self):
|
||||
# Make sure we don't force cache
|
||||
os.environ.pop("DISABLE_FILEREADER_CACHE", None)
|
||||
small_file_url = "https://raw.githubusercontent.com/commaai/openpilot/master/docs/SAFETY.md"
|
||||
# If you want large file to be larger than a chunk
|
||||
# large_file_url = "https://commadataci.blob.core.windows.net/openpilotci/0375fdf7b1ce594d/2019-06-13--08-32-25/3/fcamera.hevc"
|
||||
|
||||
# Load full small file
|
||||
self.compare_loads(small_file_url)
|
||||
|
||||
file_small = URLFile(small_file_url)
|
||||
length = file_small.get_length()
|
||||
|
||||
self.compare_loads(small_file_url, length - 100, 100)
|
||||
self.compare_loads(small_file_url, 50, 100)
|
||||
|
||||
# Load small file 100 bytes at a time
|
||||
for i in range(length // 100):
|
||||
self.compare_loads(small_file_url, 100 * i, 100)
|
||||
|
||||
def test_large_file(self):
|
||||
large_file_url = "https://commadataci.blob.core.windows.net/openpilotci/0375fdf7b1ce594d/2019-06-13--08-32-25/3/qlog.bz2"
|
||||
# Load the end 100 bytes of both files
|
||||
file_large = URLFile(large_file_url)
|
||||
length = file_large.get_length()
|
||||
|
||||
self.compare_loads(large_file_url, length - 100, 100)
|
||||
self.compare_loads(large_file_url)
|
||||
|
||||
@pytest.mark.parametrize("cache_enabled", [True, False])
|
||||
def test_recover_from_missing_file(self, host, cache_enabled):
|
||||
if cache_enabled:
|
||||
os.environ.pop("DISABLE_FILEREADER_CACHE", None)
|
||||
else:
|
||||
os.environ["DISABLE_FILEREADER_CACHE"] = "1"
|
||||
|
||||
file_url = f"{host}/test.png"
|
||||
|
||||
CachingTestRequestHandler.FILE_EXISTS = False
|
||||
length = URLFile(file_url).get_length()
|
||||
assert length == -1
|
||||
|
||||
CachingTestRequestHandler.FILE_EXISTS = True
|
||||
length = URLFile(file_url).get_length()
|
||||
assert length == 4
|
||||
|
||||
|
||||
class TestCache:
|
||||
def test_prune_cache(self, monkeypatch):
|
||||
with tempfile.TemporaryDirectory() as tmpdir:
|
||||
monkeypatch.setattr(Paths, 'download_cache_root', staticmethod(lambda: tmpdir + "/"))
|
||||
|
||||
# setup test files and manifest
|
||||
manifest_lines = []
|
||||
for i in range(3):
|
||||
fname = f"hash_{i}"
|
||||
with open(tmpdir + "/" + fname, "wb") as f:
|
||||
f.truncate(1000)
|
||||
manifest_lines.append(f"{fname} {1000 + i}")
|
||||
with open(tmpdir + "/manifest.txt", "w") as f:
|
||||
f.write('\n'.join(manifest_lines))
|
||||
|
||||
# under limit, shouldn't prune
|
||||
assert len(os.listdir(tmpdir)) == 4
|
||||
prune_cache()
|
||||
assert len(os.listdir(tmpdir)) == 4
|
||||
|
||||
# set a tiny cache limit to force eviction (1.5 chunks worth)
|
||||
monkeypatch.setattr(url_file_module, 'CACHE_SIZE', url_file_module.CHUNK_SIZE + url_file_module.CHUNK_SIZE // 2)
|
||||
|
||||
# prune_cache should evict oldest files to get under limit
|
||||
prune_cache()
|
||||
remaining = os.listdir(tmpdir)
|
||||
# should have evicted at least one file + manifest
|
||||
assert len(remaining) < 4
|
||||
# newest file should remain
|
||||
assert manifest_lines[2].split()[0] in remaining
|
||||
34
tools/lib/tests/test_comma_car_segments.py
Normal file
34
tools/lib/tests/test_comma_car_segments.py
Normal file
@@ -0,0 +1,34 @@
|
||||
import pytest
|
||||
import requests
|
||||
from iqdbc.car.fingerprints import MIGRATION
|
||||
from openpilot.tools.lib.comma_car_segments import get_comma_car_segments_database, get_url
|
||||
from openpilot.tools.lib.logreader import LogReader
|
||||
from openpilot.tools.lib.route import SegmentRange
|
||||
|
||||
|
||||
@pytest.mark.skip(reason="huggingface is flaky, run this test manually to check for issues")
|
||||
class TestCommaCarSegments:
|
||||
def test_database(self):
|
||||
database = get_comma_car_segments_database()
|
||||
|
||||
platforms = database.keys()
|
||||
|
||||
assert len(platforms) > 100
|
||||
|
||||
def test_download_segment(self):
|
||||
database = get_comma_car_segments_database()
|
||||
|
||||
fp = "SUBARU_FORESTER"
|
||||
|
||||
segment = database[fp][0]
|
||||
|
||||
sr = SegmentRange(segment)
|
||||
|
||||
url = get_url(sr.route_name, sr.slice)
|
||||
|
||||
resp = requests.get(url)
|
||||
assert resp.status_code == 200
|
||||
|
||||
lr = LogReader(url)
|
||||
CP = lr.first("carParams")
|
||||
assert MIGRATION.get(CP.carFingerprint, CP.carFingerprint) == fp
|
||||
263
tools/lib/tests/test_logreader.py
Normal file
263
tools/lib/tests/test_logreader.py
Normal file
@@ -0,0 +1,263 @@
|
||||
import capnp
|
||||
import contextlib
|
||||
import io
|
||||
import shutil
|
||||
import tempfile
|
||||
import os
|
||||
import pytest
|
||||
import requests
|
||||
|
||||
from parameterized import parameterized
|
||||
|
||||
from cereal import log as capnp_log
|
||||
from openpilot.tools.lib.logreader import LogsUnavailable, LogIterable, LogReader, parse_indirect, ReadMode
|
||||
from openpilot.tools.lib.file_sources import comma_api_source, InternalUnavailableException
|
||||
from openpilot.tools.lib.route import SegmentRange
|
||||
from openpilot.tools.lib.url_file import URLFileException
|
||||
|
||||
NUM_SEGS = 17 # number of segments in the test route
|
||||
ALL_SEGS = list(range(NUM_SEGS))
|
||||
TEST_ROUTE = "344c5c15b34f2d8a/2024-01-03--09-37-12"
|
||||
QLOG_FILE = "https://commadataci.blob.core.windows.net/openpilotci/0375fdf7b1ce594d/2019-06-13--08-32-25/3/qlog.bz2"
|
||||
|
||||
|
||||
def noop(segment: LogIterable):
|
||||
return segment
|
||||
|
||||
|
||||
@contextlib.contextmanager
|
||||
def setup_source_scenario(mocker, is_internal=False):
|
||||
internal_source_mock = mocker.patch("openpilot.tools.lib.logreader.internal_source")
|
||||
internal_source_mock.__name__ = internal_source_mock._mock_name
|
||||
|
||||
openpilotci_source_mock = mocker.patch("openpilot.tools.lib.logreader.openpilotci_source")
|
||||
openpilotci_source_mock.__name__ = openpilotci_source_mock._mock_name
|
||||
|
||||
comma_api_source_mock = mocker.patch("openpilot.tools.lib.logreader.comma_api_source")
|
||||
comma_api_source_mock.__name__ = comma_api_source_mock._mock_name
|
||||
|
||||
if is_internal:
|
||||
internal_source_mock.return_value = {3: QLOG_FILE}
|
||||
else:
|
||||
internal_source_mock.side_effect = InternalUnavailableException
|
||||
|
||||
openpilotci_source_mock.return_value = {}
|
||||
comma_api_source_mock.return_value = {3: QLOG_FILE}
|
||||
|
||||
yield
|
||||
|
||||
|
||||
class TestLogReader:
|
||||
@parameterized.expand([
|
||||
(f"{TEST_ROUTE}", ALL_SEGS),
|
||||
(f"{TEST_ROUTE.replace('/', '|')}", ALL_SEGS),
|
||||
(f"{TEST_ROUTE}--0", [0]),
|
||||
(f"{TEST_ROUTE}--5", [5]),
|
||||
(f"{TEST_ROUTE}/0", [0]),
|
||||
(f"{TEST_ROUTE}/5", [5]),
|
||||
(f"{TEST_ROUTE}/0:10", ALL_SEGS[0:10]),
|
||||
(f"{TEST_ROUTE}/0:0", []),
|
||||
(f"{TEST_ROUTE}/4:6", ALL_SEGS[4:6]),
|
||||
(f"{TEST_ROUTE}/0:-1", ALL_SEGS[0:-1]),
|
||||
(f"{TEST_ROUTE}/:5", ALL_SEGS[:5]),
|
||||
(f"{TEST_ROUTE}/2:", ALL_SEGS[2:]),
|
||||
(f"{TEST_ROUTE}/2:-1", ALL_SEGS[2:-1]),
|
||||
(f"{TEST_ROUTE}/-1", [ALL_SEGS[-1]]),
|
||||
(f"{TEST_ROUTE}/-2", [ALL_SEGS[-2]]),
|
||||
(f"{TEST_ROUTE}/-2:-1", ALL_SEGS[-2:-1]),
|
||||
(f"{TEST_ROUTE}/-4:-2", ALL_SEGS[-4:-2]),
|
||||
(f"{TEST_ROUTE}/:10:2", ALL_SEGS[:10:2]),
|
||||
(f"{TEST_ROUTE}/5::2", ALL_SEGS[5::2]),
|
||||
(f"https://useradmin.comma.ai/?onebox={TEST_ROUTE}", ALL_SEGS),
|
||||
(f"https://useradmin.comma.ai/?onebox={TEST_ROUTE.replace('/', '|')}", ALL_SEGS),
|
||||
(f"https://useradmin.comma.ai/?onebox={TEST_ROUTE.replace('/', '%7C')}", ALL_SEGS),
|
||||
])
|
||||
@pytest.mark.skip("this got flaky. internet tests are stupid.")
|
||||
def test_indirect_parsing(self, identifier, expected):
|
||||
parsed = parse_indirect(identifier)
|
||||
sr = SegmentRange(parsed)
|
||||
assert list(sr.seg_idxs) == expected, identifier
|
||||
|
||||
@parameterized.expand([
|
||||
(f"{TEST_ROUTE}", f"{TEST_ROUTE}"),
|
||||
(f"{TEST_ROUTE.replace('/', '|')}", f"{TEST_ROUTE}"),
|
||||
(f"{TEST_ROUTE}--5", f"{TEST_ROUTE}/5"),
|
||||
(f"{TEST_ROUTE}/0/q", f"{TEST_ROUTE}/0/q"),
|
||||
(f"{TEST_ROUTE}/5:6/r", f"{TEST_ROUTE}/5:6/r"),
|
||||
(f"{TEST_ROUTE}/5", f"{TEST_ROUTE}/5"),
|
||||
])
|
||||
def test_canonical_name(self, identifier, expected):
|
||||
sr = SegmentRange(identifier)
|
||||
assert str(sr) == expected
|
||||
|
||||
@pytest.mark.parametrize("cache_enabled", [True, False])
|
||||
def test_direct_parsing(self, mocker, cache_enabled):
|
||||
file_exists_mock = mocker.patch("openpilot.tools.lib.filereader.file_exists")
|
||||
if cache_enabled:
|
||||
os.environ.pop("DISABLE_FILEREADER_CACHE", None)
|
||||
else:
|
||||
os.environ["DISABLE_FILEREADER_CACHE"] = "1"
|
||||
qlog = tempfile.NamedTemporaryFile(mode='wb', delete=False)
|
||||
|
||||
with requests.get(QLOG_FILE, stream=True) as r:
|
||||
with qlog as f:
|
||||
shutil.copyfileobj(r.raw, f)
|
||||
|
||||
for f in [QLOG_FILE, qlog.name]:
|
||||
l = len(list(LogReader(f)))
|
||||
assert l > 100
|
||||
|
||||
with pytest.raises(URLFileException) if not cache_enabled else pytest.raises(AssertionError):
|
||||
l = len(list(LogReader(QLOG_FILE.replace("/3/", "/200/"))))
|
||||
|
||||
# file_exists should not be called for direct files
|
||||
assert file_exists_mock.call_count == 0
|
||||
|
||||
@parameterized.expand([
|
||||
(f"{TEST_ROUTE}///",),
|
||||
(f"{TEST_ROUTE}---",),
|
||||
(f"{TEST_ROUTE}/-4:--2",),
|
||||
(f"{TEST_ROUTE}/-a",),
|
||||
(f"{TEST_ROUTE}/j",),
|
||||
(f"{TEST_ROUTE}/0:1:2:3",),
|
||||
(f"{TEST_ROUTE}/:::3",),
|
||||
(f"{TEST_ROUTE}3",),
|
||||
(f"{TEST_ROUTE}-3",),
|
||||
(f"{TEST_ROUTE}--3a",),
|
||||
])
|
||||
def test_bad_ranges(self, segment_range):
|
||||
with pytest.raises(AssertionError):
|
||||
_ = SegmentRange(segment_range).seg_idxs
|
||||
|
||||
@pytest.mark.parametrize("segment_range, api_call", [
|
||||
(f"{TEST_ROUTE}/0", False),
|
||||
(f"{TEST_ROUTE}/:2", False),
|
||||
(f"{TEST_ROUTE}/0:", True),
|
||||
(f"{TEST_ROUTE}/-1", True),
|
||||
(f"{TEST_ROUTE}", True),
|
||||
])
|
||||
def test_slicing_api_call(self, mocker, segment_range, api_call):
|
||||
max_seg_mock = mocker.patch("openpilot.tools.lib.route.get_max_seg_number_cached")
|
||||
max_seg_mock.return_value = NUM_SEGS
|
||||
_ = SegmentRange(segment_range).seg_idxs
|
||||
assert api_call == max_seg_mock.called
|
||||
|
||||
@pytest.mark.slow
|
||||
def test_modes(self):
|
||||
qlog_len = len(list(LogReader(f"{TEST_ROUTE}/0", ReadMode.QLOG)))
|
||||
rlog_len = len(list(LogReader(f"{TEST_ROUTE}/0", ReadMode.RLOG)))
|
||||
|
||||
assert qlog_len * 6 < rlog_len
|
||||
|
||||
@pytest.mark.slow
|
||||
def test_modes_from_name(self):
|
||||
qlog_len = len(list(LogReader(f"{TEST_ROUTE}/0/q")))
|
||||
rlog_len = len(list(LogReader(f"{TEST_ROUTE}/0/r")))
|
||||
|
||||
assert qlog_len * 6 < rlog_len
|
||||
|
||||
@pytest.mark.slow
|
||||
def test_list(self):
|
||||
qlog_len = len(list(LogReader(f"{TEST_ROUTE}/0/q")))
|
||||
qlog_len_2 = len(list(LogReader([f"{TEST_ROUTE}/0/q", f"{TEST_ROUTE}/0/q"])))
|
||||
|
||||
assert qlog_len * 2 == qlog_len_2
|
||||
|
||||
@pytest.mark.slow
|
||||
def test_multiple_iterations(self, mocker):
|
||||
init_mock = mocker.patch("openpilot.tools.lib.logreader._LogFileReader")
|
||||
lr = LogReader(f"{TEST_ROUTE}/0/q")
|
||||
qlog_len1 = len(list(lr))
|
||||
qlog_len2 = len(list(lr))
|
||||
|
||||
# ensure we don't create multiple instances of _LogFileReader, which means downloading the files twice
|
||||
assert init_mock.call_count == 1
|
||||
|
||||
assert qlog_len1 == qlog_len2
|
||||
|
||||
@pytest.mark.slow
|
||||
def test_helpers(self):
|
||||
lr = LogReader(f"{TEST_ROUTE}/0/q")
|
||||
assert lr.first("carParams").carFingerprint == "SUBARU OUTBACK 6TH GEN"
|
||||
assert 0 < len(list(lr.filter("carParams"))) < len(list(lr))
|
||||
|
||||
@parameterized.expand([(True,), (False,)])
|
||||
@pytest.mark.slow
|
||||
def test_run_across_segments(self, cache_enabled):
|
||||
if cache_enabled:
|
||||
os.environ.pop("DISABLE_FILEREADER_CACHE", None)
|
||||
else:
|
||||
os.environ["DISABLE_FILEREADER_CACHE"] = "1"
|
||||
lr = LogReader(f"{TEST_ROUTE}/0:4")
|
||||
assert len(lr.run_across_segments(4, noop)) == len(list(lr))
|
||||
|
||||
@pytest.mark.slow
|
||||
def test_auto_mode(self, subtests, mocker):
|
||||
lr = LogReader(f"{TEST_ROUTE}/0/q")
|
||||
qlog_len = len(list(lr))
|
||||
log_paths_mock = mocker.patch("openpilot.tools.lib.route.Route.log_paths")
|
||||
log_paths_mock.return_value = [None] * NUM_SEGS
|
||||
# Should fall back to qlogs since rlogs are not available
|
||||
|
||||
with subtests.test("interactive_yes"):
|
||||
mocker.patch("sys.stdin", new=io.StringIO("y\n"))
|
||||
lr = LogReader(f"{TEST_ROUTE}/0", default_mode=ReadMode.AUTO_INTERACTIVE, sources=[comma_api_source])
|
||||
log_len = len(list(lr))
|
||||
assert qlog_len == log_len
|
||||
|
||||
with subtests.test("interactive_no"):
|
||||
mocker.patch("sys.stdin", new=io.StringIO("n\n"))
|
||||
with pytest.raises(LogsUnavailable):
|
||||
lr = LogReader(f"{TEST_ROUTE}/0", default_mode=ReadMode.AUTO_INTERACTIVE, sources=[comma_api_source])
|
||||
|
||||
with subtests.test("non_interactive"):
|
||||
lr = LogReader(f"{TEST_ROUTE}/0", default_mode=ReadMode.AUTO, sources=[comma_api_source])
|
||||
log_len = len(list(lr))
|
||||
assert qlog_len == log_len
|
||||
|
||||
@pytest.mark.parametrize("is_internal", [True, False])
|
||||
def test_auto_source_scenarios(self, mocker, is_internal):
|
||||
lr = LogReader(QLOG_FILE)
|
||||
qlog_len = len(list(lr))
|
||||
|
||||
with setup_source_scenario(mocker, is_internal=is_internal):
|
||||
lr = LogReader(f"{TEST_ROUTE}/3/q")
|
||||
log_len = len(list(lr))
|
||||
assert qlog_len == log_len
|
||||
|
||||
@pytest.mark.slow
|
||||
def test_sort_by_time(self):
|
||||
msgs = list(LogReader(f"{TEST_ROUTE}/0/q"))
|
||||
assert msgs != sorted(msgs, key=lambda m: m.logMonoTime)
|
||||
|
||||
msgs = list(LogReader(f"{TEST_ROUTE}/0/q", sort_by_time=True))
|
||||
assert msgs == sorted(msgs, key=lambda m: m.logMonoTime)
|
||||
|
||||
def test_only_union_types(self):
|
||||
with tempfile.NamedTemporaryFile() as qlog:
|
||||
# write valid Event messages
|
||||
num_msgs = 100
|
||||
with open(qlog.name, "wb") as f:
|
||||
f.write(b"".join(capnp_log.Event.new_message().to_bytes() for _ in range(num_msgs)))
|
||||
|
||||
msgs = list(LogReader(qlog.name))
|
||||
assert len(msgs) == num_msgs
|
||||
[m.which() for m in msgs]
|
||||
|
||||
# append non-union Event message
|
||||
event_msg = capnp_log.Event.new_message()
|
||||
non_union_bytes = bytearray(event_msg.to_bytes())
|
||||
non_union_bytes[event_msg.total_size.word_count * 8] = 0xff # set discriminant value out of range using Event word offset
|
||||
with open(qlog.name, "ab") as f:
|
||||
f.write(non_union_bytes)
|
||||
|
||||
# ensure new message is added, but is not a union type
|
||||
msgs = list(LogReader(qlog.name))
|
||||
assert len(msgs) == num_msgs + 1
|
||||
with pytest.raises(capnp.KjException):
|
||||
[m.which() for m in msgs]
|
||||
|
||||
# should not be added when only_union_types=True
|
||||
msgs = list(LogReader(qlog.name, only_union_types=True))
|
||||
assert len(msgs) == num_msgs
|
||||
[m.which() for m in msgs]
|
||||
27
tools/lib/tests/test_route_library.py
Normal file
27
tools/lib/tests/test_route_library.py
Normal file
@@ -0,0 +1,27 @@
|
||||
from collections import namedtuple
|
||||
|
||||
from openpilot.tools.lib.route import SegmentName
|
||||
|
||||
class TestRouteLibrary:
|
||||
def test_segment_name_formats(self):
|
||||
Case = namedtuple('Case', ['input', 'expected_route', 'expected_segment_num', 'expected_data_dir'])
|
||||
|
||||
cases = [ Case("a2a0ccea32023010|2023-07-27--13-01-19", "a2a0ccea32023010|2023-07-27--13-01-19", -1, None),
|
||||
Case("a2a0ccea32023010/2023-07-27--13-01-19--1", "a2a0ccea32023010|2023-07-27--13-01-19", 1, None),
|
||||
Case("a2a0ccea32023010|2023-07-27--13-01-19/2", "a2a0ccea32023010|2023-07-27--13-01-19", 2, None),
|
||||
Case("a2a0ccea32023010/2023-07-27--13-01-19/3", "a2a0ccea32023010|2023-07-27--13-01-19", 3, None),
|
||||
Case("/data/media/0/realdata/a2a0ccea32023010|2023-07-27--13-01-19", "a2a0ccea32023010|2023-07-27--13-01-19", -1, "/data/media/0/realdata"),
|
||||
Case("/data/media/0/realdata/a2a0ccea32023010|2023-07-27--13-01-19--1", "a2a0ccea32023010|2023-07-27--13-01-19", 1, "/data/media/0/realdata"),
|
||||
Case("/data/media/0/realdata/a2a0ccea32023010|2023-07-27--13-01-19/2", "a2a0ccea32023010|2023-07-27--13-01-19", 2, "/data/media/0/realdata") ]
|
||||
|
||||
def _validate(case):
|
||||
route_or_segment_name = case.input
|
||||
|
||||
s = SegmentName(route_or_segment_name, allow_route_name=True)
|
||||
|
||||
assert str(s.route_name) == case.expected_route
|
||||
assert s.segment_num == case.expected_segment_num
|
||||
assert s.data_dir == case.expected_data_dir
|
||||
|
||||
for case in cases:
|
||||
_validate(case)
|
||||
240
tools/lib/url_file.py
Normal file
240
tools/lib/url_file.py
Normal file
@@ -0,0 +1,240 @@
|
||||
import logging
|
||||
import os
|
||||
import re
|
||||
import socket
|
||||
import time
|
||||
from hashlib import md5
|
||||
from urllib3 import PoolManager, Retry
|
||||
from urllib3.response import BaseHTTPResponse
|
||||
from urllib3.util import Timeout
|
||||
|
||||
from openpilot.common.utils import atomic_write
|
||||
from openpilot.system.hardware.hw import Paths
|
||||
from urllib3.exceptions import MaxRetryError
|
||||
|
||||
# Cache chunk size
|
||||
K = 1000
|
||||
CHUNK_SIZE = 1000 * K
|
||||
CACHE_SIZE = 10 * 1024 * 1024 * 1024 # total cache size in GB
|
||||
|
||||
logging.getLogger("urllib3").setLevel(logging.WARNING)
|
||||
|
||||
|
||||
def _env_int(name: str, default: int) -> int:
|
||||
raw = os.getenv(name)
|
||||
if raw is None:
|
||||
return default
|
||||
try:
|
||||
value = int(raw)
|
||||
return value if value > 0 else default
|
||||
except ValueError:
|
||||
return default
|
||||
|
||||
|
||||
def hash_url(link: str) -> str:
|
||||
return md5((link.split("?")[0]).encode('utf-8')).hexdigest()
|
||||
|
||||
|
||||
def prune_cache(new_entry: str | None = None) -> None:
|
||||
"""Evicts oldest cache files (LRU) until cache is under the size limit."""
|
||||
# we use a manifest to avoid tons of os.stat syscalls (slow)
|
||||
manifest = {}
|
||||
manifest_path = Paths.download_cache_root() + "manifest.txt"
|
||||
if os.path.exists(manifest_path):
|
||||
with open(manifest_path) as f:
|
||||
manifest = {parts[0]: int(parts[1]) for line in f if (parts := line.strip().split()) and len(parts) == 2}
|
||||
|
||||
if new_entry:
|
||||
manifest[new_entry] = int(time.time()) # noqa: TID251
|
||||
|
||||
# evict the least recently used files until under limit
|
||||
sorted_items = sorted(manifest.items(), key=lambda x: x[1])
|
||||
while len(manifest) * CHUNK_SIZE > CACHE_SIZE and sorted_items:
|
||||
key, _ = sorted_items.pop(0)
|
||||
try:
|
||||
os.remove(Paths.download_cache_root() + key)
|
||||
except OSError:
|
||||
pass
|
||||
manifest.pop(key, None)
|
||||
|
||||
# write out manifest
|
||||
with atomic_write(manifest_path, mode="w", overwrite=True) as f:
|
||||
f.write('\n'.join(f"{k} {v}" for k, v in manifest.items()))
|
||||
|
||||
class URLFileException(Exception):
|
||||
pass
|
||||
|
||||
|
||||
class URLFile:
|
||||
_pool_manager: PoolManager | None = None
|
||||
|
||||
@staticmethod
|
||||
def reset() -> None:
|
||||
URLFile._pool_manager = None
|
||||
|
||||
@staticmethod
|
||||
def pool_manager() -> PoolManager:
|
||||
if URLFile._pool_manager is None:
|
||||
socket_options = [(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1)]
|
||||
retries = Retry(
|
||||
total=_env_int("URLFILE_RETRIES_TOTAL", 6),
|
||||
connect=_env_int("URLFILE_RETRIES_CONNECT", 6),
|
||||
read=_env_int("URLFILE_RETRIES_READ", 6),
|
||||
backoff_factor=float(os.getenv("URLFILE_RETRIES_BACKOFF", "0.75")),
|
||||
status_forcelist=[409, 429, 500, 502, 503, 504],
|
||||
)
|
||||
URLFile._pool_manager = PoolManager(num_pools=10, maxsize=100, socket_options=socket_options, retries=retries)
|
||||
return URLFile._pool_manager
|
||||
|
||||
def __init__(self, url: str, timeout: int = 10, cache: bool | None = None):
|
||||
self._url = url
|
||||
connect_timeout = _env_int("URLFILE_CONNECT_TIMEOUT", min(timeout, 10))
|
||||
read_timeout = _env_int("URLFILE_READ_TIMEOUT", max(timeout, 30))
|
||||
total_timeout = _env_int("URLFILE_TOTAL_TIMEOUT", max(read_timeout * 4, 180))
|
||||
self._timeout = Timeout(connect=connect_timeout, read=read_timeout, total=total_timeout)
|
||||
self._pos = 0
|
||||
self._length: int | None = None
|
||||
# Caching enabled by default, can be disabled with DISABLE_FILEREADER_CACHE=1, or overwritten by the cache input
|
||||
self._force_download = int(os.environ.get("DISABLE_FILEREADER_CACHE", "0")) == 1
|
||||
if cache is not None:
|
||||
self._force_download = not cache
|
||||
|
||||
if not self._force_download:
|
||||
os.makedirs(Paths.download_cache_root(), exist_ok=True)
|
||||
|
||||
def __enter__(self):
|
||||
return self
|
||||
|
||||
def __exit__(self, exc_type, exc_value, traceback) -> None:
|
||||
pass
|
||||
|
||||
def _request(self, method: str, url: str, headers: dict[str, str] | None = None) -> BaseHTTPResponse:
|
||||
try:
|
||||
return URLFile.pool_manager().request(method, url, timeout=self._timeout, headers=headers)
|
||||
except MaxRetryError as e:
|
||||
raise URLFileException(f"Failed to {method} {url}: {e}") from e
|
||||
|
||||
def get_length_online(self) -> int:
|
||||
response = self._request('HEAD', self._url)
|
||||
if not (200 <= response.status <= 299):
|
||||
return -1
|
||||
length = response.headers.get('content-length', 0)
|
||||
return int(length)
|
||||
|
||||
def get_length(self) -> int:
|
||||
if self._length is not None:
|
||||
return self._length
|
||||
|
||||
file_length_path = os.path.join(Paths.download_cache_root(), hash_url(self._url) + "_length")
|
||||
if not self._force_download and os.path.exists(file_length_path):
|
||||
with open(file_length_path) as file_length:
|
||||
content = file_length.read()
|
||||
self._length = int(content)
|
||||
return self._length
|
||||
|
||||
self._length = self.get_length_online()
|
||||
if not self._force_download and self._length != -1:
|
||||
with atomic_write(file_length_path, mode="w", overwrite=True) as file_length:
|
||||
file_length.write(str(self._length))
|
||||
return self._length
|
||||
|
||||
def read(self, ll: int | None = None) -> bytes:
|
||||
if self._force_download:
|
||||
return self.read_aux(ll=ll)
|
||||
|
||||
file_begin = self._pos
|
||||
file_end = self._pos + ll if ll is not None else self.get_length()
|
||||
assert file_end != -1, f"Remote file is empty or doesn't exist: {self._url}"
|
||||
# We have to align with chunks we store. Position is the begginiing of the latest chunk that starts before or at our file
|
||||
position = (file_begin // CHUNK_SIZE) * CHUNK_SIZE
|
||||
response = b""
|
||||
while True:
|
||||
self._pos = position
|
||||
chunk_number = self._pos / CHUNK_SIZE
|
||||
file_name = hash_url(self._url) + "_" + str(chunk_number)
|
||||
full_path = os.path.join(Paths.download_cache_root(), str(file_name))
|
||||
data = None
|
||||
# If we don't have a file, download it
|
||||
if not os.path.exists(full_path):
|
||||
data = self.read_aux(ll=CHUNK_SIZE)
|
||||
with atomic_write(full_path, mode="wb", overwrite=True) as new_cached_file:
|
||||
new_cached_file.write(data)
|
||||
prune_cache(file_name)
|
||||
else:
|
||||
with open(full_path, "rb") as cached_file:
|
||||
data = cached_file.read()
|
||||
|
||||
response += data[max(0, file_begin - position): min(CHUNK_SIZE, file_end - position)]
|
||||
|
||||
position += CHUNK_SIZE
|
||||
if position >= file_end:
|
||||
self._pos = file_end
|
||||
return response
|
||||
|
||||
def read_aux(self, ll: int | None = None) -> bytes:
|
||||
if ll is None:
|
||||
length = self.get_length()
|
||||
if length == -1:
|
||||
raise URLFileException(f"Remote file is empty or doesn't exist: {self._url}")
|
||||
end = length
|
||||
else:
|
||||
end = self._pos + ll
|
||||
data = self.get_multi_range([(self._pos, end)])
|
||||
self._pos += len(data[0])
|
||||
return data[0]
|
||||
|
||||
def get_multi_range(self, ranges: list[tuple[int, int]]) -> list[bytes]:
|
||||
# HTTP range requests are inclusive
|
||||
assert all(e > s for s, e in ranges), "Range end must be greater than start"
|
||||
rs = [f"{s}-{e-1}" for s, e in ranges if e > s]
|
||||
|
||||
r = self._request("GET", self._url, headers={"Range": "bytes=" + ",".join(rs)})
|
||||
if r.status not in [200, 206]:
|
||||
raise URLFileException(f"Expected 206 or 200 response {r.status} ({self._url})")
|
||||
|
||||
ctype = (r.headers.get("content-type") or "").lower()
|
||||
if "multipart/byteranges" not in ctype:
|
||||
return [r.data,]
|
||||
|
||||
m = re.search(r'boundary="?([^";]+)"?', ctype)
|
||||
if not m:
|
||||
raise URLFileException(f"Missing multipart boundary ({self._url})")
|
||||
boundary = m.group(1).encode()
|
||||
|
||||
parts = []
|
||||
for chunk in r.data.split(b"--" + boundary):
|
||||
if b"\r\n\r\n" not in chunk:
|
||||
continue
|
||||
payload = chunk.split(b"\r\n\r\n", 1)[1].rstrip(b"\r\n")
|
||||
if payload and payload != b"--":
|
||||
parts.append(payload)
|
||||
if len(parts) != len(ranges):
|
||||
raise URLFileException(f"Expected {len(ranges)} parts, got {len(parts)} ({self._url})")
|
||||
return parts
|
||||
|
||||
def seekable(self) -> bool:
|
||||
return True
|
||||
|
||||
def seek(self, pos: int, whence: int = 0) -> int:
|
||||
pos = int(pos)
|
||||
if whence == os.SEEK_SET:
|
||||
self._pos = pos
|
||||
elif whence == os.SEEK_CUR:
|
||||
self._pos += pos
|
||||
elif whence == os.SEEK_END:
|
||||
length = self.get_length()
|
||||
assert length != -1, "Cannot seek from end on unknown length file"
|
||||
self._pos = length + pos
|
||||
else:
|
||||
raise URLFileException("Invalid whence value")
|
||||
return self._pos
|
||||
|
||||
def tell(self) -> int:
|
||||
return self._pos
|
||||
|
||||
@property
|
||||
def name(self) -> str:
|
||||
return self._url
|
||||
|
||||
|
||||
os.register_at_fork(after_in_child=URLFile.reset)
|
||||
311
tools/lib/vidindex.py
Executable file
311
tools/lib/vidindex.py
Executable file
@@ -0,0 +1,311 @@
|
||||
#!/usr/bin/env python3
|
||||
import argparse
|
||||
import os
|
||||
import struct
|
||||
from enum import IntEnum
|
||||
|
||||
from openpilot.tools.lib.filereader import FileReader
|
||||
|
||||
DEBUG = int(os.getenv("DEBUG", "0"))
|
||||
|
||||
# compare to ffmpeg parsing
|
||||
# ffmpeg -i <input.hevc> -c copy -bsf:v trace_headers -f null - 2>&1 | grep -B4 -A32 '] 0 '
|
||||
|
||||
# H.265 specification
|
||||
# https://www.itu.int/rec/dologin_pub.asp?lang=e&id=T-REC-H.265-201802-S!!PDF-E&type=items
|
||||
|
||||
NAL_UNIT_START_CODE = b"\x00\x00\x01"
|
||||
NAL_UNIT_START_CODE_SIZE = len(NAL_UNIT_START_CODE)
|
||||
NAL_UNIT_HEADER_SIZE = 2
|
||||
|
||||
class HevcNalUnitType(IntEnum):
|
||||
TRAIL_N = 0 # RBSP structure: slice_segment_layer_rbsp( )
|
||||
TRAIL_R = 1 # RBSP structure: slice_segment_layer_rbsp( )
|
||||
TSA_N = 2 # RBSP structure: slice_segment_layer_rbsp( )
|
||||
TSA_R = 3 # RBSP structure: slice_segment_layer_rbsp( )
|
||||
STSA_N = 4 # RBSP structure: slice_segment_layer_rbsp( )
|
||||
STSA_R = 5 # RBSP structure: slice_segment_layer_rbsp( )
|
||||
RADL_N = 6 # RBSP structure: slice_segment_layer_rbsp( )
|
||||
RADL_R = 7 # RBSP structure: slice_segment_layer_rbsp( )
|
||||
RASL_N = 8 # RBSP structure: slice_segment_layer_rbsp( )
|
||||
RASL_R = 9 # RBSP structure: slice_segment_layer_rbsp( )
|
||||
RSV_VCL_N10 = 10
|
||||
RSV_VCL_R11 = 11
|
||||
RSV_VCL_N12 = 12
|
||||
RSV_VCL_R13 = 13
|
||||
RSV_VCL_N14 = 14
|
||||
RSV_VCL_R15 = 15
|
||||
BLA_W_LP = 16 # RBSP structure: slice_segment_layer_rbsp( )
|
||||
BLA_W_RADL = 17 # RBSP structure: slice_segment_layer_rbsp( )
|
||||
BLA_N_LP = 18 # RBSP structure: slice_segment_layer_rbsp( )
|
||||
IDR_W_RADL = 19 # RBSP structure: slice_segment_layer_rbsp( )
|
||||
IDR_N_LP = 20 # RBSP structure: slice_segment_layer_rbsp( )
|
||||
CRA_NUT = 21 # RBSP structure: slice_segment_layer_rbsp( )
|
||||
RSV_IRAP_VCL22 = 22
|
||||
RSV_IRAP_VCL23 = 23
|
||||
RSV_VCL24 = 24
|
||||
RSV_VCL25 = 25
|
||||
RSV_VCL26 = 26
|
||||
RSV_VCL27 = 27
|
||||
RSV_VCL28 = 28
|
||||
RSV_VCL29 = 29
|
||||
RSV_VCL30 = 30
|
||||
RSV_VCL31 = 31
|
||||
VPS_NUT = 32 # RBSP structure: video_parameter_set_rbsp( )
|
||||
SPS_NUT = 33 # RBSP structure: seq_parameter_set_rbsp( )
|
||||
PPS_NUT = 34 # RBSP structure: pic_parameter_set_rbsp( )
|
||||
AUD_NUT = 35
|
||||
EOS_NUT = 36
|
||||
EOB_NUT = 37
|
||||
FD_NUT = 38
|
||||
PREFIX_SEI_NUT = 39
|
||||
SUFFIX_SEI_NUT = 40
|
||||
RSV_NVCL41 = 41
|
||||
RSV_NVCL42 = 42
|
||||
RSV_NVCL43 = 43
|
||||
RSV_NVCL44 = 44
|
||||
RSV_NVCL45 = 45
|
||||
RSV_NVCL46 = 46
|
||||
RSV_NVCL47 = 47
|
||||
UNSPEC48 = 48
|
||||
UNSPEC49 = 49
|
||||
UNSPEC50 = 50
|
||||
UNSPEC51 = 51
|
||||
UNSPEC52 = 52
|
||||
UNSPEC53 = 53
|
||||
UNSPEC54 = 54
|
||||
UNSPEC55 = 55
|
||||
UNSPEC56 = 56
|
||||
UNSPEC57 = 57
|
||||
UNSPEC58 = 58
|
||||
UNSPEC59 = 59
|
||||
UNSPEC60 = 60
|
||||
UNSPEC61 = 61
|
||||
UNSPEC62 = 62
|
||||
UNSPEC63 = 63
|
||||
|
||||
# B.2.2 Byte stream NAL unit semantics
|
||||
# - The nal_unit_type within the nal_unit( ) syntax structure is equal to VPS_NUT, SPS_NUT or PPS_NUT.
|
||||
# - The byte stream NAL unit syntax structure contains the first NAL unit of an access unit in decoding
|
||||
# order, as specified in clause 7.4.2.4.4.
|
||||
HEVC_PARAMETER_SET_NAL_UNITS = (
|
||||
HevcNalUnitType.VPS_NUT,
|
||||
HevcNalUnitType.SPS_NUT,
|
||||
HevcNalUnitType.PPS_NUT,
|
||||
)
|
||||
|
||||
# 3.29 coded slice segment NAL unit: A NAL unit that has nal_unit_type in the range of TRAIL_N to RASL_R,
|
||||
# inclusive, or in the range of BLA_W_LP to RSV_IRAP_VCL23, inclusive, which indicates that the NAL unit
|
||||
# contains a coded slice segment
|
||||
HEVC_CODED_SLICE_SEGMENT_NAL_UNITS = (
|
||||
HevcNalUnitType.TRAIL_N,
|
||||
HevcNalUnitType.TRAIL_R,
|
||||
HevcNalUnitType.TSA_N,
|
||||
HevcNalUnitType.TSA_R,
|
||||
HevcNalUnitType.STSA_N,
|
||||
HevcNalUnitType.STSA_R,
|
||||
HevcNalUnitType.RADL_N,
|
||||
HevcNalUnitType.RADL_R,
|
||||
HevcNalUnitType.RASL_N,
|
||||
HevcNalUnitType.RASL_R,
|
||||
HevcNalUnitType.BLA_W_LP,
|
||||
HevcNalUnitType.BLA_W_RADL,
|
||||
HevcNalUnitType.BLA_N_LP,
|
||||
HevcNalUnitType.IDR_W_RADL,
|
||||
HevcNalUnitType.IDR_N_LP,
|
||||
HevcNalUnitType.CRA_NUT,
|
||||
)
|
||||
|
||||
class VideoFileInvalid(Exception):
|
||||
pass
|
||||
|
||||
def get_ue(dat: bytes, start_idx: int, skip_bits: int) -> tuple[int, int]:
|
||||
prefix_val = 0
|
||||
prefix_len = 0
|
||||
suffix_val = 0
|
||||
suffix_len = 0
|
||||
|
||||
i = start_idx
|
||||
while i < len(dat):
|
||||
j = 7
|
||||
while j >= 0:
|
||||
if skip_bits > 0:
|
||||
skip_bits -= 1
|
||||
elif prefix_val == 0:
|
||||
prefix_val = (dat[i] >> j) & 1
|
||||
prefix_len += 1
|
||||
else:
|
||||
suffix_val = (suffix_val << 1) | ((dat[i] >> j) & 1)
|
||||
suffix_len += 1
|
||||
j -= 1
|
||||
|
||||
if prefix_val == 1 and prefix_len - 1 == suffix_len:
|
||||
val = int(2**(prefix_len-1) - 1 + suffix_val)
|
||||
size = prefix_len + suffix_len
|
||||
return val, size
|
||||
i += 1
|
||||
|
||||
raise VideoFileInvalid("invalid exponential-golomb code")
|
||||
|
||||
def require_nal_unit_start(dat: bytes, nal_unit_start: int) -> None:
|
||||
if nal_unit_start < 1:
|
||||
raise ValueError("start index must be greater than zero")
|
||||
|
||||
if dat[nal_unit_start:nal_unit_start + NAL_UNIT_START_CODE_SIZE] != NAL_UNIT_START_CODE:
|
||||
raise VideoFileInvalid("data must begin with start code")
|
||||
|
||||
def get_hevc_nal_unit_length(dat: bytes, nal_unit_start: int) -> int:
|
||||
try:
|
||||
pos = dat.index(NAL_UNIT_START_CODE, nal_unit_start + NAL_UNIT_START_CODE_SIZE)
|
||||
except ValueError:
|
||||
pos = -1
|
||||
|
||||
# length of NAL unit is byte count up to next NAL unit start index
|
||||
nal_unit_len = (pos if pos != -1 else len(dat)) - nal_unit_start
|
||||
if DEBUG:
|
||||
print(" nal_unit_len:", nal_unit_len)
|
||||
return nal_unit_len
|
||||
|
||||
def get_hevc_nal_unit_type(dat: bytes, nal_unit_start: int) -> HevcNalUnitType:
|
||||
# 7.3.1.2 NAL unit header syntax
|
||||
# nal_unit_header( ) { // descriptor
|
||||
# forbidden_zero_bit f(1)
|
||||
# nal_unit_type u(6)
|
||||
# nuh_layer_id u(6)
|
||||
# nuh_temporal_id_plus1 u(3)
|
||||
# }
|
||||
header_start = nal_unit_start + NAL_UNIT_START_CODE_SIZE
|
||||
nal_unit_header = dat[header_start:header_start + NAL_UNIT_HEADER_SIZE]
|
||||
if len(nal_unit_header) != 2:
|
||||
raise VideoFileInvalid("data to short to contain nal unit header")
|
||||
nal_unit_type = HevcNalUnitType((nal_unit_header[0] >> 1) & 0x3F)
|
||||
if DEBUG:
|
||||
print(" nal_unit_type:", nal_unit_type.name, f"({nal_unit_type.value})")
|
||||
return nal_unit_type
|
||||
|
||||
def get_hevc_slice_type(dat: bytes, nal_unit_start: int, nal_unit_type: HevcNalUnitType) -> tuple[int, bool]:
|
||||
# 7.3.2.9 Slice segment layer RBSP syntax
|
||||
# slice_segment_layer_rbsp( ) {
|
||||
# slice_segment_header( )
|
||||
# slice_segment_data( )
|
||||
# rbsp_slice_segment_trailing_bits( )
|
||||
# }
|
||||
# ...
|
||||
# 7.3.6.1 General slice segment header syntax
|
||||
# slice_segment_header( ) { // descriptor
|
||||
# first_slice_segment_in_pic_flag u(1)
|
||||
# if( nal_unit_type >= BLA_W_LP && nal_unit_type <= RSV_IRAP_VCL23 )
|
||||
# no_output_of_prior_pics_flag u(1)
|
||||
# slice_pic_parameter_set_id ue(v)
|
||||
# if( !first_slice_segment_in_pic_flag ) {
|
||||
# if( dependent_slice_segments_enabled_flag )
|
||||
# dependent_slice_segment_flag u(1)
|
||||
# slice_segment_address u(v)
|
||||
# }
|
||||
# if( !dependent_slice_segment_flag ) {
|
||||
# for( i = 0; i < num_extra_slice_header_bits; i++ )
|
||||
# slice_reserved_flag[ i ] u(1)
|
||||
# slice_type ue(v)
|
||||
# ...
|
||||
|
||||
rbsp_start = nal_unit_start + NAL_UNIT_START_CODE_SIZE + NAL_UNIT_HEADER_SIZE
|
||||
skip_bits = 0
|
||||
|
||||
# 7.4.7.1 General slice segment header semantics
|
||||
# first_slice_segment_in_pic_flag equal to 1 specifies that the slice segment is the first slice segment of the picture in
|
||||
# decoding order. first_slice_segment_in_pic_flag equal to 0 specifies that the slice segment is not the first slice segment
|
||||
# of the picture in decoding order.
|
||||
is_first_slice = dat[rbsp_start] >> 7 & 1 == 1
|
||||
if not is_first_slice:
|
||||
# TODO: parse dependent_slice_segment_flag and slice_segment_address and get real slice_type
|
||||
# for now since we don't use it return -1 for slice_type
|
||||
return (-1, is_first_slice)
|
||||
skip_bits += 1 # skip past first_slice_segment_in_pic_flag
|
||||
|
||||
if nal_unit_type >= HevcNalUnitType.BLA_W_LP and nal_unit_type <= HevcNalUnitType.RSV_IRAP_VCL23:
|
||||
# 7.4.7.1 General slice segment header semantics
|
||||
# no_output_of_prior_pics_flag affects the output of previously-decoded pictures in the decoded picture buffer after the
|
||||
# decoding of an IDR or a BLA picture that is not the first picture in the bitstream as specified in Annex C.
|
||||
skip_bits += 1 # skip past no_output_of_prior_pics_flag
|
||||
|
||||
# 7.4.7.1 General slice segment header semantics
|
||||
# slice_pic_parameter_set_id specifies the value of pps_pic_parameter_set_id for the PPS in use.
|
||||
# The value of slice_pic_parameter_set_id shall be in the range of 0 to 63, inclusive.
|
||||
_, size = get_ue(dat, rbsp_start, skip_bits)
|
||||
skip_bits += size # skip past slice_pic_parameter_set_id
|
||||
|
||||
# 7.4.3.3.1 General picture parameter set RBSP semanal_unit_lenntics
|
||||
# num_extra_slice_header_bits specifies the number of extra slice header bits that are present in the slice header RBSP
|
||||
# for coded pictures referring to the PPS. The value of num_extra_slice_header_bits shall be in the range of 0 to 2, inclusive,
|
||||
# in bitstreams conforming to this version of this Specification. Other values for num_extra_slice_header_bits are reserved
|
||||
# for future use by ITU-T | ISO/IEC. However, decoders shall allow num_extra_slice_header_bits to have any value.
|
||||
# TODO: get from PPS_NUT pic_parameter_set_rbsp( ) for corresponding slice_pic_parameter_set_id
|
||||
num_extra_slice_header_bits = 0
|
||||
skip_bits += num_extra_slice_header_bits
|
||||
|
||||
# 7.4.7.1 General slice segment header semantics
|
||||
# slice_type specifies the coding type of the slice according to Table 7-7.
|
||||
# Table 7-7 - Name association to slice_type
|
||||
# slice_type | Name of slice_type
|
||||
# 0 | B (B slice)
|
||||
# 1 | P (P slice)
|
||||
# 2 | I (I slice)
|
||||
# unsigned integer 0-th order Exp-Golomb-coded syntax element with the left bit first
|
||||
slice_type, _ = get_ue(dat, rbsp_start, skip_bits)
|
||||
if DEBUG:
|
||||
print(" slice_type:", slice_type, f"(first slice: {is_first_slice})")
|
||||
if slice_type > 2:
|
||||
raise VideoFileInvalid("slice_type must be 0, 1, or 2")
|
||||
return slice_type, is_first_slice
|
||||
|
||||
def hevc_index(hevc_file_name: str, allow_corrupt: bool=False) -> tuple[list, int, bytes]:
|
||||
with FileReader(hevc_file_name) as f:
|
||||
dat = f.read()
|
||||
|
||||
if len(dat) < NAL_UNIT_START_CODE_SIZE + 1:
|
||||
raise VideoFileInvalid("data is too short")
|
||||
|
||||
if dat[0] != 0x00:
|
||||
raise VideoFileInvalid("first byte must be 0x00")
|
||||
|
||||
prefix_dat = b""
|
||||
frame_types = list()
|
||||
|
||||
i = 1 # skip past first byte 0x00
|
||||
try:
|
||||
while i < len(dat):
|
||||
require_nal_unit_start(dat, i)
|
||||
nal_unit_len = get_hevc_nal_unit_length(dat, i)
|
||||
nal_unit_type = get_hevc_nal_unit_type(dat, i)
|
||||
if nal_unit_type in HEVC_PARAMETER_SET_NAL_UNITS:
|
||||
prefix_dat += dat[i:i+nal_unit_len]
|
||||
elif nal_unit_type in HEVC_CODED_SLICE_SEGMENT_NAL_UNITS:
|
||||
slice_type, is_first_slice = get_hevc_slice_type(dat, i, nal_unit_type)
|
||||
if is_first_slice:
|
||||
frame_types.append((slice_type, i))
|
||||
i += nal_unit_len
|
||||
except Exception as e:
|
||||
if not allow_corrupt:
|
||||
raise
|
||||
print(f"ERROR: NAL unit skipped @ {i}\n", str(e))
|
||||
|
||||
return frame_types, len(dat), prefix_dat
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("input_file", type=str)
|
||||
parser.add_argument("output_prefix_file", type=str)
|
||||
parser.add_argument("output_index_file", type=str)
|
||||
args = parser.parse_args()
|
||||
|
||||
frame_types, dat_len, prefix_dat = hevc_index(args.input_file)
|
||||
with open(args.output_prefix_file, "wb") as f:
|
||||
f.write(prefix_dat)
|
||||
|
||||
with open(args.output_index_file, "wb") as f:
|
||||
for ft, fp in frame_types:
|
||||
f.write(struct.pack("<II", ft, fp))
|
||||
f.write(struct.pack("<II", 0xFFFFFFFF, dat_len))
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
1
tools/longitudinal_maneuvers/.gitignore
vendored
Normal file
1
tools/longitudinal_maneuvers/.gitignore
vendored
Normal file
@@ -0,0 +1 @@
|
||||
/longitudinal_reports/
|
||||
60
tools/longitudinal_maneuvers/README.md
Normal file
60
tools/longitudinal_maneuvers/README.md
Normal file
@@ -0,0 +1,60 @@
|
||||
# Longitudinal Maneuvers Testing Tool
|
||||
|
||||
Test your vehicle's longitudinal control tuning with this tool. The tool will test the vehicle's ability to follow a few longitudinal maneuvers and includes a tool to generate a report from the route.
|
||||
|
||||
<details><summary>Sample snapshot of a report.</summary><img width="600px" src="https://github.com/user-attachments/assets/d18d0c7d-2bde-44c1-8e86-1741ed442ad8"></details>
|
||||
|
||||
## Instructions
|
||||
|
||||
1. Check out a development branch such as `master-mici` on your device. The toggle is hidden on release branches.
|
||||
2. Locate either a large empty parking lot or road devoid of any car or foot traffic. Flat, straight road is preferred. The full maneuver suite can take 1 mile or more if left running, however it is recommended to disengage IQ.Pilot between maneuvers and turn around if there is not enough space.
|
||||
3. Turn off the vehicle and enable "Longitudinal Maneuver Mode" in Settings > Developer. The toggle requires IQ.Pilot longitudinal control and only enables while offroad. Alternatively, set the parameter manually:
|
||||
|
||||
```sh
|
||||
echo -n 1 > /data/params/d/LongitudinalManeuverMode
|
||||
```
|
||||
|
||||
4. Turn your vehicle back on. You will see the "Longitudinal Maneuver Mode" alert:
|
||||
|
||||
|
||||
|
||||
5. Ensure the road ahead is clear, as openpilot will not brake for any obstructions in this mode. Once you are ready, press "Set" on your steering wheel to start the tests. The tests will run for about 4 minutes. If you need to pause the tests, press "Cancel" on your steering wheel. You can resume the tests by pressing "Resume" on your steering wheel.
|
||||
|
||||
**Note:** For GM cars, it is recommended to hold down the resume button for all low-speed tests (starting, stopping and creep) to avoid the car entering standstill.
|
||||
|
||||
|
||||
|
||||
6. When the testing is complete, you'll see an alert that says "Maneuvers Finished." Complete the route by pulling over and turning off the vehicle.
|
||||
|
||||
|
||||
|
||||
7. Visit https://connect.comma.ai and locate the route(s). They will stand out with lots of orange intervals in their timeline. Ensure "All logs" show as "uploaded."
|
||||
|
||||
|
||||
|
||||
8. Gather the route ID and then run the report generator. The file will be exported to the same directory:
|
||||
|
||||
```sh
|
||||
$ python tools/longitudinal_maneuvers/generate_report.py 57048cfce01d9625/0000010e--5b26bc3be7 'pcm accel compensation'
|
||||
|
||||
processing report for LEXUS_ES_TSS2
|
||||
plotting maneuver: start from stop, runs: 4
|
||||
plotting maneuver: creep: alternate between +1m/s^2 and -1m/s^2, runs: 2
|
||||
plotting maneuver: gas step response: +1m/s^2 from 20mph, runs: 2
|
||||
|
||||
Report written to tools/longitudinal_maneuvers/longitudinal_reports/LEXUS_ES_TSS2_57048cfce01d9625_0000010e--5b26bc3be7.html
|
||||
```
|
||||
|
||||
`generate_report.py` also takes a path to a local `rlog.zst` or a directory of them.
|
||||
|
||||
## Testing the tooling without a car
|
||||
|
||||
`sim_maneuvers.py` runs `maneuversd` as a real process against a synthetic powertrain and writes an rlog
|
||||
that `generate_report.py` reads. Use it to verify the daemon and the report generator after changing either:
|
||||
|
||||
```sh
|
||||
$ python tools/longitudinal_maneuvers/sim_maneuvers.py --out /tmp/long/rlog.zst
|
||||
$ python tools/longitudinal_maneuvers/generate_report.py /tmp/long/rlog.zst
|
||||
```
|
||||
|
||||
The full suite takes about 4 minutes of wall clock; `--max-maneuvers N` stops early.
|
||||
187
tools/longitudinal_maneuvers/generate_report.py
Executable file
187
tools/longitudinal_maneuvers/generate_report.py
Executable file
@@ -0,0 +1,187 @@
|
||||
#!/usr/bin/env python3
|
||||
import argparse
|
||||
import base64
|
||||
import io
|
||||
import os
|
||||
import math
|
||||
import pprint
|
||||
import webbrowser
|
||||
from collections import defaultdict
|
||||
from pathlib import Path
|
||||
import matplotlib.pyplot as plt
|
||||
from openpilot.common.utils import tabulate
|
||||
|
||||
from openpilot.tools.lib.logreader import LogReader
|
||||
from openpilot.system.hardware.hw import Paths
|
||||
|
||||
|
||||
def format_car_params(CP):
|
||||
return pprint.pformat({k: v for k, v in CP.to_dict().items() if not k.endswith('DEPRECATED')}, indent=2)
|
||||
|
||||
|
||||
def report(platform, route, _description, CP, ID, maneuvers):
|
||||
output_path = Path(__file__).resolve().parent / "longitudinal_reports"
|
||||
output_fn = output_path / f"{platform}_{route.replace('/', '_')}.html"
|
||||
output_path.mkdir(exist_ok=True)
|
||||
target_cross_times = defaultdict(list)
|
||||
|
||||
builder = [
|
||||
"<style>summary { cursor: pointer; }\n td, th { padding: 8px; } </style>\n",
|
||||
"<h1>Longitudinal maneuver report</h1>\n",
|
||||
f"<h3>{platform}</h3>\n",
|
||||
f"<h3>{route}</h3>\n",
|
||||
f"<h3>{ID.gitCommit}, {ID.gitBranch}, {ID.gitRemote}</h3>\n",
|
||||
]
|
||||
if _description is not None:
|
||||
builder.append(f"<h3>Description: {_description}</h3>\n")
|
||||
builder.append(f"<details><summary><h3 style='display: inline-block;'>CarParams</h3></summary><pre>{format_car_params(CP)}</pre></details>\n")
|
||||
builder.append('{ summary }') # to be replaced below
|
||||
for description, runs in maneuvers:
|
||||
print(f'plotting maneuver: {description}, runs: {len(runs)}')
|
||||
builder.append("<div style='border-top: 1px solid #000; margin: 20px 0;'></div>\n")
|
||||
builder.append(f"<h2>{description}</h2>\n")
|
||||
for run, msgs in enumerate(runs):
|
||||
t_carControl, carControl = zip(*[(m.logMonoTime, m.carControl) for m in msgs if m.which() == 'carControl'], strict=True)
|
||||
t_carOutput, carOutput = zip(*[(m.logMonoTime, m.carOutput) for m in msgs if m.which() == 'carOutput'], strict=True)
|
||||
t_carState, carState = zip(*[(m.logMonoTime, m.carState) for m in msgs if m.which() == 'carState'], strict=True)
|
||||
t_livePose, livePose = zip(*[(m.logMonoTime, m.livePose) for m in msgs if m.which() == 'livePose'], strict=True)
|
||||
t_longitudinalPlan, longitudinalPlan = zip(*[(m.logMonoTime, m.longitudinalPlan) for m in msgs if m.which() == 'longitudinalPlan'], strict=True)
|
||||
|
||||
# make time relative seconds
|
||||
t_carControl = [(t - t_carControl[0]) / 1e9 for t in t_carControl]
|
||||
t_carOutput = [(t - t_carOutput[0]) / 1e9 for t in t_carOutput]
|
||||
t_carState = [(t - t_carState[0]) / 1e9 for t in t_carState]
|
||||
t_livePose = [(t - t_livePose[0]) / 1e9 for t in t_livePose]
|
||||
t_longitudinalPlan = [(t - t_longitudinalPlan[0]) / 1e9 for t in t_longitudinalPlan]
|
||||
|
||||
# maneuver validity
|
||||
longActive = [m.longActive for m in carControl]
|
||||
maneuver_valid = all(longActive) and (not any(cs.cruiseState.standstill for cs in carState) or CP.autoResumeSng)
|
||||
|
||||
_open = 'open' if maneuver_valid else ''
|
||||
title = f'Run #{int(run)+1}' + (' <span style="color: red">(invalid maneuver!)</span>' if not maneuver_valid else '')
|
||||
|
||||
builder.append(f"<details {_open}><summary><h3 style='display: inline-block;'>{title}</h3></summary>\n")
|
||||
|
||||
# get first acceleration target and first intersection
|
||||
aTarget = longitudinalPlan[0].aTarget
|
||||
target_cross_time = None
|
||||
builder.append(f'<h3 style="font-weight: normal">Initial aTarget: {round(aTarget, 2)} m/s^2')
|
||||
|
||||
# Localizer is noisy, require two consecutive 20Hz frames above threshold
|
||||
prev_crossed = False
|
||||
for t, lp in zip(t_livePose, livePose, strict=True):
|
||||
crossed = (0 < aTarget < lp.accelerationDevice.x) or (0 > aTarget > lp.accelerationDevice.x)
|
||||
if crossed and prev_crossed:
|
||||
builder.append(f', <strong>crossed in {t:.3f}s</strong>')
|
||||
target_cross_time = t
|
||||
if maneuver_valid:
|
||||
target_cross_times[description].append(t)
|
||||
break
|
||||
prev_crossed = crossed
|
||||
else:
|
||||
builder.append(', <strong>not crossed</strong>')
|
||||
builder.append('</h3>')
|
||||
|
||||
pitches = [math.degrees(m.orientationNED[1]) for m in carControl]
|
||||
builder.append(f'<h3 style="font-weight: normal">Average pitch: <strong>{sum(pitches) / len(pitches):0.2f} degrees</strong></h3>')
|
||||
|
||||
plt.rcParams['font.size'] = 40
|
||||
fig = plt.figure(figsize=(30, 26))
|
||||
ax = fig.subplots(4, 1, sharex=True, gridspec_kw={'height_ratios': [5, 3, 1, 1]})
|
||||
|
||||
ax[0].grid(linewidth=4)
|
||||
ax[0].plot(t_carControl, [m.actuators.accel for m in carControl], label='carControl.actuators.accel', linewidth=6)
|
||||
ax[0].plot(t_carOutput, [m.actuatorsOutput.accel for m in carOutput], label='carOutput.actuatorsOutput.accel', linewidth=6)
|
||||
ax[0].plot(t_longitudinalPlan, [m.aTarget for m in longitudinalPlan], label='longitudinalPlan.aTarget', linewidth=6)
|
||||
ax[0].plot(t_carState, [m.aEgo for m in carState], label='carState.aEgo', linewidth=6)
|
||||
ax[0].plot(t_livePose, [m.accelerationDevice.x for m in livePose], label='livePose.accelerationDevice.x', linewidth=6)
|
||||
# TODO localizer accel
|
||||
ax[0].set_ylabel('Acceleration (m/s^2)')
|
||||
#ax[0].set_ylim(-6.5, 6.5)
|
||||
ax[0].legend(prop={'size': 30})
|
||||
|
||||
if target_cross_time is not None:
|
||||
ax[0].plot(target_cross_time, aTarget, marker='o', markersize=50, markeredgewidth=7, markeredgecolor='black', markerfacecolor='None')
|
||||
|
||||
ax[1].grid(linewidth=4)
|
||||
ax[1].plot(t_carState, [m.vEgo for m in carState], 'g', label='vEgo', linewidth=6)
|
||||
ax[1].set_ylabel('Velocity (m/s)')
|
||||
ax[1].legend()
|
||||
|
||||
ax[2].plot(t_carControl, longActive, label='longActive', linewidth=6)
|
||||
ax[3].plot(t_carState, [m.gasPressed for m in carState], label='gasPressed', linewidth=6)
|
||||
ax[3].plot(t_carState, [m.brakePressed for m in carState], label='brakePressed', linewidth=6)
|
||||
for i in (2, 3):
|
||||
ax[i].set_yticks([0, 1], minor=False)
|
||||
ax[i].set_ylim(-1, 2)
|
||||
ax[i].legend()
|
||||
|
||||
ax[-1].set_xlabel("Time (s)")
|
||||
fig.tight_layout()
|
||||
|
||||
buffer = io.BytesIO()
|
||||
fig.savefig(buffer, format='webp')
|
||||
plt.close(fig)
|
||||
buffer.seek(0)
|
||||
builder.append(f"<img src='data:image/webp;base64,{base64.b64encode(buffer.getvalue()).decode()}' style='width:100%; max-width:800px;'>\n")
|
||||
builder.append("</details>\n")
|
||||
|
||||
summary = ["<h2>Summary</h2>\n"]
|
||||
cols = ['maneuver', 'crossed', 'runs', 'mean', 'min', 'max']
|
||||
table = []
|
||||
for description, runs in maneuvers:
|
||||
times = target_cross_times[description]
|
||||
l = [description, len(times), len(runs)]
|
||||
if len(times):
|
||||
l.extend([round(sum(times) / len(times), 2), round(min(times), 2), round(max(times), 2)])
|
||||
table.append(l)
|
||||
summary.append(tabulate(table, headers=cols, tablefmt='html', numalign='left') + '\n')
|
||||
|
||||
sum_idx = builder.index('{ summary }')
|
||||
builder[sum_idx:sum_idx + 1] = summary
|
||||
|
||||
with open(output_fn, "w") as f:
|
||||
f.write(''.join(builder))
|
||||
|
||||
print(f"\nOpening report: {output_fn}\n")
|
||||
webbrowser.open_new_tab(str(output_fn))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
parser = argparse.ArgumentParser(description='Generate longitudinal maneuver report from route')
|
||||
parser.add_argument('route', type=str, help='Route name (e.g. 00000000--5f742174be)')
|
||||
parser.add_argument('description', type=str, nargs='?')
|
||||
|
||||
args = parser.parse_args()
|
||||
|
||||
if '/' in args.route or '|' in args.route:
|
||||
lr = LogReader(args.route)
|
||||
else:
|
||||
segs = [seg for seg in os.listdir(Paths.log_root()) if args.route in seg]
|
||||
lr = LogReader([os.path.join(Paths.log_root(), seg, 'rlog.zst') for seg in segs])
|
||||
|
||||
CP = lr.first('carParams')
|
||||
ID = lr.first('initData')
|
||||
platform = CP.carFingerprint
|
||||
print('processing report for', platform)
|
||||
|
||||
maneuvers: list[tuple[str, list[list]]] = []
|
||||
active_prev = False
|
||||
description_prev = None
|
||||
|
||||
for msg in lr:
|
||||
if msg.which() == 'alertDebug':
|
||||
active = 'Maneuver Active' in msg.alertDebug.alertText1
|
||||
if active and not active_prev:
|
||||
if msg.alertDebug.alertText2 == description_prev:
|
||||
maneuvers[-1][1].append([])
|
||||
else:
|
||||
maneuvers.append((msg.alertDebug.alertText2, [[]]))
|
||||
description_prev = maneuvers[-1][0]
|
||||
active_prev = active
|
||||
|
||||
if active_prev:
|
||||
maneuvers[-1][1][-1].append(msg)
|
||||
|
||||
report(platform, args.route, args.description, CP, ID, maneuvers)
|
||||
18
tools/longitudinal_maneuvers/maneuver_helpers.py
Normal file
18
tools/longitudinal_maneuvers/maneuver_helpers.py
Normal file
@@ -0,0 +1,18 @@
|
||||
from enum import IntEnum
|
||||
|
||||
class Axis(IntEnum):
|
||||
TIME = 0
|
||||
EGO_POSITION = 1
|
||||
LEAD_DISTANCE= 2
|
||||
EGO_V = 3
|
||||
LEAD_V = 4
|
||||
EGO_A = 5
|
||||
D_REL = 6
|
||||
|
||||
axis_labels = {Axis.TIME: 'Time (s)',
|
||||
Axis.EGO_POSITION: 'Ego position (m)',
|
||||
Axis.LEAD_DISTANCE: 'Lead absolute position (m)',
|
||||
Axis.EGO_V: 'Ego Velocity (m/s)',
|
||||
Axis.LEAD_V: 'Lead Velocity (m/s)',
|
||||
Axis.EGO_A: 'Ego acceleration (m/s^2)',
|
||||
Axis.D_REL: 'Lead distance (m)'}
|
||||
200
tools/longitudinal_maneuvers/maneuversd.py
Executable file
200
tools/longitudinal_maneuvers/maneuversd.py
Executable file
@@ -0,0 +1,200 @@
|
||||
#!/usr/bin/env python3
|
||||
import numpy as np
|
||||
from dataclasses import dataclass
|
||||
|
||||
from cereal import messaging
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import should_stop
|
||||
|
||||
|
||||
@dataclass
|
||||
class Action:
|
||||
accel_bp: list[float] # m/s^2
|
||||
time_bp: list[float] # seconds
|
||||
|
||||
def __post_init__(self):
|
||||
assert len(self.accel_bp) == len(self.time_bp)
|
||||
|
||||
|
||||
@dataclass
|
||||
class Maneuver:
|
||||
description: str
|
||||
actions: list[Action]
|
||||
repeat: int = 0
|
||||
initial_speed: float = 0. # m/s
|
||||
|
||||
_active: bool = False
|
||||
_finished: bool = False
|
||||
_run_completed: bool = False
|
||||
_action_index: int = 0
|
||||
_action_frames: int = 0
|
||||
_ready_cnt: int = 0
|
||||
_repeated: int = 0
|
||||
|
||||
def _step(self) -> float:
|
||||
self._run_completed = False
|
||||
action = self.actions[self._action_index]
|
||||
action_accel = np.interp(self._action_frames * DT_MDL, action.time_bp, action.accel_bp)
|
||||
|
||||
self._action_frames += 1
|
||||
|
||||
# reached duration of action
|
||||
if self._action_frames > (action.time_bp[-1] / DT_MDL):
|
||||
# next action
|
||||
if self._action_index < len(self.actions) - 1:
|
||||
self._action_index += 1
|
||||
self._action_frames = 0
|
||||
# repeat maneuver
|
||||
elif self._repeated < self.repeat:
|
||||
self._repeated += 1
|
||||
self._run_completed = True
|
||||
self.reset()
|
||||
# finish maneuver
|
||||
else:
|
||||
self._run_completed = True
|
||||
self._finished = True
|
||||
|
||||
return float(action_accel)
|
||||
|
||||
def get_accel(self, v_ego: float, long_active: bool, standstill: bool, cruise_standstill: bool) -> float:
|
||||
ready = abs(v_ego - self.initial_speed) < 0.3 and long_active and not cruise_standstill
|
||||
if self.initial_speed < 0.01:
|
||||
ready = ready and standstill
|
||||
self._ready_cnt = (self._ready_cnt + 1) if ready else 0
|
||||
|
||||
if self._ready_cnt > (3. / DT_MDL):
|
||||
self._active = True
|
||||
|
||||
if not self._active:
|
||||
return min(max(self.initial_speed - v_ego, -2.), 2.)
|
||||
|
||||
return self._step()
|
||||
|
||||
def reset(self):
|
||||
self._active = False
|
||||
self._action_frames = 0
|
||||
self._action_index = 0
|
||||
|
||||
@property
|
||||
def finished(self):
|
||||
return self._finished
|
||||
|
||||
@property
|
||||
def active(self):
|
||||
return self._active
|
||||
|
||||
|
||||
MANEUVERS = [
|
||||
Maneuver(
|
||||
"come to stop",
|
||||
[Action([-0.5], [12])],
|
||||
repeat=2,
|
||||
initial_speed=5.,
|
||||
),
|
||||
Maneuver(
|
||||
"start from stop",
|
||||
[Action([1.5], [6])],
|
||||
repeat=2,
|
||||
initial_speed=0.,
|
||||
),
|
||||
Maneuver(
|
||||
"creep: alternate between +1m/s^2 and -1m/s^2",
|
||||
[
|
||||
Action([1], [3]), Action([-1], [3]),
|
||||
Action([1], [3]), Action([-1], [3]),
|
||||
Action([1], [3]), Action([-1], [3]),
|
||||
],
|
||||
repeat=2,
|
||||
initial_speed=0.,
|
||||
),
|
||||
Maneuver(
|
||||
"brake step response: -1m/s^2 from 20mph",
|
||||
[Action([-1], [3])],
|
||||
repeat=2,
|
||||
initial_speed=20. * CV.MPH_TO_MS,
|
||||
),
|
||||
Maneuver(
|
||||
"brake step response: -4m/s^2 from 20mph",
|
||||
[Action([-4], [3])],
|
||||
repeat=2,
|
||||
initial_speed=20. * CV.MPH_TO_MS,
|
||||
),
|
||||
Maneuver(
|
||||
"gas step response: +1m/s^2 from 20mph",
|
||||
[Action([1], [3])],
|
||||
repeat=2,
|
||||
initial_speed=20. * CV.MPH_TO_MS,
|
||||
),
|
||||
Maneuver(
|
||||
"gas step response: +4m/s^2 from 20mph",
|
||||
[Action([4], [3])],
|
||||
repeat=2,
|
||||
initial_speed=20. * CV.MPH_TO_MS,
|
||||
),
|
||||
]
|
||||
|
||||
|
||||
def main():
|
||||
params = Params()
|
||||
cloudlog.info("maneuversd is waiting for CarParams")
|
||||
params.get("CarParams", block=True)
|
||||
|
||||
sm = messaging.SubMaster(['carState', 'carControl', 'controlsState', 'selfdriveState', 'modelV2'], poll='modelV2')
|
||||
pm = messaging.PubMaster(['longitudinalPlan', 'iqPlan', 'driverAssistance', 'alertDebug'])
|
||||
|
||||
maneuvers = iter(MANEUVERS)
|
||||
maneuver = None
|
||||
|
||||
while True:
|
||||
sm.update()
|
||||
|
||||
if maneuver is None:
|
||||
maneuver = next(maneuvers, None)
|
||||
|
||||
alert_msg = messaging.new_message('alertDebug')
|
||||
alert_msg.valid = True
|
||||
|
||||
plan_send = messaging.new_message('longitudinalPlan')
|
||||
plan_send.valid = sm.all_checks()
|
||||
|
||||
longitudinalPlan = plan_send.longitudinalPlan
|
||||
accel = 0
|
||||
v_ego = max(sm['carState'].vEgo, 0)
|
||||
|
||||
if maneuver is not None:
|
||||
accel = maneuver.get_accel(v_ego, sm['carControl'].longActive, sm['carState'].standstill, sm['carState'].cruiseState.standstill)
|
||||
|
||||
if maneuver.active:
|
||||
alert_msg.alertDebug.alertText1 = f'Maneuver Active: {accel:0.2f} m/s^2'
|
||||
else:
|
||||
alert_msg.alertDebug.alertText1 = f'Setting up to {maneuver.initial_speed * CV.MS_TO_MPH:0.2f} mph'
|
||||
alert_msg.alertDebug.alertText2 = f'{maneuver.description}'
|
||||
else:
|
||||
alert_msg.alertDebug.alertText1 = 'Maneuvers Finished'
|
||||
|
||||
pm.send('alertDebug', alert_msg)
|
||||
|
||||
longitudinalPlan.aTarget = accel
|
||||
longitudinalPlan.shouldStop = should_stop(v_ego, accel)
|
||||
|
||||
longitudinalPlan.allowBrake = True
|
||||
longitudinalPlan.allowThrottle = True
|
||||
longitudinalPlan.hasLead = True
|
||||
|
||||
longitudinalPlan.speeds = [0.2] # triggers carControl.cruiseControl.resume in controlsd
|
||||
|
||||
pm.send('longitudinalPlan', plan_send)
|
||||
|
||||
plan_iq_send = messaging.new_message('iqPlan')
|
||||
plan_iq_send.valid = True
|
||||
pm.send('iqPlan', plan_iq_send)
|
||||
|
||||
assistance_send = messaging.new_message('driverAssistance')
|
||||
assistance_send.valid = True
|
||||
pm.send('driverAssistance', assistance_send)
|
||||
|
||||
if maneuver is not None and maneuver.finished:
|
||||
maneuver = None
|
||||
294
tools/longitudinal_maneuvers/mpc_longitudinal_tuning_report.py
Normal file
294
tools/longitudinal_maneuvers/mpc_longitudinal_tuning_report.py
Normal file
@@ -0,0 +1,294 @@
|
||||
import io
|
||||
import sys
|
||||
import markdown
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.controls.tests.test_following_distance import desired_follow_distance
|
||||
from openpilot.tools.longitudinal_maneuvers.maneuver_helpers import Axis, axis_labels
|
||||
from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
|
||||
|
||||
|
||||
def get_html_from_results(results, labels, AXIS):
|
||||
fig, ax = plt.subplots(figsize=(16, 8))
|
||||
for idx, key in enumerate(results.keys()):
|
||||
ax.plot(results[key][:, Axis.TIME], results[key][:, AXIS], label=labels[idx])
|
||||
|
||||
ax.set_xlabel(axis_labels[Axis.TIME])
|
||||
ax.set_ylabel(axis_labels[AXIS])
|
||||
ax.legend(bbox_to_anchor=(1.02, 1), loc='upper left', borderaxespad=0)
|
||||
ax.grid(True, linestyle='--', alpha=0.7)
|
||||
ax.text(-0.075, 0.5, '.', transform=ax.transAxes, color='none')
|
||||
|
||||
fig_buffer = io.StringIO()
|
||||
fig.savefig(fig_buffer, format='svg', bbox_inches='tight')
|
||||
plt.close(fig)
|
||||
return fig_buffer.getvalue() + '<br/>'
|
||||
|
||||
|
||||
def generate_mpc_tuning_report():
|
||||
htmls = []
|
||||
|
||||
results = {}
|
||||
name = 'Resuming behind lead'
|
||||
labels = []
|
||||
for lead_accel in np.linspace(1.0, 4.0, 4):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=11,
|
||||
initial_speed=0.0,
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=desired_follow_distance(0.0, 0.0),
|
||||
speed_lead_values=[0.0, 10 * lead_accel],
|
||||
cruise_values=[100, 100],
|
||||
prob_lead_values=[1.0, 1.0],
|
||||
breakpoints=[1., 11],
|
||||
)
|
||||
valid, results[lead_accel] = man.evaluate()
|
||||
labels.append(f'{lead_accel} m/s^2 lead acceleration')
|
||||
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_V))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_A))
|
||||
|
||||
|
||||
results = {}
|
||||
name = 'Approaching stopped car from 140m'
|
||||
labels = []
|
||||
for speed in np.arange(0, 45, 5):
|
||||
man = Maneuver(
|
||||
name,
|
||||
duration=30.,
|
||||
initial_speed=float(speed),
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=140.,
|
||||
speed_lead_values=[0.0, 0.],
|
||||
breakpoints=[0., 30.],
|
||||
)
|
||||
valid, results[speed] = man.evaluate()
|
||||
labels.append(f'{speed} m/s approach speed')
|
||||
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_A))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.D_REL))
|
||||
|
||||
|
||||
results = {}
|
||||
name = 'Following 5s (triangular) oscillating lead'
|
||||
labels = []
|
||||
speed = np.int64(10)
|
||||
for oscil in np.arange(0, 10, 1):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=30.,
|
||||
initial_speed=float(speed),
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=desired_follow_distance(speed, speed),
|
||||
speed_lead_values=[speed, speed, speed - oscil, speed + oscil, speed - oscil, speed + oscil, speed - oscil],
|
||||
breakpoints=[0., 2., 5, 8, 15, 18, 25.],
|
||||
)
|
||||
valid, results[oscil] = man.evaluate()
|
||||
labels.append(f'{oscil} m/s oscillation size')
|
||||
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.D_REL))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_V))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_A))
|
||||
|
||||
|
||||
results = {}
|
||||
name = 'Following 5s (sinusoidal) oscillating lead'
|
||||
labels = []
|
||||
speed = np.int64(10)
|
||||
duration = float(30)
|
||||
f_osc = 1. / 5
|
||||
for oscil in np.arange(0, 10, 1):
|
||||
bps = DT_MDL * np.arange(int(duration / DT_MDL))
|
||||
lead_speeds = speed + oscil * np.sin(2 * np.pi * f_osc * bps)
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=duration,
|
||||
initial_speed=float(speed),
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=desired_follow_distance(speed, speed),
|
||||
speed_lead_values=lead_speeds,
|
||||
breakpoints=bps,
|
||||
)
|
||||
valid, results[oscil] = man.evaluate()
|
||||
labels.append(f'{oscil} m/s oscillation size')
|
||||
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.D_REL))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_V))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_A))
|
||||
|
||||
|
||||
results = {}
|
||||
name = 'Speed profile when converging to steady state lead at 30m/s'
|
||||
labels = []
|
||||
for distance in np.arange(20, 140, 10):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=50,
|
||||
initial_speed=30.0,
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=distance,
|
||||
speed_lead_values=[30.0],
|
||||
breakpoints=[0.],
|
||||
)
|
||||
valid, results[distance] = man.evaluate()
|
||||
labels.append(f'{distance} m initial distance')
|
||||
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_V))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.D_REL))
|
||||
|
||||
|
||||
results = {}
|
||||
name = 'Speed profile when converging to steady state lead at 20m/s'
|
||||
labels = []
|
||||
for distance in np.arange(20, 140, 10):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=50,
|
||||
initial_speed=20.0,
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=distance,
|
||||
speed_lead_values=[20.0],
|
||||
breakpoints=[0.],
|
||||
)
|
||||
valid, results[distance] = man.evaluate()
|
||||
labels.append(f'{distance} m initial distance')
|
||||
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_V))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.D_REL))
|
||||
|
||||
|
||||
results = {}
|
||||
name = 'Following car at 30m/s that comes to a stop'
|
||||
labels = []
|
||||
for stop_time in np.arange(4, 14, 1):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=30,
|
||||
initial_speed=30.0,
|
||||
cruise_values=[30.0, 30.0, 30.0],
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=60.0,
|
||||
speed_lead_values=[30.0, 30.0, 0.0],
|
||||
breakpoints=[0., 5., 5 + stop_time],
|
||||
)
|
||||
valid, results[stop_time] = man.evaluate()
|
||||
labels.append(f'{stop_time} seconds stop time')
|
||||
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_A))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.D_REL))
|
||||
|
||||
|
||||
results = {}
|
||||
name = 'Response to cut-in at half follow distance'
|
||||
labels = []
|
||||
for speed in np.arange(0, 40, 5):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=20,
|
||||
initial_speed=float(speed),
|
||||
cruise_values=[speed, speed, speed],
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=desired_follow_distance(speed, speed) / 2,
|
||||
speed_lead_values=[speed, speed, speed],
|
||||
prob_lead_values=[0.0, 0.0, 1.0],
|
||||
breakpoints=[0., 5.0, 5.01],
|
||||
)
|
||||
valid, results[speed] = man.evaluate()
|
||||
labels.append(f'{speed} m/s speed')
|
||||
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_A))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.D_REL))
|
||||
|
||||
|
||||
results = {}
|
||||
name = 'Follow a lead that accelerates at 2m/s^2 until steady state speed'
|
||||
labels = []
|
||||
for speed in np.arange(0, 40, 5):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=60,
|
||||
initial_speed=0.0,
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=desired_follow_distance(0.0, 0.0),
|
||||
speed_lead_values=[0.0, 0.0, speed],
|
||||
prob_lead_values=[1.0, 1.0, 1.0],
|
||||
breakpoints=[0., 1.0, speed / 2],
|
||||
)
|
||||
valid, results[speed] = man.evaluate()
|
||||
labels.append(f'{speed} m/s speed')
|
||||
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_V))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_A))
|
||||
|
||||
|
||||
results = {}
|
||||
name = 'From stop to cruise'
|
||||
labels = []
|
||||
for speed in np.arange(0, 40, 5):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=50,
|
||||
initial_speed=0.0,
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=desired_follow_distance(0.0, 0.0),
|
||||
speed_lead_values=[0.0, 0.0],
|
||||
cruise_values=[0.0, speed],
|
||||
prob_lead_values=[0.0, 0.0],
|
||||
breakpoints=[1., 1.01],
|
||||
)
|
||||
valid, results[speed] = man.evaluate()
|
||||
labels.append(f'{speed} m/s speed')
|
||||
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_V))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_A))
|
||||
|
||||
|
||||
results = {}
|
||||
name = 'From cruise to min'
|
||||
labels = []
|
||||
for speed in np.arange(10, 40, 5):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=50,
|
||||
initial_speed=float(speed),
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=desired_follow_distance(0.0, 0.0),
|
||||
speed_lead_values=[0.0, 0.0],
|
||||
cruise_values=[speed, 10.0],
|
||||
prob_lead_values=[0.0, 0.0],
|
||||
breakpoints=[1., 1.01],
|
||||
)
|
||||
valid, results[speed] = man.evaluate()
|
||||
labels.append(f'{speed} m/s speed')
|
||||
|
||||
htmls.append(markdown.markdown('# ' + name))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_V))
|
||||
htmls.append(get_html_from_results(results, labels, Axis.EGO_A))
|
||||
|
||||
return htmls
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
htmls = generate_mpc_tuning_report()
|
||||
|
||||
if len(sys.argv) < 2:
|
||||
file_name = 'long_mpc_tune_report.html'
|
||||
else:
|
||||
file_name = sys.argv[1]
|
||||
|
||||
with open(file_name, 'w') as f:
|
||||
f.write(markdown.markdown('# MPC longitudinal tuning report'))
|
||||
for html in htmls:
|
||||
f.write(html)
|
||||
240
tools/longitudinal_maneuvers/sim_harness.py
Normal file
240
tools/longitudinal_maneuvers/sim_harness.py
Normal file
@@ -0,0 +1,240 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Closed-loop offline harness for the maneuver daemons.
|
||||
|
||||
Runs maneuversd / lateral_maneuversd as real subprocesses over msgq, drives them with a
|
||||
synthetic vehicle, and records every message to an rlog that generate_report.py can read.
|
||||
Used to validate the maneuver tooling without a car.
|
||||
"""
|
||||
import math
|
||||
import os
|
||||
import signal
|
||||
import subprocess
|
||||
import sys
|
||||
import time
|
||||
from pathlib import Path
|
||||
from typing import NamedTuple
|
||||
|
||||
import numpy as np
|
||||
import zstandard as zstd
|
||||
|
||||
from cereal import car, messaging
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_CTRL, Ratekeeper
|
||||
from openpilot.common.basedir import BASEDIR
|
||||
|
||||
PUB_100HZ = ('carState', 'carControl', 'carOutput', 'controlsState', 'selfdriveState')
|
||||
PUB_20HZ = ('modelV2', 'livePose', 'liveParameters')
|
||||
SUB = ('alertDebug', 'longitudinalPlan', 'lateralManeuverPlan')
|
||||
|
||||
STEER_RATIO = 15.0
|
||||
WHEELBASE = 2.78
|
||||
|
||||
|
||||
class LongPlan(NamedTuple):
|
||||
aTarget: float
|
||||
shouldStop: bool
|
||||
|
||||
|
||||
class LatPlan(NamedTuple):
|
||||
desiredCurvature: float
|
||||
|
||||
|
||||
class Plant:
|
||||
"""Vehicle model. Subclasses consume the daemon's plan and fill the published messages."""
|
||||
|
||||
sim = None
|
||||
PLAN = 'longitudinalPlan'
|
||||
|
||||
def __init__(self, v_ego: float = 0.0):
|
||||
self.v_ego = v_ego
|
||||
self.a_ego = 0.0
|
||||
self.curvature = 0.0 # commanded, controlsState.desiredCurvature
|
||||
self.achieved_curvature = 0.0 # measured, controlsState.curvature
|
||||
self.lat_accel = 0.0
|
||||
self.long_active = True
|
||||
self.lat_active = True
|
||||
|
||||
def step(self, dt: float, plan) -> None:
|
||||
raise NotImplementedError
|
||||
|
||||
def _angle(self, curvature: float) -> float:
|
||||
return math.degrees(curvature * WHEELBASE * STEER_RATIO)
|
||||
|
||||
def _torque(self, curvature: float) -> float:
|
||||
return float(np.clip(curvature * max(self.v_ego, 1.0) ** 2 / 3.0, -1.0, 1.0))
|
||||
|
||||
def fill_car_state(self, cs) -> None:
|
||||
cs.vEgo = float(self.v_ego)
|
||||
cs.vEgoRaw = float(self.v_ego)
|
||||
cs.vEgoCluster = float(self.v_ego)
|
||||
cs.aEgo = float(self.a_ego)
|
||||
cs.standstill = self.v_ego < 0.01
|
||||
cs.steeringAngleDeg = self._angle(self.achieved_curvature)
|
||||
cs.cruiseState.enabled = True
|
||||
cs.cruiseState.available = True
|
||||
cs.cruiseState.speed = float(max(self.v_ego, 1.0))
|
||||
|
||||
def fill_car_control(self, cc) -> None:
|
||||
cc.enabled = True
|
||||
cc.latActive = self.lat_active
|
||||
cc.longActive = self.long_active
|
||||
cc.orientationNED = [0.0, 0.0, 0.0]
|
||||
cc.actuators.curvature = float(self.curvature)
|
||||
cc.actuators.accel = float(self.a_ego)
|
||||
cc.actuators.steeringAngleDeg = self._angle(self.curvature)
|
||||
cc.actuators.torque = self._torque(self.curvature)
|
||||
|
||||
|
||||
class ManeuverSim:
|
||||
def __init__(self, module: str, plant: Plant, fingerprint: str = "TOYOTA_SIENNA",
|
||||
max_maneuvers: int = 0, timeout: float = 600.0, verbose: bool = True):
|
||||
self.module = module
|
||||
self.plant = plant
|
||||
plant.sim = self
|
||||
self.fingerprint = fingerprint
|
||||
self.max_maneuvers = max_maneuvers
|
||||
self.timeout = timeout
|
||||
self.verbose = verbose
|
||||
|
||||
self.events: list[bytes] = []
|
||||
self.alert1 = ''
|
||||
self.alert2 = ''
|
||||
self.seen_maneuvers: list[str] = []
|
||||
self.finished = False
|
||||
|
||||
def _write_car_params(self):
|
||||
CP = car.CarParams.new_message()
|
||||
CP.carFingerprint = self.fingerprint
|
||||
CP.brand = "toyota"
|
||||
CP.openpilotLongitudinalControl = True
|
||||
CP.autoResumeSng = True
|
||||
CP.steerRatio = STEER_RATIO
|
||||
CP.wheelbase = WHEELBASE
|
||||
Params().put("CarParams", CP.to_bytes())
|
||||
return CP
|
||||
|
||||
def _head_events(self, CP):
|
||||
init = messaging.new_message('initData')
|
||||
init.valid = True
|
||||
init.initData.gitCommit = "simulated"
|
||||
init.initData.gitBranch = "sim"
|
||||
init.initData.gitRemote = "iqpilot-sim"
|
||||
self.events.append(init.to_bytes())
|
||||
|
||||
cpm = messaging.new_message('carParams')
|
||||
cpm.valid = True
|
||||
cpm.carParams = CP
|
||||
self.events.append(cpm.to_bytes())
|
||||
|
||||
def _launch(self):
|
||||
env = dict(os.environ)
|
||||
env["PYTHONPATH"] = str(BASEDIR) + os.pathsep + env.get("PYTHONPATH", "")
|
||||
return subprocess.Popen([sys.executable, "-c", f"from {self.module} import main; main()"],
|
||||
cwd=str(BASEDIR), env=env, start_new_session=True)
|
||||
|
||||
def _on_alert(self, ad):
|
||||
text1, text2 = ad.alertText1, ad.alertText2
|
||||
if (text1, text2) != (self.alert1, self.alert2):
|
||||
if self.verbose:
|
||||
print(f" [{time.monotonic() - self.t_start:6.1f}s] {text1!r} | {text2!r}")
|
||||
if text2 and text2 not in self.seen_maneuvers:
|
||||
self.seen_maneuvers.append(text2)
|
||||
if text1 == 'Maneuvers Finished':
|
||||
self.finished = True
|
||||
self.alert1, self.alert2 = text1, text2
|
||||
|
||||
def run(self, out: Path) -> Path:
|
||||
self._head_events(self._write_car_params())
|
||||
|
||||
pm = messaging.PubMaster(list(PUB_100HZ) + list(PUB_20HZ))
|
||||
socks = {s: messaging.sub_sock(s, conflate=False, timeout=0) for s in SUB}
|
||||
|
||||
proc = self._launch()
|
||||
self.t_start = time.monotonic()
|
||||
rk = Ratekeeper(int(1.0 / DT_CTRL), print_delay_threshold=None)
|
||||
|
||||
plans: dict[str, object | None] = {'longitudinalPlan': None, 'lateralManeuverPlan': None}
|
||||
frame = 0
|
||||
try:
|
||||
while True:
|
||||
for s, sock in socks.items():
|
||||
while True:
|
||||
raw = sock.receive(non_blocking=True)
|
||||
if raw is None:
|
||||
break
|
||||
self.events.append(raw)
|
||||
evt = messaging.log_from_bytes(raw)
|
||||
if s == 'alertDebug':
|
||||
self._on_alert(evt.alertDebug)
|
||||
elif s == 'longitudinalPlan':
|
||||
plans[s] = LongPlan(evt.longitudinalPlan.aTarget, evt.longitudinalPlan.shouldStop)
|
||||
elif s == 'lateralManeuverPlan':
|
||||
plans[s] = LatPlan(evt.lateralManeuverPlan.desiredCurvature) if evt.valid else None
|
||||
|
||||
self.plant.step(DT_CTRL, plans[self.plant.PLAN])
|
||||
|
||||
for s in PUB_100HZ:
|
||||
raw = self._build(s).to_bytes()
|
||||
self.events.append(raw)
|
||||
pm.send(s, raw)
|
||||
|
||||
if frame % 5 == 0:
|
||||
for s in PUB_20HZ:
|
||||
raw = self._build(s).to_bytes()
|
||||
self.events.append(raw)
|
||||
pm.send(s, raw)
|
||||
|
||||
frame += 1
|
||||
if self.finished:
|
||||
break
|
||||
if self.max_maneuvers and len(self.seen_maneuvers) > self.max_maneuvers:
|
||||
break
|
||||
if time.monotonic() - self.t_start > self.timeout:
|
||||
print(" timed out")
|
||||
break
|
||||
rk.keep_time()
|
||||
finally:
|
||||
if proc.poll() is None:
|
||||
os.killpg(os.getpgid(proc.pid), signal.SIGTERM)
|
||||
proc.wait(timeout=5)
|
||||
for sock in socks.values():
|
||||
del sock
|
||||
|
||||
out.parent.mkdir(parents=True, exist_ok=True)
|
||||
out.write_bytes(zstd.compress(b"".join(self.events), 10))
|
||||
return out
|
||||
|
||||
def _build(self, s: str):
|
||||
msg = messaging.new_message(s)
|
||||
msg.valid = True
|
||||
if s == 'carState':
|
||||
self.plant.fill_car_state(msg.carState)
|
||||
elif s == 'carControl':
|
||||
self.plant.fill_car_control(msg.carControl)
|
||||
elif s == 'carOutput':
|
||||
msg.carOutput.actuatorsOutput.accel = float(self.plant.a_ego)
|
||||
msg.carOutput.actuatorsOutput.curvature = float(self.plant.curvature)
|
||||
msg.carOutput.actuatorsOutput.steeringAngleDeg = self.plant._angle(self.plant.achieved_curvature)
|
||||
msg.carOutput.actuatorsOutput.torque = self.plant._torque(self.plant.achieved_curvature)
|
||||
elif s == 'controlsState':
|
||||
msg.controlsState.curvature = float(self.plant.achieved_curvature)
|
||||
msg.controlsState.desiredCurvature = float(self.plant.curvature)
|
||||
elif s == 'selfdriveState':
|
||||
msg.selfdriveState.enabled = True
|
||||
msg.selfdriveState.active = True
|
||||
msg.selfdriveState.state = 'enabled'
|
||||
elif s == 'modelV2':
|
||||
msg.modelV2.frameId = 0
|
||||
msg.modelV2.action.desiredCurvature = 0.0
|
||||
elif s == 'livePose':
|
||||
msg.livePose.accelerationDevice.x = float(self.plant.a_ego)
|
||||
msg.livePose.accelerationDevice.y = float(self.plant.lat_accel)
|
||||
msg.livePose.velocityDevice.x = float(self.plant.v_ego)
|
||||
msg.livePose.inputsOK = True
|
||||
msg.livePose.posenetOK = True
|
||||
msg.livePose.sensorsOK = True
|
||||
elif s == 'liveParameters':
|
||||
msg.liveParameters.valid = True
|
||||
msg.liveParameters.roll = 0.0
|
||||
msg.liveParameters.steerRatio = STEER_RATIO
|
||||
return msg
|
||||
51
tools/longitudinal_maneuvers/sim_maneuvers.py
Executable file
51
tools/longitudinal_maneuvers/sim_maneuvers.py
Executable file
@@ -0,0 +1,51 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Run maneuversd against a synthetic longitudinal plant and write an rlog.
|
||||
|
||||
./tools/longitudinal_maneuvers/sim_maneuvers.py --out /tmp/long_rlog.zst
|
||||
./tools/longitudinal_maneuvers/generate_report.py /tmp/long_rlog.zst
|
||||
"""
|
||||
import argparse
|
||||
from pathlib import Path
|
||||
|
||||
from openpilot.tools.longitudinal_maneuvers.sim_harness import ManeuverSim, Plant
|
||||
|
||||
WN = 6.0 # powertrain natural frequency (rad/s)
|
||||
ZETA = 0.6 # underdamped, so actual accel overshoots the target like a real car
|
||||
|
||||
|
||||
class LongitudinalPlant(Plant):
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
self.jerk = 0.0
|
||||
|
||||
def step(self, dt, plan):
|
||||
a_target = float(plan.aTarget) if plan is not None else 0.0
|
||||
if plan is not None and plan.shouldStop:
|
||||
a_target = min(a_target, -0.5)
|
||||
|
||||
self.jerk += dt * (WN ** 2 * (a_target - self.a_ego) - 2 * ZETA * WN * self.jerk)
|
||||
self.a_ego += dt * self.jerk
|
||||
|
||||
self.v_ego = max(self.v_ego + self.a_ego * dt, 0.0)
|
||||
if self.v_ego <= 0.0:
|
||||
self.a_ego = min(self.a_ego, 0.0)
|
||||
self.jerk = min(self.jerk, 0.0)
|
||||
self.lat_accel = 0.0
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--out", type=Path, default=Path("/tmp/longitudinal_maneuvers_sim/rlog.zst"))
|
||||
parser.add_argument("--max-maneuvers", type=int, default=0, help="stop after N maneuvers (0 = all)")
|
||||
parser.add_argument("--timeout", type=float, default=900.0)
|
||||
args = parser.parse_args()
|
||||
|
||||
sim = ManeuverSim("openpilot.tools.longitudinal_maneuvers.maneuversd", LongitudinalPlant(),
|
||||
max_maneuvers=args.max_maneuvers, timeout=args.timeout)
|
||||
out = sim.run(args.out)
|
||||
print(f"\nmaneuvers seen: {sim.seen_maneuvers}")
|
||||
print(f"rlog: {out} ({out.stat().st_size / 1e6:.1f} MB)")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
81
tools/mac_setup.sh
Executable file
81
tools/mac_setup.sh
Executable file
@@ -0,0 +1,81 @@
|
||||
#!/usr/bin/env bash
|
||||
set -e
|
||||
|
||||
DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )"
|
||||
ROOT="$(cd $DIR/../ && pwd)"
|
||||
ARCH=$(uname -m)
|
||||
|
||||
# homebrew update is slow
|
||||
export HOMEBREW_NO_AUTO_UPDATE=1
|
||||
|
||||
if [[ $SHELL == "/bin/zsh" ]]; then
|
||||
RC_FILE="$HOME/.zshrc"
|
||||
elif [[ $SHELL == "/bin/bash" ]]; then
|
||||
RC_FILE="$HOME/.bash_profile"
|
||||
fi
|
||||
|
||||
# Install brew if required
|
||||
if [[ $(command -v brew) == "" ]]; then
|
||||
echo "Installing Homebrew"
|
||||
/bin/bash -c "$(curl -fsSL https://raw.githubusercontent.com/Homebrew/install/HEAD/install.sh)"
|
||||
echo "[ ] installed brew t=$SECONDS"
|
||||
|
||||
# make brew available now
|
||||
if [[ $ARCH == "x86_64" ]]; then
|
||||
echo 'eval "$(/usr/local/bin/brew shellenv)"' >> $RC_FILE
|
||||
eval "$(/usr/local/bin/brew shellenv)"
|
||||
else
|
||||
echo 'eval "$(/opt/homebrew/bin/brew shellenv)"' >> $RC_FILE
|
||||
eval "$(/opt/homebrew/bin/brew shellenv)"
|
||||
fi
|
||||
else
|
||||
brew up
|
||||
fi
|
||||
|
||||
brew bundle --file=$DIR/Brewfile
|
||||
|
||||
echo "[ ] finished brew install t=$SECONDS"
|
||||
|
||||
BREW_PREFIX=$(brew --prefix)
|
||||
|
||||
# archive backend tools for pip dependencies
|
||||
export LDFLAGS="$LDFLAGS -L${BREW_PREFIX}/opt/zlib/lib"
|
||||
export LDFLAGS="$LDFLAGS -L${BREW_PREFIX}/opt/bzip2/lib"
|
||||
export CPPFLAGS="$CPPFLAGS -I${BREW_PREFIX}/opt/zlib/include"
|
||||
export CPPFLAGS="$CPPFLAGS -I${BREW_PREFIX}/opt/bzip2/include"
|
||||
|
||||
# pycurl curl/openssl backend dependencies
|
||||
export LDFLAGS="$LDFLAGS -L${BREW_PREFIX}/opt/openssl@3/lib"
|
||||
export CPPFLAGS="$CPPFLAGS -I${BREW_PREFIX}/opt/openssl@3/include"
|
||||
export PYCURL_CURL_CONFIG=/usr/bin/curl-config
|
||||
export PYCURL_SSL_LIBRARY=openssl
|
||||
|
||||
# install python dependencies
|
||||
$DIR/install_python_dependencies.sh
|
||||
echo "[ ] installed python dependencies t=$SECONDS"
|
||||
|
||||
# brew does not link qt5 by default
|
||||
# check if qt5 can be linked, if not, prompt the user to link it
|
||||
QT_BIN_LOCATION="$(command -v lupdate || :)"
|
||||
if [ -n "$QT_BIN_LOCATION" ]; then
|
||||
# if qt6 is linked, prompt the user to unlink it and link the right version
|
||||
QT_BIN_VERSION="$(lupdate -version | awk '{print $NF}')"
|
||||
if [[ ! "$QT_BIN_VERSION" =~ 5\.[0-9]+\.[0-9]+ ]]; then
|
||||
echo
|
||||
echo "lupdate/lrelease available at PATH is $QT_BIN_VERSION"
|
||||
if [[ "$QT_BIN_LOCATION" == "$(brew --prefix)/"* ]]; then
|
||||
echo "Run the following command to link qt5:"
|
||||
echo "brew unlink qt@6 && brew link qt@5"
|
||||
else
|
||||
echo "Remove conflicting qt entries from PATH and run the following command to link qt5:"
|
||||
echo "brew link qt@5"
|
||||
fi
|
||||
fi
|
||||
else
|
||||
brew link qt@5
|
||||
fi
|
||||
|
||||
echo
|
||||
echo "---- OPENPILOT SETUP DONE ----"
|
||||
echo "Open a new shell or configure your active shell env by running:"
|
||||
echo "source $RC_FILE"
|
||||
511
tools/op.sh
Executable file
511
tools/op.sh
Executable file
@@ -0,0 +1,511 @@
|
||||
#!/usr/bin/env bash
|
||||
|
||||
if [[ ! "${BASH_SOURCE[0]}" = "${0}" ]]; then
|
||||
echo "Invalid invocation! This script must not be sourced."
|
||||
echo "Run 'op.sh' directly or check your .bashrc for a valid alias"
|
||||
return 0
|
||||
fi
|
||||
|
||||
set -e
|
||||
|
||||
RED='\033[0;31m'
|
||||
GREEN='\033[0;32m'
|
||||
UNDERLINE='\033[4m'
|
||||
BOLD='\033[1m'
|
||||
NC='\033[0m'
|
||||
|
||||
SHELL_NAME="$(basename ${SHELL})"
|
||||
RC_FILE="${HOME}/.$(basename ${SHELL})rc"
|
||||
if [ "$(uname)" == "Darwin" ] && [ $SHELL == "/bin/bash" ]; then
|
||||
RC_FILE="$HOME/.bash_profile"
|
||||
fi
|
||||
function op_install() {
|
||||
echo "Installing op system-wide..."
|
||||
CMD="\nalias op='"$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )/op.sh" \"\$@\"'\n"
|
||||
grep "alias op=" "$RC_FILE" &> /dev/null || printf "$CMD" >> $RC_FILE
|
||||
echo -e " ↳ [${GREEN}✔${NC}] op installed successfully. Open a new shell to use it."
|
||||
}
|
||||
|
||||
function loge() {
|
||||
if [[ -f "$LOG_FILE" ]]; then
|
||||
# error type
|
||||
echo "$1" >> $LOG_FILE
|
||||
# error log
|
||||
echo "$2" >> $LOG_FILE
|
||||
fi
|
||||
}
|
||||
|
||||
function op_run_command() {
|
||||
CMD="$@"
|
||||
|
||||
echo -e "${BOLD}Running command →${NC} $CMD │"
|
||||
for ((i=0; i<$((19 + ${#CMD})); i++)); do
|
||||
echo -n "─"
|
||||
done
|
||||
echo -e "┘\n"
|
||||
|
||||
if [[ -z "$DRY" ]]; then
|
||||
eval "$CMD"
|
||||
fi
|
||||
}
|
||||
|
||||
# be default, assume openpilot dir is in current directory
|
||||
OPENPILOT_ROOT=$(pwd)
|
||||
function op_get_openpilot_dir() {
|
||||
# First try traversing up the directory tree
|
||||
while [[ "$OPENPILOT_ROOT" != '/' ]];
|
||||
do
|
||||
if find "$OPENPILOT_ROOT/launch_openpilot.sh" -maxdepth 1 -mindepth 1 &> /dev/null; then
|
||||
return 0
|
||||
fi
|
||||
OPENPILOT_ROOT="$(readlink -f "$OPENPILOT_ROOT/"..)"
|
||||
done
|
||||
|
||||
# Fallback to hardcoded directories if not found
|
||||
for dir in "$HOME/openpilot" "/data/openpilot"; do
|
||||
if [[ -f "$dir/launch_openpilot.sh" ]]; then
|
||||
OPENPILOT_ROOT="$dir"
|
||||
return 0
|
||||
fi
|
||||
done
|
||||
}
|
||||
|
||||
function op_install_post_commit() {
|
||||
op_get_openpilot_dir
|
||||
if [[ ! -d $OPENPILOT_ROOT/.git/hooks/post-commit.d ]]; then
|
||||
mkdir $OPENPILOT_ROOT/.git/hooks/post-commit.d
|
||||
mv $OPENPILOT_ROOT/.git/hooks/post-commit $OPENPILOT_ROOT/.git/hooks/post-commit.d 2>/dev/null || true
|
||||
fi
|
||||
cd $OPENPILOT_ROOT/.git/hooks
|
||||
ln -sf ../../scripts/post-commit post-commit
|
||||
}
|
||||
|
||||
function op_check_openpilot_dir() {
|
||||
echo "Checking for openpilot directory..."
|
||||
if [[ -f "$OPENPILOT_ROOT/launch_openpilot.sh" ]]; then
|
||||
echo -e " ↳ [${GREEN}✔${NC}] openpilot found."
|
||||
return 0
|
||||
fi
|
||||
|
||||
echo -e " ↳ [${RED}✗${NC}] openpilot directory not found! Make sure that you are"
|
||||
echo " inside the openpilot directory or specify one with the"
|
||||
echo " --dir option!"
|
||||
return 1
|
||||
}
|
||||
|
||||
function op_check_git() {
|
||||
echo "Checking for git..."
|
||||
if ! command -v "git" > /dev/null 2>&1; then
|
||||
echo -e " ↳ [${RED}✗${NC}] git not found on your system!"
|
||||
return 1
|
||||
else
|
||||
echo -e " ↳ [${GREEN}✔${NC}] git found."
|
||||
fi
|
||||
|
||||
echo "Checking for git lfs files..."
|
||||
if [[ $(file -b $OPENPILOT_ROOT/selfdrive/modeld/models/dmonitoring_model.onnx) == "data" ]]; then
|
||||
echo -e " ↳ [${GREEN}✔${NC}] git lfs files found."
|
||||
else
|
||||
echo -e " ↳ [${RED}✗${NC}] git lfs files not found! Run 'git lfs pull'"
|
||||
return 1
|
||||
fi
|
||||
|
||||
echo "Checking for git submodules..."
|
||||
for name in $(git config --file .gitmodules --get-regexp path | awk '{ print $2 }' | tr '\n' ' '); do
|
||||
if [[ -z $(ls $OPENPILOT_ROOT/$name) ]]; then
|
||||
echo -e " ↳ [${RED}✗${NC}] git submodule $name not found! Run 'git submodule update --init --recursive'"
|
||||
return 1
|
||||
fi
|
||||
done
|
||||
echo -e " ↳ [${GREEN}✔${NC}] git submodules found."
|
||||
}
|
||||
|
||||
function op_check_os() {
|
||||
echo "Checking for compatible os version..."
|
||||
if [[ "$OSTYPE" == "linux-gnu"* ]]; then
|
||||
|
||||
if [ -f "/etc/os-release" ]; then
|
||||
source /etc/os-release
|
||||
case "$VERSION_CODENAME" in
|
||||
"jammy" | "kinetic" | "noble" | "focal")
|
||||
echo -e " ↳ [${GREEN}✔${NC}] Ubuntu $VERSION_CODENAME detected."
|
||||
;;
|
||||
* )
|
||||
echo -e " ↳ [${RED}✗${NC}] Incompatible Ubuntu version $VERSION_CODENAME detected!"
|
||||
loge "ERROR_INCOMPATIBLE_UBUNTU" "$VERSION_CODENAME"
|
||||
return 1
|
||||
;;
|
||||
esac
|
||||
else
|
||||
echo -e " ↳ [${RED}✗${NC}] No /etc/os-release on your system. Make sure you're running on Ubuntu, or similar!"
|
||||
loge "ERROR_UNKNOWN_UBUNTU"
|
||||
return 1
|
||||
fi
|
||||
|
||||
elif [[ "$OSTYPE" == "darwin"* ]]; then
|
||||
echo -e " ↳ [${GREEN}✔${NC}] macOS detected."
|
||||
else
|
||||
echo -e " ↳ [${RED}✗${NC}] OS type $OSTYPE not supported!"
|
||||
loge "ERROR_UNKNOWN_OS" "$OSTYPE"
|
||||
return 1
|
||||
fi
|
||||
}
|
||||
|
||||
function op_check_python() {
|
||||
echo "Checking for compatible python version..."
|
||||
REQUIRED_PYTHON_VERSION=$(grep "requires-python" $OPENPILOT_ROOT/pyproject.toml)
|
||||
INSTALLED_PYTHON_VERSION=$(python3 --version 2> /dev/null || true)
|
||||
|
||||
if [[ -z $INSTALLED_PYTHON_VERSION ]]; then
|
||||
echo -e " ↳ [${RED}✗${NC}] python3 not found on your system. You need python version satisfying $(echo $REQUIRED_PYTHON_VERSION | cut -d '=' -f2-) to continue!"
|
||||
loge "ERROR_PYTHON_NOT_FOUND"
|
||||
return 1
|
||||
else
|
||||
if python3 -c "
|
||||
import sys, re
|
||||
req = '''$REQUIRED_PYTHON_VERSION'''
|
||||
bounds = re.findall(r'[0-9]+(?:\.[0-9]+)*', req)
|
||||
def parse(v): parts = list(map(int, v.split('.'))); return parts + [0]*(3-len(parts))
|
||||
lb, ub = parse(bounds[0]), parse(bounds[1])
|
||||
vi = list(sys.version_info[:3])
|
||||
sys.exit(0 if lb <= vi < ub else 1)
|
||||
"; then
|
||||
echo -e " ↳ [${GREEN}✔${NC}] $INSTALLED_PYTHON_VERSION detected."
|
||||
else
|
||||
echo -e " ↳ [${RED}✗${NC}] You need a python version satisfying $(echo $REQUIRED_PYTHON_VERSION | cut -d '=' -f2-) to continue!"
|
||||
loge "ERROR_PYTHON_VERSION" "$INSTALLED_PYTHON_VERSION"
|
||||
return 1
|
||||
fi
|
||||
fi
|
||||
}
|
||||
|
||||
function op_check_venv() {
|
||||
echo "Checking for venv..."
|
||||
if [[ -f $OPENPILOT_ROOT/.venv/bin/activate ]]; then
|
||||
echo -e " ↳ [${GREEN}✔${NC}] venv detected."
|
||||
else
|
||||
echo -e " ↳ [${RED}✗${NC}] Can't activate venv in $OPENPILOT_ROOT. Assuming global env!"
|
||||
fi
|
||||
}
|
||||
|
||||
function op_before_cmd() {
|
||||
if [[ ! -z "$NO_VERIFY" ]]; then
|
||||
return 0
|
||||
fi
|
||||
|
||||
op_get_openpilot_dir
|
||||
cd $OPENPILOT_ROOT
|
||||
|
||||
result="$((op_check_openpilot_dir ) 2>&1)" || (echo -e "$result" && return 1)
|
||||
result="${result}\n$(( op_check_git ) 2>&1)" || (echo -e "$result" && return 1)
|
||||
result="${result}\n$(( op_check_os ) 2>&1)" || (echo -e "$result" && return 1)
|
||||
result="${result}\n$(( op_check_venv ) 2>&1)" || (echo -e "$result" && return 1)
|
||||
|
||||
op_activate_venv
|
||||
|
||||
result="${result}\n$(( op_check_python ) 2>&1)" || (echo -e "$result" && return 1)
|
||||
|
||||
if [[ -z $VERBOSE ]]; then
|
||||
echo -e "${BOLD}Checking system →${NC} [${GREEN}✔${NC}]"
|
||||
else
|
||||
echo -e "$result"
|
||||
fi
|
||||
}
|
||||
|
||||
function op_setup() {
|
||||
op_get_openpilot_dir
|
||||
cd $OPENPILOT_ROOT
|
||||
|
||||
op_check_openpilot_dir
|
||||
op_check_os
|
||||
|
||||
echo "Installing dependencies..."
|
||||
st="$(date +%s)"
|
||||
if [[ "$OSTYPE" == "linux-gnu"* ]]; then
|
||||
SETUP_SCRIPT="tools/ubuntu_setup.sh"
|
||||
elif [[ "$OSTYPE" == "darwin"* ]]; then
|
||||
SETUP_SCRIPT="tools/mac_setup.sh"
|
||||
fi
|
||||
if ! $OPENPILOT_ROOT/$SETUP_SCRIPT; then
|
||||
echo -e " ↳ [${RED}✗${NC}] Dependencies installation failed!"
|
||||
loge "ERROR_DEPENDENCIES_INSTALLATION"
|
||||
return 1
|
||||
fi
|
||||
et="$(date +%s)"
|
||||
echo -e " ↳ [${GREEN}✔${NC}] Dependencies installed successfully in $((et - st)) seconds."
|
||||
|
||||
echo "Getting git submodules..."
|
||||
st="$(date +%s)"
|
||||
if ! git submodule update --jobs 4 --init --recursive; then
|
||||
echo -e " ↳ [${RED}✗${NC}] Getting git submodules failed!"
|
||||
loge "ERROR_GIT_SUBMODULES"
|
||||
return 1
|
||||
fi
|
||||
et="$(date +%s)"
|
||||
echo -e " ↳ [${GREEN}✔${NC}] Submodules installed successfully in $((et - st)) seconds."
|
||||
|
||||
echo "Pulling git lfs files..."
|
||||
st="$(date +%s)"
|
||||
if ! git lfs pull; then
|
||||
echo -e " ↳ [${RED}✗${NC}] Pulling git lfs files failed!"
|
||||
loge "ERROR_GIT_LFS"
|
||||
return 1
|
||||
fi
|
||||
et="$(date +%s)"
|
||||
echo -e " ↳ [${GREEN}✔${NC}] Files pulled successfully in $((et - st)) seconds."
|
||||
|
||||
op_check
|
||||
}
|
||||
|
||||
function op_auth() {
|
||||
op_before_cmd
|
||||
op_run_command tools/lib/auth.py "$@"
|
||||
}
|
||||
|
||||
function op_activate_venv() {
|
||||
# bash 3.2 can't handle this without the 'set +e'
|
||||
set +e
|
||||
source $OPENPILOT_ROOT/.venv/bin/activate &> /dev/null || true
|
||||
set -e
|
||||
}
|
||||
|
||||
function op_venv() {
|
||||
op_before_cmd
|
||||
|
||||
if [[ ! -f $OPENPILOT_ROOT/.venv/bin/activate ]]; then
|
||||
echo -e "No venv found in $OPENPILOT_ROOT"
|
||||
return 1
|
||||
fi
|
||||
|
||||
case $SHELL_NAME in
|
||||
"zsh")
|
||||
ZSHRC_DIR=$(mktemp -d 2>/dev/null || mktemp -d -t 'tmp_zsh')
|
||||
echo "source $RC_FILE; source $OPENPILOT_ROOT/.venv/bin/activate" >> $ZSHRC_DIR/.zshrc
|
||||
ZDOTDIR=$ZSHRC_DIR zsh ;;
|
||||
*)
|
||||
bash --rcfile <(echo "source $RC_FILE; source $OPENPILOT_ROOT/.venv/bin/activate") ;;
|
||||
esac
|
||||
}
|
||||
|
||||
function op_adb() {
|
||||
op_before_cmd
|
||||
op_run_command tools/scripts/adb_ssh.sh "$@"
|
||||
}
|
||||
|
||||
function op_ssh() {
|
||||
op_before_cmd
|
||||
op_run_command tools/scripts/ssh.py "$@"
|
||||
}
|
||||
|
||||
function op_check() {
|
||||
VERBOSE=1
|
||||
op_before_cmd
|
||||
unset VERBOSE
|
||||
}
|
||||
|
||||
function op_esim() {
|
||||
op_before_cmd
|
||||
op_run_command system/hardware/esim.py "$@"
|
||||
}
|
||||
|
||||
function op_build() {
|
||||
CDIR=$(pwd)
|
||||
op_before_cmd
|
||||
cd "$CDIR"
|
||||
if [[ -f "/AGNOS" ]]; then
|
||||
# needed on AGNOS to not run out of memory
|
||||
op_run_command system/manager/build.py
|
||||
else
|
||||
# scons is fine on PC
|
||||
op_run_command scons $@
|
||||
fi
|
||||
}
|
||||
|
||||
function op_juggle() {
|
||||
op_before_cmd
|
||||
op_run_command tools/plotjuggler/juggle.py $@
|
||||
}
|
||||
|
||||
function op_lint() {
|
||||
op_before_cmd
|
||||
op_run_command scripts/lint/lint.sh $@
|
||||
}
|
||||
|
||||
function op_test() {
|
||||
op_before_cmd
|
||||
op_run_command pytest $@
|
||||
}
|
||||
|
||||
function op_replay() {
|
||||
op_before_cmd
|
||||
op_run_command tools/replay/replay $@
|
||||
}
|
||||
|
||||
function op_cabana() {
|
||||
op_before_cmd
|
||||
op_run_command tools/cabana/cabana $@
|
||||
}
|
||||
|
||||
function op_sim() {
|
||||
op_before_cmd
|
||||
op_run_command exec tools/sim/run_bridge.py &
|
||||
op_run_command exec tools/sim/launch_openpilot.sh
|
||||
}
|
||||
|
||||
function op_clip() {
|
||||
op_before_cmd
|
||||
local python_bin="$OPENPILOT_ROOT/.venv/bin/python3"
|
||||
if [[ ! -x "$python_bin" ]]; then
|
||||
python_bin="python3"
|
||||
fi
|
||||
|
||||
local cmd=("$python_bin" "$OPENPILOT_ROOT/tools/clip/run.py" "$@")
|
||||
local cmd_str
|
||||
printf -v cmd_str '%q ' "${cmd[@]}"
|
||||
cmd_str="${cmd_str% }"
|
||||
|
||||
echo -e "${BOLD}Running command →${NC} $cmd_str │"
|
||||
for ((i=0; i<$((19 + ${#cmd_str})); i++)); do
|
||||
echo -n "─"
|
||||
done
|
||||
echo -e "┘\n"
|
||||
|
||||
if [[ -z "$DRY" ]]; then
|
||||
"${cmd[@]}"
|
||||
fi
|
||||
}
|
||||
|
||||
function op_switch() {
|
||||
REMOTE="origin"
|
||||
if [ "$#" -gt 1 ]; then
|
||||
REMOTE="$1"
|
||||
shift
|
||||
fi
|
||||
|
||||
if [ -z "$1" ]; then
|
||||
echo -e "${BOLD}${UNDERLINE}Usage:${NC} op switch [REMOTE] <BRANCH>"
|
||||
return 1
|
||||
fi
|
||||
BRANCH="$1"
|
||||
|
||||
git config --replace-all remote.origin.fetch "+refs/heads/*:refs/remotes/origin/*"
|
||||
git submodule deinit --all --force
|
||||
git fetch "$REMOTE" "$BRANCH"
|
||||
git checkout -f FETCH_HEAD
|
||||
git checkout -B "$BRANCH" --track "$REMOTE"/"$BRANCH"
|
||||
git submodule deinit --all --force
|
||||
git reset --hard "${REMOTE}/${BRANCH}"
|
||||
git clean -df
|
||||
git submodule update --init --recursive
|
||||
git submodule foreach git reset --hard
|
||||
git submodule foreach git clean -df
|
||||
}
|
||||
|
||||
function op_start() {
|
||||
if [[ -f "/AGNOS" ]]; then
|
||||
op_before_cmd
|
||||
op_run_command sudo systemctl restart comma $@
|
||||
fi
|
||||
}
|
||||
|
||||
function op_stop() {
|
||||
if [[ -f "/AGNOS" ]]; then
|
||||
op_before_cmd
|
||||
op_run_command sudo systemctl stop comma $@
|
||||
fi
|
||||
}
|
||||
|
||||
function op_default() {
|
||||
echo "An openpilot helper"
|
||||
echo ""
|
||||
echo -e "${BOLD}${UNDERLINE}Description:${NC}"
|
||||
echo " op is your entry point for all things related to openpilot development."
|
||||
echo " op is only a wrapper for existing scripts, tools, and commands."
|
||||
echo " op will always show you what it will run on your system."
|
||||
echo ""
|
||||
echo -e "${BOLD}${UNDERLINE}Usage:${NC} op [OPTIONS] <COMMAND>"
|
||||
echo ""
|
||||
echo -e "${BOLD}${UNDERLINE}Commands [System]:${NC}"
|
||||
echo -e " ${BOLD}auth${NC} Authenticate yourself for API use"
|
||||
echo -e " ${BOLD}check${NC} Check the development environment (git, os, python) to start using openpilot"
|
||||
echo -e " ${BOLD}esim${NC} Manage eSIM profiles on your comma device"
|
||||
echo -e " ${BOLD}venv${NC} Activate the python virtual environment"
|
||||
echo -e " ${BOLD}setup${NC} Install openpilot dependencies"
|
||||
echo -e " ${BOLD}build${NC} Run the openpilot build system in the current working directory"
|
||||
echo -e " ${BOLD}install${NC} Install the 'op' tool system wide"
|
||||
echo -e " ${BOLD}switch${NC} Switch to a different git branch with a clean slate (nukes any changes)"
|
||||
echo -e " ${BOLD}start${NC} Starts (or restarts) openpilot"
|
||||
echo -e " ${BOLD}stop${NC} Stops openpilot"
|
||||
echo ""
|
||||
echo -e "${BOLD}${UNDERLINE}Commands [Tooling]:${NC}"
|
||||
echo -e " ${BOLD}juggle${NC} Run PlotJuggler"
|
||||
echo -e " ${BOLD}replay${NC} Run Replay"
|
||||
echo -e " ${BOLD}cabana${NC} Run Cabana"
|
||||
echo -e " ${BOLD}clip${NC} Run clip (linux only)"
|
||||
echo -e " ${BOLD}adb${NC} Run adb shell"
|
||||
echo -e " ${BOLD}ssh${NC} comma prime SSH helper"
|
||||
echo ""
|
||||
echo -e "${BOLD}${UNDERLINE}Commands [Testing]:${NC}"
|
||||
echo -e " ${BOLD}sim${NC} Run openpilot in a simulator"
|
||||
echo -e " ${BOLD}lint${NC} Run the linter"
|
||||
echo -e " ${BOLD}post-commit${NC} Install the linter as a post-commit hook"
|
||||
echo -e " ${BOLD}test${NC} Run all unit tests from pytest"
|
||||
echo ""
|
||||
echo -e "${BOLD}${UNDERLINE}Options:${NC}"
|
||||
echo -e " ${BOLD}-d, --dir${NC}"
|
||||
echo " Specify the openpilot directory you want to use"
|
||||
echo -e " ${BOLD}--dry${NC}"
|
||||
echo " Don't actually run anything, just print what would be run"
|
||||
echo -e " ${BOLD}-n, --no-verify${NC}"
|
||||
echo " Skip environment check before running commands"
|
||||
echo ""
|
||||
echo -e "${BOLD}${UNDERLINE}Examples:${NC}"
|
||||
echo " op setup"
|
||||
echo " Run the setup script to install"
|
||||
echo " openpilot's dependencies."
|
||||
echo ""
|
||||
echo " op build -j4"
|
||||
echo " Compile openpilot using 4 cores"
|
||||
echo ""
|
||||
echo " op juggle --demo"
|
||||
echo " Run PlotJuggler on the demo route"
|
||||
}
|
||||
|
||||
|
||||
function _op() {
|
||||
# parse Options
|
||||
case $1 in
|
||||
-d | --dir ) shift 1; OPENPILOT_ROOT="$1"; shift 1 ;;
|
||||
--dry ) shift 1; DRY="1" ;;
|
||||
-n | --no-verify ) shift 1; NO_VERIFY="1" ;;
|
||||
-l | --log ) shift 1; LOG_FILE="$1" ; shift 1 ;;
|
||||
esac
|
||||
|
||||
# parse Commands
|
||||
case $1 in
|
||||
auth ) shift 1; op_auth "$@" ;;
|
||||
venv ) shift 1; op_venv "$@" ;;
|
||||
check ) shift 1; op_check "$@" ;;
|
||||
esim ) shift 1; op_esim "$@" ;;
|
||||
setup ) shift 1; op_setup "$@" ;;
|
||||
build ) shift 1; op_build "$@" ;;
|
||||
juggle ) shift 1; op_juggle "$@" ;;
|
||||
cabana ) shift 1; op_cabana "$@" ;;
|
||||
lint ) shift 1; op_lint "$@" ;;
|
||||
test ) shift 1; op_test "$@" ;;
|
||||
replay ) shift 1; op_replay "$@" ;;
|
||||
clip ) shift 1; op_clip "$@" ;;
|
||||
sim ) shift 1; op_sim "$@" ;;
|
||||
install ) shift 1; op_install "$@" ;;
|
||||
switch ) shift 1; op_switch "$@" ;;
|
||||
start ) shift 1; op_start "$@" ;;
|
||||
stop ) shift 1; op_stop "$@" ;;
|
||||
restart ) shift 1; op_restart "$@" ;;
|
||||
post-commit ) shift 1; op_install_post_commit "$@" ;;
|
||||
adb ) shift 1; op_adb "$@" ;;
|
||||
ssh ) shift 1; op_ssh "$@" ;;
|
||||
* ) op_default "$@" ;;
|
||||
esac
|
||||
}
|
||||
|
||||
_op $@
|
||||
3
tools/plotjuggler/.gitignore
vendored
Normal file
3
tools/plotjuggler/.gitignore
vendored
Normal file
@@ -0,0 +1,3 @@
|
||||
bin/
|
||||
bin
|
||||
*.rlog
|
||||
77
tools/plotjuggler/README.md
Normal file
77
tools/plotjuggler/README.md
Normal file
@@ -0,0 +1,77 @@
|
||||
# PlotJuggler
|
||||
|
||||
[PlotJuggler](https://github.com/facontidavide/PlotJuggler) is a tool to quickly visualize time series data, and we've written plugins to parse openpilot logs. Check out our plugins: https://github.com/commaai/PlotJuggler.
|
||||
|
||||
## Installation
|
||||
|
||||
Once you've [set up the openpilot environment](../README.md), this command will download PlotJuggler and install our plugins:
|
||||
|
||||
`cd tools/plotjuggler && ./juggle.py --install`
|
||||
|
||||
## Usage
|
||||
|
||||
```
|
||||
$ ./juggle.py -h
|
||||
usage: juggle.py [-h] [--demo] [--can] [--stream] [--layout [LAYOUT]] [--install] [--dbc DBC]
|
||||
[route_or_segment_name]
|
||||
|
||||
A helper to run PlotJuggler on openpilot routes
|
||||
|
||||
positional arguments:
|
||||
route_or_segment_name
|
||||
The route or segment name to plot (cabana share URL accepted) (default: None)
|
||||
|
||||
optional arguments:
|
||||
-h, --help show this help message and exit
|
||||
--demo Use the demo route instead of providing one (default: False)
|
||||
--can Parse CAN data (default: False)
|
||||
--stream Start PlotJuggler in streaming mode (default: False)
|
||||
--layout [LAYOUT] Run PlotJuggler with a pre-defined layout (default: None)
|
||||
--install Install or update PlotJuggler + plugins (default: False)
|
||||
--dbc DBC Set the DBC name to load for parsing CAN data. If not set, the DBC will be automatically
|
||||
inferred from the logs. (default: None)
|
||||
|
||||
```
|
||||
|
||||
Example using route name:
|
||||
|
||||
`./juggle.py "a2a0ccea32023010/2023-07-27--13-01-19"`
|
||||
|
||||
Examples using segment:
|
||||
|
||||
`./juggle.py "a2a0ccea32023010/2023-07-27--13-01-19/1"`
|
||||
|
||||
`./juggle.py "a2a0ccea32023010/2023-07-27--13-01-19/1/q" # use qlogs`
|
||||
|
||||
Example using segment range:
|
||||
|
||||
`./juggle.py "a2a0ccea32023010/2023-07-27--13-01-19/0:1"`
|
||||
|
||||
## Streaming
|
||||
|
||||
Explore live data from your car! Follow these steps to stream from your comma device to your laptop:
|
||||
- Enable wifi tethering on your comma device
|
||||
- [SSH into your device](https://github.com/commaai/openpilot/wiki/SSH) and run `cd /data/openpilot && ./cereal/messaging/bridge`
|
||||
- On your laptop, connect to the device's wifi hotspot
|
||||
- Start PlotJuggler with `ZMQ=1 ./juggle.py --stream`, find the `Cereal Subscriber` plugin in the dropdown under Streaming, and click `Start`.
|
||||
|
||||
If streaming to PlotJuggler from a replay on your PC, simply run: `./juggle.py --stream` and start the cereal subscriber.
|
||||
|
||||
## Demo
|
||||
|
||||
For a quick demo, go through the installation step and run this command:
|
||||
|
||||
`./juggle.py --demo --layout=layouts/tuning.xml`
|
||||
|
||||
## Layouts
|
||||
|
||||
If you create a layout that's useful for others, consider upstreaming it.
|
||||
|
||||
### Tuning
|
||||
|
||||
Use this layout to improve your car's tuning and generate plots for tuning PRs. Also see the [tuning wiki](https://github.com/commaai/openpilot/wiki/Tuning) and tuning PR template.
|
||||
|
||||
`--layout layouts/tuning.xml`
|
||||
|
||||
|
||||
|
||||
141
tools/plotjuggler/juggle.py
Executable file
141
tools/plotjuggler/juggle.py
Executable file
@@ -0,0 +1,141 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import sys
|
||||
import platform
|
||||
import shutil
|
||||
import subprocess
|
||||
import tarfile
|
||||
import tempfile
|
||||
import requests
|
||||
import argparse
|
||||
from functools import partial
|
||||
|
||||
from iqdbc.car.fingerprints import MIGRATION
|
||||
from openpilot.common.basedir import BASEDIR
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.tools.cabana.dbc.generate_dbc_json import generate_dbc_dict
|
||||
from openpilot.tools.lib.logreader import LogReader, ReadMode, save_log
|
||||
from openpilot.selfdrive.test.process_replay.migration import migrate_all
|
||||
|
||||
juggle_dir = os.path.dirname(os.path.realpath(__file__))
|
||||
|
||||
os.environ['LD_LIBRARY_PATH'] = os.environ.get('LD_LIBRARY_PATH', '') + f":{juggle_dir}/bin/"
|
||||
|
||||
DEMO_ROUTE = "a2a0ccea32023010|2023-07-27--13-01-19"
|
||||
RELEASES_URL = "https://github.com/commaai/PlotJuggler/releases/download/latest"
|
||||
INSTALL_DIR = os.path.join(juggle_dir, "bin")
|
||||
PLOTJUGGLER_BIN = os.path.join(juggle_dir, "bin/plotjuggler")
|
||||
MINIMUM_PLOTJUGGLER_VERSION = (3, 5, 2)
|
||||
MAX_STREAMING_BUFFER_SIZE = 1000
|
||||
|
||||
|
||||
def install():
|
||||
m = f"{platform.system()}-{platform.machine()}"
|
||||
supported = ("Linux-x86_64", "Linux-aarch64", "Darwin-arm64", "Darwin-x86_64")
|
||||
if m not in supported:
|
||||
raise Exception(f"Unsupported platform: '{m}'. Supported platforms: {supported}")
|
||||
|
||||
if os.path.exists(INSTALL_DIR):
|
||||
shutil.rmtree(INSTALL_DIR)
|
||||
os.mkdir(INSTALL_DIR)
|
||||
|
||||
url = os.path.join(RELEASES_URL, m + ".tar.gz")
|
||||
with requests.get(url, stream=True, timeout=10) as r, tempfile.NamedTemporaryFile() as tmp:
|
||||
r.raise_for_status()
|
||||
with open(tmp.name, 'wb') as tmpf:
|
||||
for chunk in r.iter_content(chunk_size=1024 * 1024):
|
||||
tmpf.write(chunk)
|
||||
|
||||
with tarfile.open(tmp.name) as tar:
|
||||
tar.extractall(path=INSTALL_DIR, filter="data")
|
||||
|
||||
|
||||
def get_plotjuggler_version():
|
||||
out = subprocess.check_output([PLOTJUGGLER_BIN, "-v"], encoding="utf-8").strip()
|
||||
version = out.split(" ")[1]
|
||||
return tuple(map(int, version.split(".")))
|
||||
|
||||
|
||||
def start_juggler(fn=None, dbc=None, layout=None, route_or_segment_name=None, platform=None):
|
||||
env = os.environ.copy()
|
||||
env["BASEDIR"] = BASEDIR
|
||||
env["PATH"] = f"{INSTALL_DIR}:{os.getenv('PATH', '')}"
|
||||
if dbc:
|
||||
if os.path.exists(dbc):
|
||||
dbc = os.path.abspath(dbc)
|
||||
env["DBC_NAME"] = dbc
|
||||
|
||||
extra_args = ""
|
||||
if fn is not None:
|
||||
extra_args += f" -d {os.path.abspath(fn)}"
|
||||
if layout is not None:
|
||||
extra_args += f" -l {os.path.abspath(layout)}"
|
||||
if route_or_segment_name is not None:
|
||||
extra_args += f" --window_title \"{route_or_segment_name}{f' ({platform})' if platform is not None else ''}\""
|
||||
|
||||
cmd = f'{PLOTJUGGLER_BIN} --buffer_size {MAX_STREAMING_BUFFER_SIZE} --plugin_folders {INSTALL_DIR}{extra_args}'
|
||||
subprocess.call(cmd, shell=True, env=env, cwd=juggle_dir)
|
||||
|
||||
|
||||
def process(can, lr):
|
||||
return [d for d in lr if can or d.which() not in ['can', 'sendcan'] and not d.which().startswith('customReserved')]
|
||||
|
||||
|
||||
def juggle_route(route_or_segment_name, can, layout, dbc, should_migrate):
|
||||
lr = LogReader(route_or_segment_name, default_mode=ReadMode.AUTO_INTERACTIVE)
|
||||
|
||||
all_data = lr.run_across_segments(24, partial(process, can))
|
||||
if should_migrate:
|
||||
all_data = migrate_all(all_data)
|
||||
|
||||
# Infer DBC name from logs
|
||||
platform = None
|
||||
if dbc is None:
|
||||
try:
|
||||
CP = lr.first('carParams')
|
||||
platform = MIGRATION.get(CP.carFingerprint, CP.carFingerprint)
|
||||
dbc = generate_dbc_dict()[platform]
|
||||
except Exception:
|
||||
cloudlog.exception("Failed to get DBC name from logs!")
|
||||
|
||||
with tempfile.NamedTemporaryFile(suffix='.rlog', dir=juggle_dir) as tmp:
|
||||
save_log(tmp.name, all_data, compress=False)
|
||||
del all_data
|
||||
start_juggler(tmp.name, dbc, layout, route_or_segment_name, platform)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser(description="A helper to run PlotJuggler on openpilot routes",
|
||||
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
|
||||
|
||||
parser.add_argument("--demo", action="store_true", help="Use the demo route instead of providing one")
|
||||
parser.add_argument("--can", action="store_true", help="Parse CAN data")
|
||||
parser.add_argument("--stream", action="store_true", help="Start PlotJuggler in streaming mode")
|
||||
parser.add_argument("--no-migration", action="store_true", help="Do not perform log migration")
|
||||
parser.add_argument("--layout", nargs='?', help="Run PlotJuggler with a pre-defined layout")
|
||||
parser.add_argument("--install", action="store_true", help="Install or update PlotJuggler + plugins")
|
||||
parser.add_argument("--dbc", help="Set the DBC name to load for parsing CAN data. If not set, the DBC will be automatically inferred from the logs.")
|
||||
parser.add_argument("route_or_segment_name", nargs='?', help="The route or segment name to plot (cabana share URL accepted)")
|
||||
|
||||
if len(sys.argv) == 1:
|
||||
parser.print_help()
|
||||
sys.exit()
|
||||
args = parser.parse_args()
|
||||
|
||||
if args.install:
|
||||
install()
|
||||
sys.exit()
|
||||
|
||||
if not os.path.exists(PLOTJUGGLER_BIN):
|
||||
print("PlotJuggler is missing. Downloading...")
|
||||
install()
|
||||
|
||||
if get_plotjuggler_version() < MINIMUM_PLOTJUGGLER_VERSION:
|
||||
print("PlotJuggler is out of date. Installing update...")
|
||||
install()
|
||||
|
||||
if args.stream:
|
||||
start_juggler(layout=args.layout)
|
||||
else:
|
||||
route_or_segment_name = DEMO_ROUTE if args.demo else args.route_or_segment_name.strip()
|
||||
juggle_route(route_or_segment_name, args.can, args.layout, args.dbc, not args.no_migration)
|
||||
86
tools/plotjuggler/layouts/CAN-bus-debug.xml
Normal file
86
tools/plotjuggler/layouts/CAN-bus-debug.xml
Normal file
@@ -0,0 +1,86 @@
|
||||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<root>
|
||||
<tabbed_widget name="Main Window" parent="main_window">
|
||||
<Tab containers="1" tab_name="tab1">
|
||||
<Container>
|
||||
<DockSplitter sizes="0.33362;0.33276;0.33362" count="3" orientation="-">
|
||||
<DockArea name="CAN RX">
|
||||
<plot style="Lines" flip_y="false" mode="TimeSeries" flip_x="false">
|
||||
<range top="1101.875000" left="0.000000" bottom="-26.875000" right="60.526742"/>
|
||||
<limitY/>
|
||||
<curve name="/pandaStates/0/canState0/totalRxCnt" color="#f14cc1">
|
||||
<transform alias="/pandaStates/0/canState0/totalRxCnt[Derivative]" name="Derivative">
|
||||
<options radioChecked="radioCustom" lineEdit="1.0"/>
|
||||
</transform>
|
||||
</curve>
|
||||
<curve name="/pandaStates/0/canState1/totalRxCnt" color="#9467bd">
|
||||
<transform alias="/pandaStates/0/canState1/totalRxCnt[Derivative]" name="Derivative">
|
||||
<options radioChecked="radioCustom" lineEdit="1.0"/>
|
||||
</transform>
|
||||
</curve>
|
||||
<curve name="/pandaStates/0/canState2/totalRxCnt" color="#ff7f0e">
|
||||
<transform alias="/pandaStates/0/canState2/totalRxCnt[Derivative]" name="Derivative">
|
||||
<options radioChecked="radioCustom" lineEdit="1.0"/>
|
||||
</transform>
|
||||
</curve>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="CAN TX">
|
||||
<plot style="Lines" flip_y="false" mode="TimeSeries" flip_x="false">
|
||||
<range top="455.100000" left="0.000000" bottom="-11.100000" right="60.526742"/>
|
||||
<limitY/>
|
||||
<curve name="/pandaStates/0/canState0/totalTxCnt" color="#17becf">
|
||||
<transform alias="/pandaStates/0/canState0/totalTxCnt[Derivative]" name="Derivative">
|
||||
<options radioChecked="radioCustom" lineEdit="1.0"/>
|
||||
</transform>
|
||||
</curve>
|
||||
<curve name="/pandaStates/0/canState1/totalTxCnt" color="#bcbd22">
|
||||
<transform alias="/pandaStates/0/canState1/totalTxCnt[Derivative]" name="Derivative">
|
||||
<options radioChecked="radioCustom" lineEdit="1.0"/>
|
||||
</transform>
|
||||
</curve>
|
||||
<curve name="/pandaStates/0/canState2/totalTxCnt" color="#1f77b4">
|
||||
<transform alias="/pandaStates/0/canState2/totalTxCnt[Derivative]" name="Derivative">
|
||||
<options radioChecked="radioCustom" lineEdit="1.0"/>
|
||||
</transform>
|
||||
</curve>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="CAN errors">
|
||||
<plot style="Lines" flip_y="false" mode="TimeSeries" flip_x="false">
|
||||
<range top="2515.350000" left="0.000000" bottom="-61.350000" right="60.526742"/>
|
||||
<limitY/>
|
||||
<curve name="/pandaStates/0/canState0/totalErrorCnt" color="#1f77b4">
|
||||
<transform alias="/pandaStates/0/canState0/totalErrorCnt[Derivative]" name="Derivative">
|
||||
<options radioChecked="radioCustom" lineEdit="1.0"/>
|
||||
</transform>
|
||||
</curve>
|
||||
<curve name="/pandaStates/0/canState1/totalErrorCnt" color="#d62728">
|
||||
<transform alias="/pandaStates/0/canState1/totalErrorCnt[Derivative]" name="Derivative">
|
||||
<options radioChecked="radioCustom" lineEdit="1.0"/>
|
||||
</transform>
|
||||
</curve>
|
||||
<curve name="/pandaStates/0/canState2/totalErrorCnt" color="#1ac938">
|
||||
<transform alias="/pandaStates/0/canState2/totalErrorCnt[Derivative]" name="Derivative">
|
||||
<options radioChecked="radioCustom" lineEdit="1.0"/>
|
||||
</transform>
|
||||
</curve>
|
||||
</plot>
|
||||
</DockArea>
|
||||
</DockSplitter>
|
||||
</Container>
|
||||
</Tab>
|
||||
<currentTabIndex index="0"/>
|
||||
</tabbed_widget>
|
||||
<use_relative_time_offset enabled="1"/>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<Plugins>
|
||||
<plugin ID="DataLoad Rlog"/>
|
||||
<plugin ID="Cereal Subscriber"/>
|
||||
</Plugins>
|
||||
<customMathEquations/>
|
||||
<snippets/>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
</root>
|
||||
|
||||
148
tools/plotjuggler/layouts/camera-timings.xml
Normal file
148
tools/plotjuggler/layouts/camera-timings.xml
Normal file
@@ -0,0 +1,148 @@
|
||||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<root>
|
||||
<tabbed_widget name="Main Window" parent="main_window">
|
||||
<Tab tab_name="SOF / EOF (encodeIdx)" containers="1">
|
||||
<Container>
|
||||
<DockSplitter orientation="-" sizes="0.500885;0.499115" count="2">
|
||||
<DockArea name="...">
|
||||
<plot flip_x="false" mode="TimeSeries" flip_y="false" style="Lines">
|
||||
<range bottom="35000000.000000" left="0.000000" top="65000000.000000" right="630.006367"/>
|
||||
<limitY max="6.5e+07" min="3.5e+07"/>
|
||||
<curve color="#1f77b4" name="/driverEncodeIdx/timestampSof">
|
||||
<transform name="Derivative" alias="/driverEncodeIdx/timestampSof[Derivative]">
|
||||
<options lineEdit="1.0" radioChecked="radioCustom"/>
|
||||
</transform>
|
||||
</curve>
|
||||
<curve color="#d62728" name="/roadEncodeIdx/timestampSof">
|
||||
<transform name="Derivative" alias="/roadEncodeIdx/timestampSof[Derivative]">
|
||||
<options lineEdit="1.0" radioChecked="radioCustom"/>
|
||||
</transform>
|
||||
</curve>
|
||||
<curve color="#1ac938" name="/wideRoadEncodeIdx/timestampSof">
|
||||
<transform name="Derivative" alias="/wideRoadEncodeIdx/timestampSof[Derivative]">
|
||||
<options lineEdit="1.0" radioChecked="radioCustom"/>
|
||||
</transform>
|
||||
</curve>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot flip_x="false" mode="TimeSeries" flip_y="false" style="Lines">
|
||||
<range bottom="35000000.000000" left="0.000000" top="65000000.000000" right="630.006367"/>
|
||||
<limitY max="6.5e+07" min="3.5e+07"/>
|
||||
<curve color="#f14cc1" name="/driverEncodeIdx/timestampEof">
|
||||
<transform name="Derivative" alias="/driverEncodeIdx/timestampEof[Derivative]">
|
||||
<options lineEdit="1.0" radioChecked="radioCustom"/>
|
||||
</transform>
|
||||
</curve>
|
||||
<curve color="#9467bd" name="/roadEncodeIdx/timestampEof">
|
||||
<transform name="Derivative" alias="/roadEncodeIdx/timestampEof[Derivative]">
|
||||
<options lineEdit="1.0" radioChecked="radioCustom"/>
|
||||
</transform>
|
||||
</curve>
|
||||
<curve color="#17becf" name="/wideRoadEncodeIdx/timestampEof">
|
||||
<transform name="Derivative" alias="/wideRoadEncodeIdx/timestampEof[Derivative]">
|
||||
<options lineEdit="1.0" radioChecked="radioCustom"/>
|
||||
</transform>
|
||||
</curve>
|
||||
</plot>
|
||||
</DockArea>
|
||||
</DockSplitter>
|
||||
</Container>
|
||||
</Tab>
|
||||
<Tab tab_name="model timings" containers="1">
|
||||
<Container>
|
||||
<DockSplitter orientation="-" sizes="0.5;0.5" count="2">
|
||||
<DockArea name="...">
|
||||
<plot flip_x="false" mode="TimeSeries" flip_y="false" style="Lines">
|
||||
<range bottom="0.015143" left="0.000000" top="0.016865" right="630.006367"/>
|
||||
<limitY/>
|
||||
<curve color="#ff7f0e" name="/modelV2/modelExecutionTime"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot flip_x="false" mode="TimeSeries" flip_y="false" style="Lines">
|
||||
<range bottom="-0.100000" left="0.000000" top="0.100000" right="630.006367"/>
|
||||
<limitY/>
|
||||
<curve color="#f14cc1" name="/modelV2/frameDropPerc"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
</DockSplitter>
|
||||
</Container>
|
||||
</Tab>
|
||||
<Tab tab_name="sensor info" containers="1">
|
||||
<Container>
|
||||
<DockSplitter orientation="-" sizes="1" count="1">
|
||||
<DockArea name="...">
|
||||
<plot flip_x="false" mode="TimeSeries" flip_y="false" style="Lines">
|
||||
<range bottom="-0.100000" left="0.000000" top="0.100000" right="630.006367"/>
|
||||
<limitY/>
|
||||
<curve color="#bcbd22" name="/driverCameraState/sensor"/>
|
||||
<curve color="#1f77b4" name="/roadCameraState/sensor"/>
|
||||
<curve color="#d62728" name="/wideRoadCameraState/sensor"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
</DockSplitter>
|
||||
</Container>
|
||||
</Tab>
|
||||
<Tab tab_name="SOF / EOF (cameraState)" containers="1">
|
||||
<Container>
|
||||
<DockSplitter orientation="-" sizes="0.500885;0.499115" count="2">
|
||||
<DockArea name="...">
|
||||
<plot flip_x="false" mode="TimeSeries" flip_y="false" style="Lines">
|
||||
<range bottom="35000000.000000" left="0.000000" top="65000000.000000" right="630.006367"/>
|
||||
<limitY max="6.5e+07" min="3.5e+07"/>
|
||||
<curve color="#1f77b4" name="/driverCameraState/timestampSof">
|
||||
<transform name="Derivative" alias="/driverCameraState/timestampSof[Derivative]">
|
||||
<options lineEdit="1.0" radioChecked="radioCustom"/>
|
||||
</transform>
|
||||
</curve>
|
||||
<curve color="#d62728" name="/roadCameraState/timestampSof">
|
||||
<transform name="Derivative" alias="/roadCameraState/timestampSof[Derivative]">
|
||||
<options lineEdit="1.0" radioChecked="radioCustom"/>
|
||||
</transform>
|
||||
</curve>
|
||||
<curve color="#1ac938" name="/wideRoadCameraState/timestampSof">
|
||||
<transform name="Derivative" alias="/wideRoadCameraState/timestampSof[Derivative]">
|
||||
<options lineEdit="1.0" radioChecked="radioCustom"/>
|
||||
</transform>
|
||||
</curve>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot flip_x="false" mode="TimeSeries" flip_y="false" style="Lines">
|
||||
<range bottom="35000000.000000" left="0.000000" top="65000000.000000" right="630.006367"/>
|
||||
<limitY max="6.5e+07" min="3.5e+07"/>
|
||||
<curve color="#ff7f0e" name="/driverCameraState/timestampEof">
|
||||
<transform name="Derivative" alias="/driverCameraState/timestampEof[Derivative]">
|
||||
<options lineEdit="1.0" radioChecked="radioCustom"/>
|
||||
</transform>
|
||||
</curve>
|
||||
<curve color="#f14cc1" name="/roadCameraState/timestampEof">
|
||||
<transform name="Derivative" alias="/roadCameraState/timestampEof[Derivative]">
|
||||
<options lineEdit="1.0" radioChecked="radioCustom"/>
|
||||
</transform>
|
||||
</curve>
|
||||
<curve color="#9467bd" name="/wideRoadCameraState/timestampEof">
|
||||
<transform name="Derivative" alias="/wideRoadCameraState/timestampEof[Derivative]">
|
||||
<options lineEdit="1.0" radioChecked="radioCustom"/>
|
||||
</transform>
|
||||
</curve>
|
||||
</plot>
|
||||
</DockArea>
|
||||
</DockSplitter>
|
||||
</Container>
|
||||
</Tab>
|
||||
<currentTabIndex index="0"/>
|
||||
</tabbed_widget>
|
||||
<use_relative_time_offset enabled="1"/>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<Plugins>
|
||||
<plugin ID="DataLoad Rlog"/>
|
||||
<plugin ID="Cereal Subscriber"/>
|
||||
</Plugins>
|
||||
<customMathEquations/>
|
||||
<snippets/>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
</root>
|
||||
|
||||
72
tools/plotjuggler/layouts/can-states.xml
Normal file
72
tools/plotjuggler/layouts/can-states.xml
Normal file
@@ -0,0 +1,72 @@
|
||||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<root>
|
||||
<tabbed_widget parent="main_window" name="Main Window">
|
||||
<Tab containers="1" tab_name="tab1">
|
||||
<Container>
|
||||
<DockSplitter count="2" sizes="0.500381;0.499619" orientation="-">
|
||||
<DockSplitter count="2" sizes="0.5;0.5" orientation="|">
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" mode="TimeSeries" flip_y="false" flip_x="false">
|
||||
<range right="632.799721" bottom="-17755.925000" top="771630.925000" left="0.000000"/>
|
||||
<limitY/>
|
||||
<curve color="#1f77b4" name="/pandaStates/0/canState0/totalRxCnt"/>
|
||||
<curve color="#d62728" name="/pandaStates/0/canState1/totalRxCnt"/>
|
||||
<curve color="#1ac938" name="/pandaStates/0/canState2/totalRxCnt"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" mode="TimeSeries" flip_y="false" flip_x="false">
|
||||
<range right="632.799721" bottom="-18545.500000" top="760365.500000" left="0.000000"/>
|
||||
<limitY/>
|
||||
<curve color="#ff7f0e" name="/pandaStates/0/canState0/totalTxCnt"/>
|
||||
<curve color="#f14cc1" name="/pandaStates/0/canState1/totalTxCnt"/>
|
||||
<curve color="#9467bd" name="/pandaStates/0/canState2/totalTxCnt"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
</DockSplitter>
|
||||
<DockSplitter count="3" sizes="0.333333;0.333333;0.333333" orientation="|">
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" mode="TimeSeries" flip_y="false" flip_x="false">
|
||||
<range right="632.799721" bottom="-1.350000" top="55.350000" left="0.000000"/>
|
||||
<limitY/>
|
||||
<curve color="#ff7f0e" name="/pandaStates/0/canState0/totalRxLostCnt"/>
|
||||
<curve color="#f14cc1" name="/pandaStates/0/canState1/totalRxLostCnt"/>
|
||||
<curve color="#9467bd" name="/pandaStates/0/canState2/totalRxLostCnt"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" mode="TimeSeries" flip_y="false" flip_x="false">
|
||||
<range right="632.799721" bottom="-0.050000" top="2.050000" left="0.000000"/>
|
||||
<limitY/>
|
||||
<curve color="#17becf" name="/pandaStates/0/canState0/totalTxLostCnt"/>
|
||||
<curve color="#bcbd22" name="/pandaStates/0/canState1/totalTxLostCnt"/>
|
||||
<curve color="#1f77b4" name="/pandaStates/0/canState2/totalTxLostCnt"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" mode="TimeSeries" flip_y="false" flip_x="false">
|
||||
<range right="632.799721" bottom="-0.100000" top="0.100000" left="0.000000"/>
|
||||
<limitY/>
|
||||
<curve color="#17becf" name="/pandaStates/0/canState0/busOffCnt"/>
|
||||
<curve color="#1ac938" name="/pandaStates/0/canState1/busOffCnt"/>
|
||||
<curve color="#bcbd22" name="/pandaStates/0/canState2/busOffCnt"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
</DockSplitter>
|
||||
</DockSplitter>
|
||||
</Container>
|
||||
</Tab>
|
||||
<currentTabIndex index="0"/>
|
||||
</tabbed_widget>
|
||||
<use_relative_time_offset enabled="1"/>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<Plugins>
|
||||
<plugin ID="DataLoad Rlog"/>
|
||||
<plugin ID="Cereal Subscriber"/>
|
||||
</Plugins>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
</root>
|
||||
|
||||
61
tools/plotjuggler/layouts/controls_mismatch_debug.xml
Normal file
61
tools/plotjuggler/layouts/controls_mismatch_debug.xml
Normal file
@@ -0,0 +1,61 @@
|
||||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<root>
|
||||
<tabbed_widget parent="main_window" name="Main Window">
|
||||
<Tab tab_name="tab1" containers="1">
|
||||
<Container>
|
||||
<DockSplitter orientation="-" count="5" sizes="0.2;0.2;0.2;0.2;0.2">
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" flip_x="false" mode="TimeSeries" flip_y="false">
|
||||
<range top="1.025000" bottom="-0.025000" left="0.018309" right="59.674401"/>
|
||||
<limitY/>
|
||||
<curve color="#1f77b4" name="/carControl/enabled"/>
|
||||
<curve color="#d62728" name="/pandaStates/0/controlsAllowed"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" flip_x="false" mode="TimeSeries" flip_y="false">
|
||||
<range top="27.087398" bottom="-0.905168" left="0.018309" right="59.674401"/>
|
||||
<limitY/>
|
||||
<curve color="#9467bd" name="/controlsState/cumLagMs"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" flip_x="false" mode="TimeSeries" flip_y="false">
|
||||
<range top="1.025000" bottom="-0.025000" left="0.018309" right="59.674401"/>
|
||||
<limitY/>
|
||||
<curve color="#1f77b4" name="/pandaStates/0/safetyRxInvalid"/>
|
||||
<curve color="#e801ce" name="/pandaStates/0/safetyRxChecksInvalid"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" flip_x="false" mode="TimeSeries" flip_y="false">
|
||||
<range top="158.850000" bottom="-2.850000" left="0.018309" right="59.674401"/>
|
||||
<limitY/>
|
||||
<curve color="#d62728" name="/pandaStates/0/safetyTxBlocked"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" flip_x="false" mode="TimeSeries" flip_y="false">
|
||||
<range top="1.025000" bottom="-0.025000" left="0.018309" right="59.674401"/>
|
||||
<limitY/>
|
||||
<curve color="#1ac938" name="/carState/gasPressed"/>
|
||||
<curve color="#ff7f0e" name="/carState/brakePressed"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
</DockSplitter>
|
||||
</Container>
|
||||
</Tab>
|
||||
<currentTabIndex index="0"/>
|
||||
</tabbed_widget>
|
||||
<use_relative_time_offset enabled="1"/>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<Plugins>
|
||||
<plugin ID="DataLoad Rlog"/>
|
||||
<plugin ID="Cereal Subscriber"/>
|
||||
</Plugins>
|
||||
<customMathEquations/>
|
||||
<snippets/>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
</root>
|
||||
|
||||
62
tools/plotjuggler/layouts/gps.xml
Normal file
62
tools/plotjuggler/layouts/gps.xml
Normal file
@@ -0,0 +1,62 @@
|
||||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<root>
|
||||
<tabbed_widget parent="main_window" name="Main Window">
|
||||
<Tab tab_name="tab1" containers="1">
|
||||
<Container>
|
||||
<DockSplitter orientation="-" sizes="0.24977;0.250689;0.24977;0.24977" count="4">
|
||||
<DockArea name="...">
|
||||
<plot flip_y="false" flip_x="false" style="Lines" mode="TimeSeries">
|
||||
<range left="0.000000" right="1678.753571" bottom="-0.025000" top="1.025000"/>
|
||||
<limitY/>
|
||||
<curve name="/gpsLocationExternal/hasFix" color="#1f77b4"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot flip_y="false" flip_x="false" style="Lines" mode="TimeSeries">
|
||||
<range left="0.000000" right="1678.753571" bottom="-0.425000" top="17.425000"/>
|
||||
<limitY/>
|
||||
<curve name="/gpsLocationExternal/satelliteCount" color="#d62728"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot flip_y="false" flip_x="false" style="Lines" mode="TimeSeries">
|
||||
<range left="0.000000" right="1678.753571" bottom="0.000000" top="3.000000"/>
|
||||
<limitY max="3" min="0"/>
|
||||
<curve name="/gpsLocationExternal/horizontalAccuracy" color="#1ac938"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot flip_y="false" flip_x="false" style="Lines" mode="TimeSeries">
|
||||
<range left="0.000000" right="1678.753571" bottom="-17.262000" top="766.374004"/>
|
||||
<limitY/>
|
||||
<curve name="/gpsLocationExternal/horizontalAccuracy" color="#1ac938"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
</DockSplitter>
|
||||
</Container>
|
||||
</Tab>
|
||||
<currentTabIndex index="0"/>
|
||||
</tabbed_widget>
|
||||
<use_relative_time_offset enabled="1"/>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<Plugins>
|
||||
<plugin ID="DataLoad CSV">
|
||||
<default time_axis="" delimiter="0"/>
|
||||
</plugin>
|
||||
<plugin ID="DataLoad Rlog"/>
|
||||
<plugin ID="DataLoad ULog"/>
|
||||
<plugin ID="Cereal Subscriber"/>
|
||||
<plugin ID="UDP Server"/>
|
||||
<plugin ID="WebSocket Server"/>
|
||||
<plugin ID="ZMQ Subscriber"/>
|
||||
<plugin ID="Fast Fourier Transform"/>
|
||||
<plugin ID="Quaternion to RPY"/>
|
||||
<plugin ID="CSV Exporter"/>
|
||||
</Plugins>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<customMathEquations/>
|
||||
<snippets/>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
</root>
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user