merge in tools
George Hotz
parent
80d6953862
commit
29ac3da7b8
|
|
@ -0,0 +1,21 @@
|
|||
MIT License
|
||||
|
||||
Copyright (c) 2018 comma.ai
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
copies of the Software, and to permit persons to whom the Software is
|
||||
furnished to do so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in all
|
||||
copies or substantial portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
SOFTWARE.
|
||||
|
|
@ -0,0 +1,294 @@
|
|||
openpilot-tools
|
||||
============
|
||||
|
||||
Repo which contains tools to facilitate development and debugging of [openpilot](openpilot.comma.ai).
|
||||
|
||||
|
||||
|
||||
|
||||
Table of Contents
|
||||
============
|
||||
|
||||
<!--ts-->
|
||||
* [Requirements](#requirements)
|
||||
* [Setup](#setup)
|
||||
* [Tool examples](#tool-examples)
|
||||
* [Replay driving data](#replay-driving-data)
|
||||
* [Debug car controls](#debug-car-controls)
|
||||
* [Stream replayed CAN messages to EON](#stream-replayed-can-messages-to-eon)
|
||||
* [Stream EON video data to a PC](#stream-eon-video-data-to-a-pc)
|
||||
* [Welcomed contributions](#welcomed-contributions)
|
||||
<!--te-->
|
||||
|
||||
|
||||
Requirements
|
||||
============
|
||||
|
||||
openpilot-tools and the following setup steps are developed and tested on Ubuntu 16.04, MacOS 10.14.2 and Python 3.7.3.
|
||||
|
||||
Setup
|
||||
============
|
||||
|
||||
|
||||
1. Install native dependencies (Mac and Ubuntu sections listed below)
|
||||
|
||||
**Ubuntu**
|
||||
|
||||
- core tools
|
||||
```bash
|
||||
sudo apt install git curl python-pip
|
||||
sudo pip install --upgrade pip>=18.0 pipenv
|
||||
```
|
||||
|
||||
- ffmpeg (tested with 3.3.2)
|
||||
```bash
|
||||
sudo apt install ffmpeg libavformat-dev libavcodec-dev libavdevice-dev libavutil-dev libswscale-dev libavresample-dev libavfilter-dev
|
||||
```
|
||||
|
||||
- build tools
|
||||
```bash
|
||||
sudo apt install autoconf automake clang clang-3.8 libtool pkg-config build-essential
|
||||
```
|
||||
|
||||
- libarchive-dev (tested with 3.1.2-11ubuntu0.16.04.4)
|
||||
```bash
|
||||
sudo apt install libarchive-dev
|
||||
```
|
||||
|
||||
- qt python binding (tested with python-qt4, 4.11.4+dfsg-1build4)
|
||||
```bash
|
||||
sudo apt install python-qt4
|
||||
```
|
||||
|
||||
- zmq 4.2.3 (required for replay)
|
||||
```bash
|
||||
curl -LO https://github.com/zeromq/libzmq/releases/download/v4.2.3/zeromq-4.2.3.tar.gz
|
||||
tar xfz zeromq-4.2.3.tar.gz
|
||||
cd zeromq-4.2.3
|
||||
./autogen.sh
|
||||
./configure CPPFLAGS=-DPIC CFLAGS=-fPIC CXXFLAGS=-fPIC LDFLAGS=-fPIC --disable-shared --enable-static
|
||||
make
|
||||
sudo make install
|
||||
```
|
||||
|
||||
**Mac**
|
||||
|
||||
- brew
|
||||
``` bash
|
||||
/usr/bin/ruby -e "$(curl -fsSL https://raw.githubusercontent.com/Homebrew/install/master/install)"
|
||||
```
|
||||
|
||||
- core tools
|
||||
``` bash
|
||||
brew install git
|
||||
sudo pip install --upgrade pip pipenv
|
||||
xcode-select --install
|
||||
```
|
||||
|
||||
- ffmpeg (tested with 3.4.1)
|
||||
```bash
|
||||
brew install ffmpeg
|
||||
```
|
||||
|
||||
- build tools
|
||||
```bash
|
||||
brew install autoconf automake libtool llvm pkg-config
|
||||
```
|
||||
|
||||
- libarchive-dev (tested with 3.3.3)
|
||||
```bash
|
||||
brew install libarchive
|
||||
```
|
||||
|
||||
- qt for Mac
|
||||
```bash
|
||||
brew install qt
|
||||
```
|
||||
|
||||
- zmq 4.3.1 (required for replay)
|
||||
```bash
|
||||
brew install zeromq
|
||||
```
|
||||
|
||||
2. Install Cap'n Proto
|
||||
|
||||
```bash
|
||||
curl -O https://capnproto.org/capnproto-c++-0.6.1.tar.gz
|
||||
tar xvf capnproto-c++-0.6.1.tar.gz
|
||||
cd capnproto-c++-0.6.1
|
||||
./configure --prefix=/usr/local CPPFLAGS=-DPIC CFLAGS=-fPIC CXXFLAGS=-fPIC LDFLAGS=-fPIC --disable-shared --enable-static
|
||||
make -j4
|
||||
sudo make install
|
||||
|
||||
cd ..
|
||||
git clone https://github.com/commaai/c-capnproto.git
|
||||
cd c-capnproto
|
||||
git submodule update --init --recursive
|
||||
autoreconf -f -i -s
|
||||
CFLAGS="-fPIC" ./configure --prefix=/usr/local
|
||||
make -j4
|
||||
sudo make install
|
||||
```
|
||||
|
||||
|
||||
|
||||
|
||||
2. Clone openpilot if you haven't already
|
||||
|
||||
```bash
|
||||
git clone https://github.com/commaai/openpilot.git
|
||||
cd openpilot
|
||||
pipenv install # Install dependencies in a virtualenv
|
||||
pipenv shell # Activate the virtualenv
|
||||
```
|
||||
|
||||
**For Mac users**
|
||||
|
||||
Recompile longitudinal_mpc for mac
|
||||
|
||||
Navigate to:
|
||||
``` bash
|
||||
cd selfdrive/controls/lib/longitudinal_mpc
|
||||
make clean
|
||||
make
|
||||
```
|
||||
|
||||
3. Clone tools within openpilot, and install dependencies
|
||||
|
||||
```bash
|
||||
git clone https://github.com/commaai/openpilot-tools.git tools
|
||||
cd tools
|
||||
git checkout <tag> # the tag must match the openpilot version you are using (see https://github.com/commaai/openpilot-tools/tags)
|
||||
pip install -r requirements.txt # Install openpilot-tools dependencies in virtualenv
|
||||
```
|
||||
|
||||
4. Add openpilot to your `PYTHONPATH`.
|
||||
|
||||
For bash users:
|
||||
```bash
|
||||
echo 'export PYTHONPATH="$PYTHONPATH:<path-to-openpilot>"' >> ~/.bashrc
|
||||
source ~/.bashrc
|
||||
```
|
||||
|
||||
5. Add some folders to root
|
||||
```bash
|
||||
sudo mkdir /data
|
||||
sudo mkdir /data/params
|
||||
sudo chown $USER /data/params
|
||||
```
|
||||
|
||||
6. Try out some tools!
|
||||
|
||||
|
||||
Tool examples
|
||||
============
|
||||
|
||||
|
||||
Replay driving data
|
||||
-------------
|
||||
|
||||
**Hardware needed**: none
|
||||
|
||||
`unlogger.py` replays data collected with [chffrplus](https://github.com/commaai/chffrplus) or [openpilot](https://github.com/commaai/openpilot).
|
||||
|
||||
You'll need to download log and camera files into a local directory. Download these from the footer of the comma [explorer](https://my.comma.ai) or SCP from your device.
|
||||
|
||||
Usage:
|
||||
|
||||
```
|
||||
python replay/unlogger.py <route-name> <path-to-data-directory>
|
||||
|
||||
#Example:
|
||||
|
||||
#python replay/unlogger.py '99c94dc769b5d96e|2018-11-14--13-31-42' /home/batman/unlogger_data
|
||||
|
||||
#Within /home/batman/unlogger_data:
|
||||
# 99c94dc769b5d96e|2018-11-14--13-31-42--0--fcamera.hevc
|
||||
# 99c94dc769b5d96e|2018-11-14--13-31-42--0--rlog.bz2
|
||||
# ...
|
||||
|
||||
# In another terminal you can run a debug visualizer:
|
||||
python replay/ui.py # Define the environmental variable HORIZONTAL is the ui layout is too tall
|
||||
```
|
||||
|
||||
|
||||
|
||||
Debug car controls
|
||||
-------------
|
||||
|
||||
**Hardware needed**: [panda](panda.comma.ai), [giraffe](https://comma.ai/shop/products/giraffe/), joystick
|
||||
|
||||
Use the panda's OBD-II port to connect with your car and a usb cable to connect the panda to your pc.
|
||||
Also, connect a joystick to your pc.
|
||||
|
||||
`joystickd.py` runs a deamon that reads inputs from a joystick and publishes them over zmq.
|
||||
`boardd.py` sends the CAN messages from your pc to the panda.
|
||||
`debug_controls` is a mocked version of `controlsd.py` and uses input from a joystick to send controls to your car.
|
||||
|
||||
Usage:
|
||||
```
|
||||
python carcontrols/joystickd.py
|
||||
|
||||
# In another terminal:
|
||||
selfdrive/boardd/tests/boardd_old.py # Make sure the safety setting is hardcoded to ALL_OUTPUT
|
||||
|
||||
# In another terminal:
|
||||
python carcontrols/debug_controls.py
|
||||
|
||||
```
|
||||
|
||||
|
||||
|
||||
Stream replayed CAN messages to EON
|
||||
-------------
|
||||
|
||||
**Hardware needed**: 2 x [panda](panda.comma.ai), [debug board](https://comma.ai/shop/products/panda-debug-board/), [EON](https://comma.ai/shop/products/eon-gold-dashcam-devkit/).
|
||||
|
||||
It is possible to replay CAN messages as they were recorded and forward them to a EON.
|
||||
Connect 2 pandas to the debug board. A panda connects to the PC, the other panda connects to the EON.
|
||||
|
||||
Usage:
|
||||
```
|
||||
# With MOCK=1 boardd will read logged can messages from a replay and send them to the panda.
|
||||
MOCK=1 selfdrive/boardd/tests/boardd_old.py
|
||||
|
||||
# In another terminal:
|
||||
python replay/unlogger.py <route-name> <path-to-data-directory>
|
||||
|
||||
```
|
||||
|
||||
|
||||
|
||||
Stream EON video data to a PC
|
||||
-------------
|
||||
|
||||
**Hardware needed**: [EON](https://comma.ai/shop/products/eon-gold-dashcam-devkit/), [comma Smays](https://comma.ai/shop/products/comma-smays-adapter/).
|
||||
|
||||
You can connect your EON to your pc using the Ethernet cable provided with the comma Smays and you'll be able to stream data from your EON, in real time, with low latency. A useful application is being able to stream the raw video frames at 20fps, as captured by the EON's camera.
|
||||
|
||||
Usage:
|
||||
```
|
||||
# ssh into the eon and run loggerd with the flag "--stream". In ../selfdrive/manager.py you can change:
|
||||
# ...
|
||||
# "loggerd": ("selfdrive/loggerd", ["./loggerd"]),
|
||||
# ...
|
||||
# with:
|
||||
# ...
|
||||
# "loggerd": ("selfdrive/loggerd", ["./loggerd", "--stream"]),
|
||||
# ...
|
||||
|
||||
# On the PC:
|
||||
# To receive frames from the EON and re-publish them. Set PYGAME env variable if you want to display the video stream
|
||||
python streamer/streamerd.py
|
||||
```
|
||||
|
||||
|
||||
|
||||
|
||||
Welcomed contributions
|
||||
=============
|
||||
|
||||
* Documentation: code comments, better tutorials, etc..
|
||||
* Support for other platforms other than Ubuntu 16.04.
|
||||
* Performance improvements: the tools have been developed on high-performance workstations (12+ logical cores with 32+ GB of RAM), so they are not optimized for running efficiently. For example, `ui.py` might not be able to run real-time on most PCs.
|
||||
* More tools: anything that you think might be helpful to others.
|
||||
|
|
@ -0,0 +1,100 @@
|
|||
#!/usr/bin/env python
|
||||
import struct
|
||||
from common.numpy_fast import clip
|
||||
from common.params import Params
|
||||
from copy import copy
|
||||
from cereal import car, log
|
||||
import cereal.messaging as messaging
|
||||
from selfdrive.car.car_helpers import get_car
|
||||
from selfdrive.boardd.boardd import can_list_to_can_capnp
|
||||
|
||||
HwType = log.HealthData.HwType
|
||||
|
||||
|
||||
def steer_thread():
|
||||
poller = messaging.Poller()
|
||||
|
||||
logcan = messaging.sub_sock('can')
|
||||
health = messaging.sub_sock('health')
|
||||
joystick_sock = messaging.sub_sock('testJoystick', conflate=True, poller=poller)
|
||||
|
||||
carstate = messaging.pub_sock('carState')
|
||||
carcontrol = messaging.pub_sock('carControl')
|
||||
sendcan = messaging.pub_sock('sendcan')
|
||||
|
||||
button_1_last = 0
|
||||
enabled = False
|
||||
|
||||
# wait for health and CAN packets
|
||||
hw_type = messaging.recv_one(health).health.hwType
|
||||
has_relay = hw_type in [HwType.blackPanda, HwType.uno]
|
||||
print("Waiting for CAN messages...")
|
||||
messaging.get_one_can(logcan)
|
||||
|
||||
CI, CP = get_car(logcan, sendcan, has_relay)
|
||||
Params().put("CarParams", CP.to_bytes())
|
||||
|
||||
CC = car.CarControl.new_message()
|
||||
|
||||
while True:
|
||||
|
||||
# send
|
||||
joystick = messaging.recv_one(joystick_sock)
|
||||
can_strs = messaging.drain_sock_raw(logcan, wait_for_one=True)
|
||||
CS = CI.update(CC, can_strs)
|
||||
|
||||
# Usually axis run in pairs, up/down for one, and left/right for
|
||||
# the other.
|
||||
actuators = car.CarControl.Actuators.new_message()
|
||||
|
||||
if joystick is not None:
|
||||
axis_3 = clip(-joystick.testJoystick.axes[3] * 1.05, -1., 1.) # -1 to 1
|
||||
actuators.steer = axis_3
|
||||
actuators.steerAngle = axis_3 * 43. # deg
|
||||
axis_1 = clip(-joystick.testJoystick.axes[1] * 1.05, -1., 1.) # -1 to 1
|
||||
actuators.gas = max(axis_1, 0.)
|
||||
actuators.brake = max(-axis_1, 0.)
|
||||
|
||||
pcm_cancel_cmd = joystick.testJoystick.buttons[0]
|
||||
button_1 = joystick.testJoystick.buttons[1]
|
||||
if button_1 and not button_1_last:
|
||||
enabled = not enabled
|
||||
|
||||
button_1_last = button_1
|
||||
|
||||
#print "enable", enabled, "steer", actuators.steer, "accel", actuators.gas - actuators.brake
|
||||
|
||||
hud_alert = 0
|
||||
audible_alert = 0
|
||||
if joystick.testJoystick.buttons[2]:
|
||||
audible_alert = "beepSingle"
|
||||
if joystick.testJoystick.buttons[3]:
|
||||
audible_alert = "chimeRepeated"
|
||||
hud_alert = "steerRequired"
|
||||
|
||||
CC.actuators.gas = actuators.gas
|
||||
CC.actuators.brake = actuators.brake
|
||||
CC.actuators.steer = actuators.steer
|
||||
CC.actuators.steerAngle = actuators.steerAngle
|
||||
CC.hudControl.visualAlert = hud_alert
|
||||
CC.hudControl.setSpeed = 20
|
||||
CC.cruiseControl.cancel = pcm_cancel_cmd
|
||||
CC.enabled = enabled
|
||||
can_sends = CI.apply(CC)
|
||||
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
|
||||
|
||||
# broadcast carState
|
||||
cs_send = messaging.new_message()
|
||||
cs_send.init('carState')
|
||||
cs_send.carState = copy(CS)
|
||||
carstate.send(cs_send.to_bytes())
|
||||
|
||||
# broadcast carControl
|
||||
cc_send = messaging.new_message()
|
||||
cc_send.init('carControl')
|
||||
cc_send.carControl = copy(CC)
|
||||
carcontrol.send(cc_send.to_bytes())
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
steer_thread()
|
||||
|
|
@ -0,0 +1,125 @@
|
|||
#!/usr/bin/env python
|
||||
import pygame
|
||||
|
||||
# Define some colors
|
||||
BLACK = ( 0, 0, 0)
|
||||
WHITE = ( 255, 255, 255)
|
||||
|
||||
# This is a simple class that will help us print to the screen
|
||||
# It has nothing to do with the joysticks, just outputting the
|
||||
# information.
|
||||
class TextPrint:
|
||||
def __init__(self):
|
||||
self.reset()
|
||||
self.font = pygame.font.Font(None, 20)
|
||||
|
||||
def printf(self, screen, textString):
|
||||
textBitmap = self.font.render(textString, True, BLACK)
|
||||
screen.blit(textBitmap, [self.x, self.y])
|
||||
self.y += self.line_height
|
||||
|
||||
def reset(self):
|
||||
self.x = 10
|
||||
self.y = 10
|
||||
self.line_height = 15
|
||||
|
||||
def indent(self):
|
||||
self.x += 10
|
||||
|
||||
def unindent(self):
|
||||
self.x -= 10
|
||||
|
||||
|
||||
pygame.init()
|
||||
|
||||
# Set the width and height of the screen [width,height]
|
||||
size = [500, 700]
|
||||
screen = pygame.display.set_mode(size)
|
||||
|
||||
pygame.display.set_caption("My Game")
|
||||
|
||||
#Loop until the user clicks the close button.
|
||||
done = False
|
||||
|
||||
# Used to manage how fast the screen updates
|
||||
clock = pygame.time.Clock()
|
||||
|
||||
# Initialize the joysticks
|
||||
pygame.joystick.init()
|
||||
|
||||
# Get ready to print
|
||||
textPrint = TextPrint()
|
||||
|
||||
# -------- Main Program Loop -----------
|
||||
while done==False:
|
||||
# EVENT PROCESSING STEP
|
||||
for event in pygame.event.get(): # User did something
|
||||
if event.type == pygame.QUIT: # If user clicked close
|
||||
done=True # Flag that we are done so we exit this loop
|
||||
|
||||
# Possible joystick actions: JOYAXISMOTION JOYBALLMOTION JOYBUTTONDOWN JOYBUTTONUP JOYHATMOTION
|
||||
if event.type == pygame.JOYBUTTONDOWN:
|
||||
print("Joystick button pressed.")
|
||||
if event.type == pygame.JOYBUTTONUP:
|
||||
print("Joystick button released.")
|
||||
|
||||
|
||||
# DRAWING STEP
|
||||
# First, clear the screen to white. Don't put other drawing commands
|
||||
# above this, or they will be erased with this command.
|
||||
screen.fill(WHITE)
|
||||
textPrint.reset()
|
||||
|
||||
# Get count of joysticks
|
||||
joystick_count = pygame.joystick.get_count()
|
||||
|
||||
textPrint.printf(screen, "Number of joysticks: {}".format(joystick_count) )
|
||||
textPrint.indent()
|
||||
|
||||
# For each joystick:
|
||||
joystick = pygame.joystick.Joystick(0)
|
||||
joystick.init()
|
||||
|
||||
textPrint.printf(screen, "Joystick {}".format(0) )
|
||||
textPrint.indent()
|
||||
|
||||
# Get the name from the OS for the controller/joystick
|
||||
name = joystick.get_name()
|
||||
textPrint.printf(screen, "Joystick name: {}".format(name) )
|
||||
|
||||
# Usually axis run in pairs, up/down for one, and left/right for
|
||||
# the other.
|
||||
axes = joystick.get_numaxes()
|
||||
textPrint.printf(screen, "Number of axes: {}".format(axes) )
|
||||
textPrint.indent()
|
||||
|
||||
for i in range( axes ):
|
||||
axis = joystick.get_axis( i )
|
||||
textPrint.printf(screen, "Axis {} value: {:>6.3f}".format(i, axis) )
|
||||
textPrint.unindent()
|
||||
|
||||
buttons = joystick.get_numbuttons()
|
||||
textPrint.printf(screen, "Number of buttons: {}".format(buttons) )
|
||||
textPrint.indent()
|
||||
|
||||
for i in range( buttons ):
|
||||
button = joystick.get_button( i )
|
||||
textPrint.printf(screen, "Button {:>2} value: {}".format(i,button) )
|
||||
textPrint.unindent()
|
||||
|
||||
|
||||
textPrint.unindent()
|
||||
|
||||
|
||||
# ALL CODE TO DRAW SHOULD GO ABOVE THIS COMMENT
|
||||
|
||||
# Go ahead and update the screen with what we've drawn.
|
||||
pygame.display.flip()
|
||||
|
||||
# Limit to 20 frames per second
|
||||
clock.tick(20)
|
||||
|
||||
# Close the window and quit.
|
||||
# If you forget this line, the program will 'hang'
|
||||
# on exit if running from IDLE.
|
||||
pygame.quit ()
|
||||
|
|
@ -0,0 +1,68 @@
|
|||
#!/usr/bin/env python
|
||||
|
||||
# This process publishes joystick events. Such events can be suscribed by
|
||||
# mocked car controller scripts.
|
||||
|
||||
|
||||
### this process needs pygame and can't run on the EON ###
|
||||
|
||||
import pygame
|
||||
import zmq
|
||||
import cereal.messaging as messaging
|
||||
|
||||
|
||||
def joystick_thread():
|
||||
joystick_sock = messaging.pub_sock('testJoystick')
|
||||
|
||||
pygame.init()
|
||||
|
||||
# Used to manage how fast the screen updates
|
||||
clock = pygame.time.Clock()
|
||||
|
||||
# Initialize the joysticks
|
||||
pygame.joystick.init()
|
||||
|
||||
# Get count of joysticks
|
||||
joystick_count = pygame.joystick.get_count()
|
||||
if joystick_count > 1:
|
||||
raise ValueError("More than one joystick attached")
|
||||
elif joystick_count < 1:
|
||||
raise ValueError("No joystick found")
|
||||
|
||||
# -------- Main Program Loop -----------
|
||||
while True:
|
||||
# EVENT PROCESSING STEP
|
||||
for event in pygame.event.get(): # User did something
|
||||
if event.type == pygame.QUIT: # If user clicked close
|
||||
pass
|
||||
# Available joystick events: JOYAXISMOTION JOYBALLMOTION JOYBUTTONDOWN JOYBUTTONUP JOYHATMOTION
|
||||
if event.type == pygame.JOYBUTTONDOWN:
|
||||
print("Joystick button pressed.")
|
||||
if event.type == pygame.JOYBUTTONUP:
|
||||
print("Joystick button released.")
|
||||
|
||||
joystick = pygame.joystick.Joystick(0)
|
||||
joystick.init()
|
||||
|
||||
# Usually axis run in pairs, up/down for one, and left/right for
|
||||
# the other.
|
||||
axes = []
|
||||
buttons = []
|
||||
|
||||
for a in range(joystick.get_numaxes()):
|
||||
axes.append(joystick.get_axis(a))
|
||||
|
||||
for b in range(joystick.get_numbuttons()):
|
||||
buttons.append(bool(joystick.get_button(b)))
|
||||
|
||||
dat = messaging.new_message()
|
||||
dat.init('testJoystick')
|
||||
dat.testJoystick.axes = axes
|
||||
dat.testJoystick.buttons = buttons
|
||||
joystick_sock.send(dat.to_bytes())
|
||||
|
||||
# Limit to 100 frames per second
|
||||
clock.tick(100)
|
||||
|
||||
if __name__ == "__main__":
|
||||
joystick_thread()
|
||||
|
|
@ -0,0 +1 @@
|
|||
cframereader.cpp
|
||||
|
|
@ -0,0 +1,176 @@
|
|||
#include "FrameReader.hpp"
|
||||
#include <assert.h>
|
||||
#include <unistd.h>
|
||||
|
||||
static int ffmpeg_lockmgr_cb(void **arg, enum AVLockOp op) {
|
||||
pthread_mutex_t *mutex = (pthread_mutex_t *)*arg;
|
||||
int err;
|
||||
|
||||
switch (op) {
|
||||
case AV_LOCK_CREATE:
|
||||
mutex = (pthread_mutex_t *)malloc(sizeof(*mutex));
|
||||
if (!mutex)
|
||||
return AVERROR(ENOMEM);
|
||||
if ((err = pthread_mutex_init(mutex, NULL))) {
|
||||
free(mutex);
|
||||
return AVERROR(err);
|
||||
}
|
||||
*arg = mutex;
|
||||
return 0;
|
||||
case AV_LOCK_OBTAIN:
|
||||
if ((err = pthread_mutex_lock(mutex)))
|
||||
return AVERROR(err);
|
||||
|
||||
return 0;
|
||||
case AV_LOCK_RELEASE:
|
||||
if ((err = pthread_mutex_unlock(mutex)))
|
||||
return AVERROR(err);
|
||||
|
||||
return 0;
|
||||
case AV_LOCK_DESTROY:
|
||||
if (mutex)
|
||||
pthread_mutex_destroy(mutex);
|
||||
free(mutex);
|
||||
*arg = NULL;
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
FrameReader::FrameReader(const char *fn) {
|
||||
int ret;
|
||||
|
||||
ret = av_lockmgr_register(ffmpeg_lockmgr_cb);
|
||||
assert(ret >= 0);
|
||||
|
||||
avformat_network_init();
|
||||
av_register_all();
|
||||
|
||||
snprintf(url, sizeof(url)-1, "http://data.comma.life/%s", fn);
|
||||
t = new std::thread([&]() { this->loaderThread(); });
|
||||
}
|
||||
|
||||
void FrameReader::loaderThread() {
|
||||
int ret;
|
||||
|
||||
if (avformat_open_input(&pFormatCtx, url, NULL, NULL) != 0) {
|
||||
fprintf(stderr, "error loading %s\n", url);
|
||||
valid = false;
|
||||
return;
|
||||
}
|
||||
av_dump_format(pFormatCtx, 0, url, 0);
|
||||
|
||||
auto pCodecCtxOrig = pFormatCtx->streams[0]->codec;
|
||||
auto pCodec = avcodec_find_decoder(pCodecCtxOrig->codec_id);
|
||||
assert(pCodec != NULL);
|
||||
|
||||
pCodecCtx = avcodec_alloc_context3(pCodec);
|
||||
ret = avcodec_copy_context(pCodecCtx, pCodecCtxOrig);
|
||||
assert(ret == 0);
|
||||
|
||||
ret = avcodec_open2(pCodecCtx, pCodec, NULL);
|
||||
assert(ret >= 0);
|
||||
|
||||
sws_ctx = sws_getContext(width, height, AV_PIX_FMT_YUV420P,
|
||||
width, height, AV_PIX_FMT_BGR24,
|
||||
SWS_BILINEAR, NULL, NULL, NULL);
|
||||
assert(sws_ctx != NULL);
|
||||
|
||||
AVPacket *pkt = (AVPacket *)malloc(sizeof(AVPacket));
|
||||
assert(pkt != NULL);
|
||||
bool first = true;
|
||||
while (av_read_frame(pFormatCtx, pkt)>=0) {
|
||||
//printf("%d pkt %d %d\n", pkts.size(), pkt->size, pkt->pos);
|
||||
if (first) {
|
||||
AVFrame *pFrame = av_frame_alloc();
|
||||
int frameFinished;
|
||||
avcodec_decode_video2(pCodecCtx, pFrame, &frameFinished, pkt);
|
||||
first = false;
|
||||
}
|
||||
pkts.push_back(pkt);
|
||||
pkt = (AVPacket *)malloc(sizeof(AVPacket));
|
||||
assert(pkt != NULL);
|
||||
}
|
||||
free(pkt);
|
||||
printf("framereader download done\n");
|
||||
joined = true;
|
||||
|
||||
// cache
|
||||
while (1) {
|
||||
GOPCache(to_cache.get());
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void FrameReader::GOPCache(int idx) {
|
||||
AVFrame *pFrame;
|
||||
int gop = idx - idx%15;
|
||||
|
||||
mcache.lock();
|
||||
bool has_gop = cache.find(gop) != cache.end();
|
||||
mcache.unlock();
|
||||
|
||||
if (!has_gop) {
|
||||
//printf("caching %d\n", gop);
|
||||
for (int i = gop; i < gop+15; i++) {
|
||||
if (i >= pkts.size()) break;
|
||||
//printf("decode %d\n", i);
|
||||
int frameFinished;
|
||||
pFrame = av_frame_alloc();
|
||||
avcodec_decode_video2(pCodecCtx, pFrame, &frameFinished, pkts[i]);
|
||||
uint8_t *dat = toRGB(pFrame)->data[0];
|
||||
mcache.lock();
|
||||
cache.insert(std::make_pair(i, dat));
|
||||
mcache.unlock();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
AVFrame *FrameReader::toRGB(AVFrame *pFrame) {
|
||||
AVFrame *pFrameRGB = av_frame_alloc();
|
||||
int numBytes = avpicture_get_size(AV_PIX_FMT_BGR24, pFrame->width, pFrame->height);
|
||||
uint8_t *buffer = (uint8_t *)av_malloc(numBytes*sizeof(uint8_t));
|
||||
avpicture_fill((AVPicture *)pFrameRGB, buffer, AV_PIX_FMT_BGR24, pFrame->width, pFrame->height);
|
||||
sws_scale(sws_ctx, (uint8_t const * const *)pFrame->data,
|
||||
pFrame->linesize, 0, pFrame->height,
|
||||
pFrameRGB->data, pFrameRGB->linesize);
|
||||
return pFrameRGB;
|
||||
}
|
||||
|
||||
uint8_t *FrameReader::get(int idx) {
|
||||
if (!valid) return NULL;
|
||||
waitForReady();
|
||||
// TODO: one line?
|
||||
uint8_t *dat = NULL;
|
||||
|
||||
// lookahead
|
||||
to_cache.put(idx);
|
||||
to_cache.put(idx+15);
|
||||
|
||||
mcache.lock();
|
||||
auto it = cache.find(idx);
|
||||
if (it != cache.end()) {
|
||||
dat = it->second;
|
||||
}
|
||||
mcache.unlock();
|
||||
|
||||
if (dat == NULL) {
|
||||
to_cache.put_front(idx);
|
||||
// lookahead
|
||||
while (dat == NULL) {
|
||||
// wait for frame
|
||||
usleep(50*1000);
|
||||
// check for frame
|
||||
mcache.lock();
|
||||
auto it = cache.find(idx);
|
||||
if (it != cache.end()) dat = it->second;
|
||||
mcache.unlock();
|
||||
if (dat == NULL) {
|
||||
printf(".");
|
||||
fflush(stdout);
|
||||
}
|
||||
}
|
||||
}
|
||||
return dat;
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,58 @@
|
|||
#ifndef FRAMEREADER_HPP
|
||||
#define FRAMEREADER_HPP
|
||||
|
||||
#include <unistd.h>
|
||||
#include <vector>
|
||||
#include <map>
|
||||
#include <thread>
|
||||
#include <mutex>
|
||||
#include <list>
|
||||
#include <condition_variable>
|
||||
|
||||
#include "channel.hpp"
|
||||
|
||||
// independent of QT, needs ffmpeg
|
||||
extern "C" {
|
||||
#include <libavcodec/avcodec.h>
|
||||
#include <libavformat/avformat.h>
|
||||
#include <libswscale/swscale.h>
|
||||
}
|
||||
|
||||
|
||||
class FrameReader {
|
||||
public:
|
||||
FrameReader(const char *fn);
|
||||
uint8_t *get(int idx);
|
||||
AVFrame *toRGB(AVFrame *);
|
||||
void waitForReady() {
|
||||
while (!joined) usleep(10*1000);
|
||||
}
|
||||
int getRGBSize() { return width*height*3; }
|
||||
void loaderThread();
|
||||
void cacherThread();
|
||||
private:
|
||||
AVFormatContext *pFormatCtx = NULL;
|
||||
AVCodecContext *pCodecCtx = NULL;
|
||||
|
||||
struct SwsContext *sws_ctx = NULL;
|
||||
|
||||
int width = 1164;
|
||||
int height = 874;
|
||||
|
||||
std::vector<AVPacket *> pkts;
|
||||
|
||||
std::thread *t;
|
||||
bool joined = false;
|
||||
|
||||
std::map<int, uint8_t *> cache;
|
||||
std::mutex mcache;
|
||||
|
||||
void GOPCache(int idx);
|
||||
channel<int> to_cache;
|
||||
|
||||
bool valid = true;
|
||||
char url[0x400];
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
|
|
@ -0,0 +1,8 @@
|
|||
Import('env')
|
||||
from sysconfig import get_paths
|
||||
env['CPPPATH'] += [get_paths()['include']]
|
||||
|
||||
from Cython.Build import cythonize
|
||||
cythonize("cframereader.pyx")
|
||||
env.SharedLibrary(File('cframereader.so'), ['cframereader.cpp', 'FrameReader.cpp'], LIBS=['avformat', 'avcodec', 'avutil', 'swscale'])
|
||||
|
||||
|
|
@ -0,0 +1,20 @@
|
|||
# distutils: language = c++
|
||||
# cython: language_level=3
|
||||
|
||||
cdef extern from "FrameReader.hpp":
|
||||
cdef cppclass CFrameReader "FrameReader":
|
||||
CFrameReader(const char *)
|
||||
char *get(int)
|
||||
|
||||
cdef class FrameReader():
|
||||
cdef CFrameReader *fr
|
||||
|
||||
def __cinit__(self, fn):
|
||||
self.fr = new CFrameReader(fn)
|
||||
|
||||
def __dealloc__(self):
|
||||
del self.fr
|
||||
|
||||
def get(self, idx):
|
||||
self.fr.get(idx)
|
||||
|
||||
|
|
@ -0,0 +1,35 @@
|
|||
#ifndef CHANNEL_HPP
|
||||
#define CHANNEL_HPP
|
||||
|
||||
#include <mutex>
|
||||
#include <list>
|
||||
#include <condition_variable>
|
||||
|
||||
template<class item>
|
||||
class channel {
|
||||
private:
|
||||
std::list<item> queue;
|
||||
std::mutex m;
|
||||
std::condition_variable cv;
|
||||
public:
|
||||
void put(const item &i) {
|
||||
std::unique_lock<std::mutex> lock(m);
|
||||
queue.push_back(i);
|
||||
cv.notify_one();
|
||||
}
|
||||
void put_front(const item &i) {
|
||||
std::unique_lock<std::mutex> lock(m);
|
||||
queue.push_front(i);
|
||||
cv.notify_one();
|
||||
}
|
||||
item get() {
|
||||
std::unique_lock<std::mutex> lock(m);
|
||||
cv.wait(lock, [&](){ return !queue.empty(); });
|
||||
item result = queue.front();
|
||||
queue.pop_front();
|
||||
return result;
|
||||
}
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
|
|
@ -0,0 +1,352 @@
|
|||
import functools
|
||||
import threading
|
||||
import inspect
|
||||
import sys
|
||||
import select
|
||||
import struct
|
||||
from math import sqrt
|
||||
from collections import OrderedDict, deque
|
||||
from time import time
|
||||
|
||||
from tools.lib.pollable_queue import PollableQueue, Empty, Full, ExistentialError
|
||||
|
||||
EndSentinel = object()
|
||||
|
||||
|
||||
def _sync_inner_generator(input_queue, *args, **kwargs):
|
||||
func = args[0]
|
||||
args = args[1:]
|
||||
|
||||
get = input_queue.get
|
||||
while True:
|
||||
item = get()
|
||||
if item is EndSentinel:
|
||||
return
|
||||
|
||||
cookie, value = item
|
||||
yield cookie, func(value, *args, **kwargs)
|
||||
|
||||
|
||||
def _async_streamer_async_inner(input_queue, output_queue, generator_func, args, kwargs):
|
||||
put = output_queue.put
|
||||
put_end = True
|
||||
try:
|
||||
g = generator_func(input_queue, *args, **kwargs)
|
||||
for item in g:
|
||||
put((time(), item))
|
||||
g.close()
|
||||
except ExistentialError:
|
||||
put_end = False
|
||||
raise
|
||||
finally:
|
||||
if put_end:
|
||||
put((None, EndSentinel))
|
||||
|
||||
def _running_mean_var(ltc_stats, x):
|
||||
old_mean, var = ltc_stats
|
||||
mean = min(600., 0.98 * old_mean + 0.02 * x)
|
||||
var = min(5., max(0.1, 0.98 * var + 0.02 * (mean - x) * (old_mean - x)))
|
||||
return mean, var
|
||||
|
||||
def _find_next_resend(sent_messages, ltc_stats):
|
||||
if not sent_messages:
|
||||
return None, None
|
||||
|
||||
oldest_sent_idx = sent_messages._OrderedDict__root[1][2]
|
||||
send_time, _ = sent_messages[oldest_sent_idx]
|
||||
|
||||
# Assume message has been lost if it is >10 standard deviations from mean.
|
||||
mean, var = ltc_stats
|
||||
next_resend_time = send_time + mean + 40. * sqrt(var)
|
||||
|
||||
return oldest_sent_idx, next_resend_time
|
||||
|
||||
|
||||
def _do_cleanup(input_queue, output_queue, num_workers, sentinels_received, num_outstanding):
|
||||
input_fd = input_queue.put_fd()
|
||||
output_fd = output_queue.get_fd()
|
||||
|
||||
poller = select.epoll()
|
||||
poller.register(input_fd, select.EPOLLOUT)
|
||||
poller.register(output_fd, select.EPOLLIN)
|
||||
|
||||
remaining_outputs = []
|
||||
end_sentinels_to_send = num_workers - sentinels_received
|
||||
while sentinels_received < num_workers:
|
||||
evts = dict(poller.poll(-1 if num_outstanding > 0 else 10.))
|
||||
if not evts:
|
||||
# Workers aren't responding, crash.
|
||||
break
|
||||
|
||||
if output_fd in evts:
|
||||
_, maybe_sentinel = output_queue.get()
|
||||
if maybe_sentinel is EndSentinel:
|
||||
sentinels_received += 1
|
||||
else:
|
||||
remaining_outputs.append(maybe_sentinel[1])
|
||||
num_outstanding -= 1
|
||||
|
||||
if input_fd in evts:
|
||||
if end_sentinels_to_send > 0:
|
||||
input_queue.put_nowait(EndSentinel)
|
||||
end_sentinels_to_send -= 1
|
||||
else:
|
||||
poller.modify(input_fd, 0)
|
||||
|
||||
# TODO: Raise an exception when a queue thread raises one.
|
||||
assert sentinels_received == num_workers, (sentinels_received, num_workers)
|
||||
assert output_queue.empty()
|
||||
return remaining_outputs
|
||||
|
||||
def _generate_results(input_stream, input_queue, worker_output_queue, output_queue,
|
||||
num_workers, max_outstanding):
|
||||
pack_cookie = struct.pack
|
||||
|
||||
# Maps idx -> (send_time, input)
|
||||
sent_messages = OrderedDict()
|
||||
oldest_sent_idx = None
|
||||
next_resend_time = None
|
||||
ltc_stats = 5., 10.
|
||||
|
||||
# Maps idx -> result
|
||||
received_messages = {}
|
||||
next_out = 0
|
||||
|
||||
# Start things off by pulling the first value.
|
||||
next_in_item = next(input_stream, EndSentinel)
|
||||
inputs_remain = next_in_item is not EndSentinel
|
||||
sentinels_received = 0
|
||||
|
||||
input_fd = input_queue.put_fd()
|
||||
worker_output_fd = worker_output_queue.get_fd()
|
||||
output_fd = output_queue.put_fd()
|
||||
|
||||
poller = select.epoll()
|
||||
poller.register(input_fd, select.EPOLLOUT)
|
||||
poller.register(worker_output_fd, select.EPOLLIN)
|
||||
poller.register(output_fd, 0)
|
||||
|
||||
# Keep sending/retrying until the input stream and sent messages are all done.
|
||||
while sentinels_received < num_workers and (inputs_remain or sent_messages):
|
||||
if max_outstanding:
|
||||
can_send_new = (len(sent_messages) < max_outstanding and
|
||||
len(received_messages) < max_outstanding and inputs_remain)
|
||||
else:
|
||||
can_send_new = inputs_remain
|
||||
|
||||
if (next_resend_time and now >= next_resend_time) or can_send_new:
|
||||
poller.modify(input_fd, select.EPOLLOUT)
|
||||
else:
|
||||
poller.modify(input_fd, 0)
|
||||
|
||||
if next_resend_time:
|
||||
t = max(0, next_resend_time - now)
|
||||
evts = dict(poller.poll(t))
|
||||
else:
|
||||
evts = dict(poller.poll())
|
||||
now = time()
|
||||
|
||||
if output_fd in evts:
|
||||
output_queue.put_nowait(received_messages.pop(next_out))
|
||||
next_out += 1
|
||||
|
||||
if next_out not in received_messages:
|
||||
poller.modify(output_fd, 0)
|
||||
|
||||
if worker_output_fd in evts:
|
||||
for receive_time, maybe_sentinel in worker_output_queue.get_multiple_nowait():
|
||||
# Check for EndSentinel in case of worker crash.
|
||||
if maybe_sentinel is EndSentinel:
|
||||
sentinels_received += 1
|
||||
continue
|
||||
idx_bytes, value = maybe_sentinel
|
||||
idx = struct.unpack("<Q", idx_bytes)[0]
|
||||
|
||||
# Don't store duplicates.
|
||||
sent_message = sent_messages.pop(idx, None)
|
||||
if sent_message is not None:
|
||||
received_messages[idx] = value
|
||||
ltc_stats = _running_mean_var(ltc_stats, receive_time - sent_message[0])
|
||||
if idx == oldest_sent_idx:
|
||||
oldest_sent_idx, next_resend_time = _find_next_resend(sent_messages, ltc_stats)
|
||||
|
||||
if idx == next_out:
|
||||
poller.modify(output_fd, select.EPOLLOUT)
|
||||
else:
|
||||
if oldest_sent_idx is not None:
|
||||
# The message was resent, so it is at least old as the oldest tracked message.
|
||||
ltc_stats = _running_mean_var(ltc_stats, now - sent_messages[oldest_sent_idx][0])
|
||||
elif input_fd in evts:
|
||||
if can_send_new:
|
||||
# We're under the limit, get the next input.
|
||||
send_idx, send_value = next_in_item
|
||||
input_queue.put_nowait((pack_cookie("<Q", send_idx), send_value))
|
||||
sent_messages[next_in_item[0]] = now, next_in_item[1]
|
||||
next_in_item = next(input_stream, EndSentinel)
|
||||
inputs_remain = next_in_item is not EndSentinel
|
||||
|
||||
if oldest_sent_idx is None:
|
||||
oldest_sent_idx, next_resend_time = _find_next_resend(sent_messages, ltc_stats)
|
||||
else:
|
||||
# Move the resent item to the end.
|
||||
send_time, resend_input = sent_messages.pop(oldest_sent_idx)
|
||||
|
||||
sys.stdout.write("Resending {} (ltc, mean, var) = ({}, {}, {})\n".format(
|
||||
oldest_sent_idx, now - send_time, ltc_stats[0], ltc_stats[1]))
|
||||
input_queue.put_nowait((pack_cookie("<Q", oldest_sent_idx), resend_input))
|
||||
|
||||
sent_messages[oldest_sent_idx] = now, resend_input
|
||||
oldest_sent_idx, next_resend_time = _find_next_resend(sent_messages, ltc_stats)
|
||||
|
||||
# Return remaining messages.
|
||||
while next_out in received_messages:
|
||||
output_queue.put(received_messages.pop(next_out))
|
||||
next_out += 1
|
||||
|
||||
_do_cleanup(input_queue, worker_output_queue, num_workers, sentinels_received, 0)
|
||||
output_queue.put(EndSentinel)
|
||||
|
||||
|
||||
def _generate_results_unreliable(input_stream, input_queue, worker_output_queue,
|
||||
output_queue, num_workers, max_outstanding_unused):
|
||||
# Start things off by pulling the first value.
|
||||
next_in_item = next(input_stream, EndSentinel)
|
||||
inputs_remain = next_in_item is not EndSentinel
|
||||
|
||||
# TODO: Use heapq to return oldest message.
|
||||
received_messages = deque()
|
||||
pack_cookie = struct.pack
|
||||
|
||||
input_fd = input_queue.put_fd()
|
||||
worker_output_fd = worker_output_queue.get_fd()
|
||||
output_fd = output_queue.put_fd()
|
||||
|
||||
poller = select.epoll()
|
||||
poller.register(input_fd, select.EPOLLOUT)
|
||||
poller.register(worker_output_fd, select.EPOLLIN)
|
||||
poller.register(output_fd, 0)
|
||||
|
||||
# Keep sending/retrying until the input stream and sent messages are all done.
|
||||
num_outstanding = 0
|
||||
sentinels_received = 0
|
||||
while sentinels_received < num_workers and (inputs_remain or received_messages):
|
||||
# Avoid poll() if we can easily detect that there is work to do.
|
||||
evts = (input_fd if inputs_remain and not input_queue.full() else 0, output_fd
|
||||
if not output_queue.full() and len(received_messages) else 0, worker_output_fd
|
||||
if not worker_output_queue.empty() else 0)
|
||||
if all(evt == 0 for evt in evts):
|
||||
evts = dict(poller.poll())
|
||||
|
||||
if output_fd in evts:
|
||||
output_queue.put(received_messages.pop())
|
||||
|
||||
if len(received_messages) == 0:
|
||||
poller.modify(output_fd, 0)
|
||||
|
||||
if worker_output_fd in evts:
|
||||
for receive_time, maybe_sentinel in worker_output_queue.get_multiple():
|
||||
# Check for EndSentinel in case of worker crash.
|
||||
if maybe_sentinel is EndSentinel:
|
||||
sentinels_received += 1
|
||||
continue
|
||||
received_messages.appendleft(maybe_sentinel[1])
|
||||
num_outstanding -= 1
|
||||
poller.modify(output_fd, select.EPOLLOUT)
|
||||
|
||||
if input_fd in evts:
|
||||
# We're under the limit, get the next input.
|
||||
send_idx, send_value = next_in_item
|
||||
input_queue.put((pack_cookie("<Q", send_idx), send_value))
|
||||
next_in_item = next(input_stream, EndSentinel)
|
||||
inputs_remain = next_in_item is not EndSentinel
|
||||
num_outstanding += 1
|
||||
|
||||
if not inputs_remain:
|
||||
poller.modify(input_fd, 0)
|
||||
|
||||
# TODO: Track latency even though we don't retry.
|
||||
for value in _do_cleanup(input_queue, worker_output_queue, num_workers,
|
||||
sentinels_received, num_outstanding):
|
||||
output_queue.put(value)
|
||||
output_queue.put(EndSentinel)
|
||||
|
||||
|
||||
def _async_generator(func, max_workers, in_q_size, out_q_size, max_outstanding,
|
||||
async_inner, reliable):
|
||||
if async_inner:
|
||||
assert inspect.isgeneratorfunction(
|
||||
func), "async_inner == True but {} is not a generator".format(func)
|
||||
|
||||
@functools.wraps(func)
|
||||
def wrapper(input_sequence_or_self, *args, **kwargs):
|
||||
# HACK: Determine whether the first arg is "self". ismethod returns False here.
|
||||
if inspect.getargspec(func).args[0] == "self":
|
||||
inner_func = func.__get__(input_sequence_or_self, type(input_sequence_or_self))
|
||||
input_sequence = args[0]
|
||||
args = args[1:]
|
||||
else:
|
||||
inner_func = func
|
||||
input_sequence = input_sequence_or_self
|
||||
input_stream = enumerate(iter(input_sequence))
|
||||
|
||||
if reliable:
|
||||
generate_func = _generate_results
|
||||
else:
|
||||
generate_func = _generate_results_unreliable
|
||||
|
||||
input_queue = PollableQueue(in_q_size)
|
||||
worker_output_queue = PollableQueue(8 * max_workers)
|
||||
output_queue = PollableQueue(out_q_size)
|
||||
|
||||
# Start the worker threads.
|
||||
if async_inner:
|
||||
generator_func = inner_func
|
||||
else:
|
||||
args = (inner_func,) + args
|
||||
generator_func = _sync_inner_generator
|
||||
|
||||
worker_threads = []
|
||||
for _ in range(max_workers):
|
||||
t = threading.Thread(
|
||||
target=_async_streamer_async_inner,
|
||||
args=(input_queue, worker_output_queue, generator_func, args, kwargs))
|
||||
t.daemon = True
|
||||
t.start()
|
||||
worker_threads.append(t)
|
||||
|
||||
master_thread = threading.Thread(
|
||||
target=generate_func,
|
||||
args=(input_stream, input_queue, worker_output_queue, output_queue, max_workers,
|
||||
max_outstanding))
|
||||
master_thread.daemon = True
|
||||
master_thread.start()
|
||||
|
||||
try:
|
||||
while True:
|
||||
for value in output_queue.get_multiple():
|
||||
if value is EndSentinel:
|
||||
return
|
||||
else:
|
||||
yield value
|
||||
finally:
|
||||
# Make sure work is done and the threads are stopped.
|
||||
for t in worker_threads:
|
||||
t.join(1)
|
||||
master_thread.join(1)
|
||||
|
||||
input_queue.close()
|
||||
worker_output_queue.close()
|
||||
output_queue.close()
|
||||
|
||||
return wrapper
|
||||
|
||||
|
||||
def async_generator(max_workers=1,
|
||||
in_q_size=10,
|
||||
out_q_size=12,
|
||||
max_outstanding=10000,
|
||||
async_inner=False,
|
||||
reliable=True):
|
||||
return (
|
||||
lambda f: _async_generator(f, max_workers, in_q_size, out_q_size, max_outstanding, async_inner, reliable)
|
||||
)
|
||||
|
|
@ -0,0 +1,9 @@
|
|||
import os
|
||||
from tools.lib.file_helpers import mkdirs_exists_ok
|
||||
|
||||
DEFAULT_CACHE_DIR = os.path.expanduser("~/.commacache")
|
||||
|
||||
def cache_path_for_file_path(fn, cache_prefix=None):
|
||||
dir_ = os.path.join(DEFAULT_CACHE_DIR, "local")
|
||||
mkdirs_exists_ok(dir_)
|
||||
return os.path.join(dir_, os.path.abspath(fn).replace("/", "_"))
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
class DataUnreadableError(Exception):
|
||||
pass
|
||||
|
|
@ -0,0 +1,23 @@
|
|||
import os
|
||||
from atomicwrites import AtomicWriter
|
||||
|
||||
def atomic_write_in_dir(path, **kwargs):
|
||||
"""Creates an atomic writer using a temporary file in the same directory
|
||||
as the destination file.
|
||||
"""
|
||||
writer = AtomicWriter(path, **kwargs)
|
||||
return writer._open(_get_fileobject_func(writer, os.path.dirname(path)))
|
||||
|
||||
def _get_fileobject_func(writer, temp_dir):
|
||||
def _get_fileobject():
|
||||
file_obj = writer.get_fileobject(dir=temp_dir)
|
||||
os.chmod(file_obj.name, 0o644)
|
||||
return file_obj
|
||||
return _get_fileobject
|
||||
|
||||
def mkdirs_exists_ok(path):
|
||||
try:
|
||||
os.makedirs(path)
|
||||
except OSError:
|
||||
if not os.path.isdir(path):
|
||||
raise
|
||||
|
|
@ -0,0 +1,3 @@
|
|||
def FileReader(fn):
|
||||
return open(fn, 'rb')
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1 @@
|
|||
index_log
|
||||
|
|
@ -0,0 +1,19 @@
|
|||
CC := gcc
|
||||
CXX := g++
|
||||
PHONELIBS?=../../../../phonelibs
|
||||
|
||||
UNAME_S := $(shell uname -s)
|
||||
ifeq ($(UNAME_S),Darwin)
|
||||
CAPNP_FLAGS := /usr/local/lib/libcapnp.a /usr/local/lib/libkj.a
|
||||
else
|
||||
CAPNP_FLAGS := -I $(PHONELIBS)/capnp-cpp/include -I $(PHONELIBS)/capnp-cpp/include
|
||||
CAPNP_LIBS := -L $(PHONELIBS)/capnp-cpp/x64/lib -L $(PHONELIBS)/capnp-cpp/x64/lib -l:libcapnp.a -l:libkj.a
|
||||
endif
|
||||
|
||||
index_log: index_log.cc
|
||||
$(eval $@_TMP := $(shell mktemp))
|
||||
$(CXX) -std=gnu++11 -o $($@_TMP) \
|
||||
index_log.cc \
|
||||
$(CAPNP_FLAGS) \
|
||||
$(CAPNP_LIBS)
|
||||
mv $($@_TMP) $@
|
||||
|
|
@ -0,0 +1,66 @@
|
|||
#include <cstdio>
|
||||
#include <cstdlib>
|
||||
#include <cassert>
|
||||
#include <string>
|
||||
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
|
||||
#include <sys/stat.h>
|
||||
#include <sys/mman.h>
|
||||
|
||||
#include <kj/io.h>
|
||||
#include <capnp/serialize.h>
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
|
||||
if (argc != 3) {
|
||||
printf("usage: %s <log_path> <index_output_path>\n", argv[0]);
|
||||
return 1;
|
||||
}
|
||||
|
||||
const std::string log_fn = argv[1];
|
||||
const std::string index_fn = argv[2];
|
||||
|
||||
int log_fd = open(log_fn.c_str(), O_RDONLY, 0);
|
||||
assert(log_fd >= 0);
|
||||
|
||||
off_t log_size = lseek(log_fd, 0, SEEK_END);
|
||||
lseek(log_fd, 0, SEEK_SET);
|
||||
|
||||
FILE* index_f = NULL;
|
||||
if (index_fn == "-") {
|
||||
index_f = stdout;
|
||||
} else {
|
||||
index_f = fopen(index_fn.c_str(), "wb");
|
||||
}
|
||||
assert(index_f);
|
||||
|
||||
void* log_data = mmap(NULL, log_size, PROT_READ, MAP_PRIVATE, log_fd, 0);
|
||||
assert(log_data);
|
||||
|
||||
auto words = kj::arrayPtr((const capnp::word*)log_data, log_size/sizeof(capnp::word));
|
||||
while (words.size() > 0) {
|
||||
|
||||
uint64_t idx = ((uintptr_t)words.begin() - (uintptr_t)log_data);
|
||||
// printf("%llu - %ld\n", idx, words.size());
|
||||
const char* idx_bytes = (const char*)&idx;
|
||||
fwrite(idx_bytes, 8, 1, index_f);
|
||||
try {
|
||||
capnp::FlatArrayMessageReader reader(words);
|
||||
words = kj::arrayPtr(reader.getEnd(), words.end());
|
||||
} catch (kj::Exception exc) {
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
munmap(log_data, log_size);
|
||||
|
||||
fclose(index_f);
|
||||
|
||||
close(log_fd);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,92 @@
|
|||
#!/usr/bin/env python
|
||||
import os
|
||||
import sys
|
||||
import termios
|
||||
import atexit
|
||||
from select import select
|
||||
|
||||
class KBHit:
|
||||
|
||||
def __init__(self):
|
||||
'''Creates a KBHit object that you can call to do various keyboard things.
|
||||
'''
|
||||
|
||||
self.set_kbhit_terminal()
|
||||
|
||||
def set_kbhit_terminal(self):
|
||||
# Save the terminal settings
|
||||
self.fd = sys.stdin.fileno()
|
||||
self.new_term = termios.tcgetattr(self.fd)
|
||||
self.old_term = termios.tcgetattr(self.fd)
|
||||
|
||||
# New terminal setting unbuffered
|
||||
self.new_term[3] = (self.new_term[3] & ~termios.ICANON & ~termios.ECHO)
|
||||
termios.tcsetattr(self.fd, termios.TCSAFLUSH, self.new_term)
|
||||
|
||||
# Support normal-terminal reset at exit
|
||||
atexit.register(self.set_normal_term)
|
||||
|
||||
def set_normal_term(self):
|
||||
''' Resets to normal terminal. On Windows this is a no-op.
|
||||
'''
|
||||
|
||||
if os.name == 'nt':
|
||||
pass
|
||||
|
||||
else:
|
||||
termios.tcsetattr(self.fd, termios.TCSAFLUSH, self.old_term)
|
||||
|
||||
|
||||
def getch(self):
|
||||
''' Returns a keyboard character after kbhit() has been called.
|
||||
Should not be called in the same program as getarrow().
|
||||
'''
|
||||
return sys.stdin.read(1)
|
||||
|
||||
|
||||
def getarrow(self):
|
||||
''' 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().
|
||||
'''
|
||||
|
||||
if os.name == 'nt':
|
||||
msvcrt.getch() # skip 0xE0
|
||||
c = msvcrt.getch()
|
||||
vals = [72, 77, 80, 75]
|
||||
|
||||
else:
|
||||
c = sys.stdin.read(3)[2]
|
||||
vals = [65, 67, 66, 68]
|
||||
|
||||
return vals.index(ord(c.decode('utf-8')))
|
||||
|
||||
|
||||
def kbhit(self):
|
||||
''' Returns True if keyboard character was hit, False otherwise.
|
||||
'''
|
||||
dr,dw,de = select([sys.stdin], [], [], 0)
|
||||
return dr != []
|
||||
|
||||
|
||||
# Test
|
||||
if __name__ == "__main__":
|
||||
|
||||
kb = KBHit()
|
||||
|
||||
print('Hit any key, or ESC to exit')
|
||||
|
||||
while True:
|
||||
|
||||
if kb.kbhit():
|
||||
c = kb.getch()
|
||||
if ord(c) == 27: # ESC
|
||||
break
|
||||
print(c)
|
||||
|
||||
kb.set_normal_term()
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
class lazy_property(object):
|
||||
"""Defines a property whose value will be computed only once and as needed.
|
||||
|
||||
This can only be used on instance methods.
|
||||
"""
|
||||
def __init__(self, func):
|
||||
self._func = func
|
||||
|
||||
def __get__(self, obj_self, cls):
|
||||
value = self._func(obj_self)
|
||||
setattr(obj_self, self._func.__name__, value)
|
||||
return value
|
||||
|
|
@ -0,0 +1,111 @@
|
|||
from cereal import log as capnp_log
|
||||
|
||||
def write_can_to_msg(data, src, msg):
|
||||
if not isinstance(data[0], Sequence):
|
||||
data = [data]
|
||||
|
||||
can_msgs = msg.init('can', len(data))
|
||||
for i, d in enumerate(data):
|
||||
if d[0] < 0: continue # ios bug
|
||||
cc = can_msgs[i]
|
||||
cc.address = d[0]
|
||||
cc.busTime = 0
|
||||
cc.dat = hex_to_str(d[2])
|
||||
if len(d) == 4:
|
||||
cc.src = d[3]
|
||||
cc.busTime = d[1]
|
||||
else:
|
||||
cc.src = src
|
||||
|
||||
def convert_old_pkt_to_new(old_pkt):
|
||||
m, d = old_pkt
|
||||
msg = capnp_log.Event.new_message()
|
||||
|
||||
if len(m) == 3:
|
||||
_, pid, t = m
|
||||
msg.logMonoTime = t
|
||||
else:
|
||||
t, pid = m
|
||||
msg.logMonoTime = int(t * 1e9)
|
||||
|
||||
last_velodyne_time = None
|
||||
|
||||
if pid == PID_OBD:
|
||||
write_can_to_msg(d, 0, msg)
|
||||
elif pid == PID_CAM:
|
||||
frame = msg.init('frame')
|
||||
frame.frameId = d[0]
|
||||
frame.timestampEof = msg.logMonoTime
|
||||
# iOS
|
||||
elif pid == PID_IGPS:
|
||||
loc = msg.init('gpsLocation')
|
||||
loc.latitude = d[0]
|
||||
loc.longitude = d[1]
|
||||
loc.speed = d[2]
|
||||
loc.timestamp = int(m[0]*1000.0) # on iOS, first number is wall time in seconds
|
||||
loc.flags = 1 | 4 # has latitude, longitude, and speed.
|
||||
elif pid == PID_IMOTION:
|
||||
user_acceleration = d[:3]
|
||||
gravity = d[3:6]
|
||||
|
||||
# iOS separates gravity from linear acceleration, so we recombine them.
|
||||
# Apple appears to use this constant for the conversion.
|
||||
g = -9.8
|
||||
acceleration = [g*(a + b) for a, b in zip(user_acceleration, gravity)]
|
||||
|
||||
accel_event = msg.init('sensorEvents', 1)[0]
|
||||
accel_event.acceleration.v = acceleration
|
||||
# android
|
||||
elif pid == PID_GPS:
|
||||
if len(d) <= 6 or d[-1] == "gps":
|
||||
loc = msg.init('gpsLocation')
|
||||
loc.latitude = d[0]
|
||||
loc.longitude = d[1]
|
||||
loc.speed = d[2]
|
||||
if len(d) > 6:
|
||||
loc.timestamp = d[6]
|
||||
loc.flags = 1 | 4 # has latitude, longitude, and speed.
|
||||
elif pid == PID_ACCEL:
|
||||
val = d[2] if type(d[2]) != type(0.0) else d
|
||||
accel_event = msg.init('sensorEvents', 1)[0]
|
||||
accel_event.acceleration.v = val
|
||||
elif pid == PID_GYRO:
|
||||
val = d[2] if type(d[2]) != type(0.0) else d
|
||||
gyro_event = msg.init('sensorEvents', 1)[0]
|
||||
gyro_event.init('gyro').v = val
|
||||
elif pid == PID_LIDAR:
|
||||
lid = msg.init('lidarPts')
|
||||
lid.idx = d[3]
|
||||
elif pid == PID_APPLANIX:
|
||||
loc = msg.init('liveLocation')
|
||||
loc.status = d[18]
|
||||
|
||||
loc.lat, loc.lon, loc.alt = d[0:3]
|
||||
loc.vNED = d[3:6]
|
||||
|
||||
loc.roll = d[6]
|
||||
loc.pitch = d[7]
|
||||
loc.heading = d[8]
|
||||
|
||||
loc.wanderAngle = d[9]
|
||||
loc.trackAngle = d[10]
|
||||
|
||||
loc.speed = d[11]
|
||||
|
||||
loc.gyro = d[12:15]
|
||||
loc.accel = d[15:18]
|
||||
elif pid == PID_IBAROMETER:
|
||||
pressure_event = msg.init('sensorEvents', 1)[0]
|
||||
_, pressure = d[0:2]
|
||||
pressure_event.init('pressure').v = [pressure] # Kilopascals
|
||||
elif pid == PID_IINIT and len(d) == 4:
|
||||
init_event = msg.init('initData')
|
||||
init_event.deviceType = capnp_log.InitData.DeviceType.chffrIos
|
||||
|
||||
build_info = init_event.init('iosBuildInfo')
|
||||
build_info.appVersion = d[0]
|
||||
build_info.appBuild = int(d[1])
|
||||
build_info.osVersion = d[2]
|
||||
build_info.deviceModel = d[3]
|
||||
|
||||
return msg.as_reader()
|
||||
|
|
@ -0,0 +1,205 @@
|
|||
import os
|
||||
import sys
|
||||
import gzip
|
||||
import zlib
|
||||
import json
|
||||
import bz2
|
||||
import tempfile
|
||||
import requests
|
||||
import subprocess
|
||||
from aenum import Enum
|
||||
import capnp
|
||||
import numpy as np
|
||||
|
||||
import platform
|
||||
|
||||
from tools.lib.exceptions import DataUnreadableError
|
||||
try:
|
||||
from xx.chffr.lib.filereader import FileReader
|
||||
except ImportError:
|
||||
from tools.lib.filereader import FileReader
|
||||
from tools.lib.log_util import convert_old_pkt_to_new
|
||||
from cereal import log as capnp_log
|
||||
|
||||
OP_PATH = os.path.dirname(os.path.dirname(capnp_log.__file__))
|
||||
|
||||
def index_log(fn):
|
||||
index_log_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), "index_log")
|
||||
index_log = os.path.join(index_log_dir, "index_log")
|
||||
phonelibs_dir = os.path.join(OP_PATH, 'phonelibs')
|
||||
|
||||
subprocess.check_call(["make", "PHONELIBS=" + phonelibs_dir], cwd=index_log_dir, stdout=subprocess.DEVNULL)
|
||||
|
||||
try:
|
||||
dat = subprocess.check_output([index_log, fn, "-"])
|
||||
except subprocess.CalledProcessError:
|
||||
raise DataUnreadableError("%s capnp is corrupted/truncated" % fn)
|
||||
return np.frombuffer(dat, dtype=np.uint64)
|
||||
|
||||
def event_read_multiple_bytes(dat):
|
||||
with tempfile.NamedTemporaryFile() as dat_f:
|
||||
dat_f.write(dat)
|
||||
dat_f.flush()
|
||||
idx = index_log(dat_f.name)
|
||||
|
||||
end_idx = np.uint64(len(dat))
|
||||
idx = np.append(idx, end_idx)
|
||||
|
||||
return [capnp_log.Event.from_bytes(dat[idx[i]:idx[i+1]])
|
||||
for i in range(len(idx)-1)]
|
||||
|
||||
|
||||
# this is an iterator itself, and uses private variables from LogReader
|
||||
class MultiLogIterator(object):
|
||||
def __init__(self, log_paths, wraparound=True):
|
||||
self._log_paths = log_paths
|
||||
self._wraparound = wraparound
|
||||
|
||||
self._first_log_idx = next(i for i in range(len(log_paths)) if log_paths[i] is not None)
|
||||
self._current_log = self._first_log_idx
|
||||
self._idx = 0
|
||||
self._log_readers = [None]*len(log_paths)
|
||||
self.start_time = self._log_reader(self._first_log_idx)._ts[0]
|
||||
|
||||
def _log_reader(self, i):
|
||||
if self._log_readers[i] is None and self._log_paths[i] is not None:
|
||||
log_path = self._log_paths[i]
|
||||
print("LogReader:%s" % log_path)
|
||||
self._log_readers[i] = LogReader(log_path)
|
||||
|
||||
return self._log_readers[i]
|
||||
|
||||
def __iter__(self):
|
||||
return self
|
||||
|
||||
def _inc(self):
|
||||
lr = self._log_reader(self._current_log)
|
||||
if self._idx < len(lr._ents)-1:
|
||||
self._idx += 1
|
||||
else:
|
||||
self._idx = 0
|
||||
self._current_log = next(i for i in range(self._current_log + 1, len(self._log_readers) + 1) if i == len(self._log_readers) or self._log_paths[i] is not None)
|
||||
# wraparound
|
||||
if self._current_log == len(self._log_readers):
|
||||
if self._wraparound:
|
||||
self._current_log = self._first_log_idx
|
||||
else:
|
||||
raise StopIteration
|
||||
|
||||
def __next__(self):
|
||||
while 1:
|
||||
lr = self._log_reader(self._current_log)
|
||||
ret = lr._ents[self._idx]
|
||||
if lr._do_conversion:
|
||||
ret = convert_old_pkt_to_new(ret, lr.data_version)
|
||||
self._inc()
|
||||
return ret
|
||||
|
||||
def tell(self):
|
||||
# returns seconds from start of log
|
||||
return (self._log_reader(self._current_log)._ts[self._idx] - self.start_time) * 1e-9
|
||||
|
||||
def seek(self, ts):
|
||||
# seek to nearest minute
|
||||
minute = int(ts/60)
|
||||
if minute >= len(self._log_paths) or self._log_paths[minute] is None:
|
||||
return False
|
||||
|
||||
self._current_log = minute
|
||||
|
||||
# HACK: O(n) seek afterward
|
||||
self._idx = 0
|
||||
while self.tell() < ts:
|
||||
self._inc()
|
||||
return True
|
||||
|
||||
|
||||
class LogReader(object):
|
||||
def __init__(self, fn, canonicalize=True, only_union_types=False):
|
||||
_, ext = os.path.splitext(fn)
|
||||
data_version = None
|
||||
|
||||
with FileReader(fn) as f:
|
||||
dat = f.read()
|
||||
|
||||
# decompress file
|
||||
if ext == ".gz" and ("log_" in fn or "log2" in fn):
|
||||
dat = zlib.decompress(dat, zlib.MAX_WBITS|32)
|
||||
elif ext == ".bz2":
|
||||
dat = bz2.decompress(dat)
|
||||
elif ext == ".7z":
|
||||
if platform.system() == "Darwin":
|
||||
os.environ["LA_LIBRARY_FILEPATH"] = "/usr/local/opt/libarchive/lib/libarchive.dylib"
|
||||
import libarchive.public
|
||||
with libarchive.public.memory_reader(dat) as aa:
|
||||
mdat = []
|
||||
for it in aa:
|
||||
for bb in it.get_blocks():
|
||||
mdat.append(bb)
|
||||
dat = ''.join(mdat)
|
||||
|
||||
# TODO: extension shouln't be a proxy for DeviceType
|
||||
if ext == "":
|
||||
if dat[0] == "[":
|
||||
needs_conversion = True
|
||||
ents = [json.loads(x) for x in dat.strip().split("\n")[:-1]]
|
||||
if "_" in fn:
|
||||
data_version = fn.split("_")[1]
|
||||
else:
|
||||
# old rlogs weren't bz2 compressed
|
||||
needs_conversion = False
|
||||
ents = event_read_multiple_bytes(dat)
|
||||
elif ext == ".gz":
|
||||
if "log_" in fn:
|
||||
# Zero data file.
|
||||
ents = [json.loads(x) for x in dat.strip().split("\n")[:-1]]
|
||||
needs_conversion = True
|
||||
elif "log2" in fn:
|
||||
needs_conversion = False
|
||||
ents = event_read_multiple_bytes(dat)
|
||||
else:
|
||||
raise Exception("unknown extension")
|
||||
elif ext == ".bz2":
|
||||
needs_conversion = False
|
||||
ents = event_read_multiple_bytes(dat)
|
||||
elif ext == ".7z":
|
||||
needs_conversion = True
|
||||
ents = [json.loads(x) for x in dat.strip().split("\n")]
|
||||
else:
|
||||
raise Exception("unknown extension")
|
||||
|
||||
if needs_conversion:
|
||||
# TODO: should we call convert_old_pkt_to_new to generate this?
|
||||
self._ts = [x[0][0]*1e9 for x in ents]
|
||||
else:
|
||||
self._ts = [x.logMonoTime for x in ents]
|
||||
|
||||
self.data_version = data_version
|
||||
self._do_conversion = needs_conversion and canonicalize
|
||||
self._only_union_types = only_union_types
|
||||
self._ents = ents
|
||||
|
||||
def __iter__(self):
|
||||
for ent in self._ents:
|
||||
if self._do_conversion:
|
||||
yield convert_old_pkt_to_new(ent, self.data_version)
|
||||
elif self._only_union_types:
|
||||
try:
|
||||
ent.which()
|
||||
yield ent
|
||||
except capnp.lib.capnp.KjException:
|
||||
pass
|
||||
else:
|
||||
yield ent
|
||||
|
||||
def load_many_logs_canonical(log_paths):
|
||||
"""Load all logs for a sequence of log paths."""
|
||||
for log_path in log_paths:
|
||||
for msg in LogReader(log_path):
|
||||
yield msg
|
||||
|
||||
if __name__ == "__main__":
|
||||
log_path = sys.argv[1]
|
||||
lr = LogReader(log_path)
|
||||
for msg in lr:
|
||||
print(msg)
|
||||
|
|
@ -0,0 +1,24 @@
|
|||
Simple easy-to-use hacky matroska parser
|
||||
|
||||
Define your handler class:
|
||||
|
||||
class MyMatroskaHandler(mkvparse.MatroskaHandler):
|
||||
def tracks_available(self):
|
||||
...
|
||||
|
||||
def segment_info_available(self):
|
||||
...
|
||||
|
||||
def frame(self, track_id, timestamp, data, more_laced_blocks, duration, keyframe_flag, invisible_flag, discardable_flag):
|
||||
...
|
||||
|
||||
and `mkvparse.mkvparse(file, MyMatroskaHandler())`
|
||||
|
||||
|
||||
Supports lacing and setting global timecode scale, subtitles (BlockGroup). Does not support cues, tags, chapters, seeking and so on. Supports resyncing when something bad is encountered in matroska stream.
|
||||
|
||||
Also contains example of generation of Matroska files from python
|
||||
|
||||
Subtitles should remain as text, binary data gets encoded to hex.
|
||||
|
||||
Licence=MIT
|
||||
|
|
@ -0,0 +1,188 @@
|
|||
#!/usr/bin/env python
|
||||
import sys
|
||||
import random
|
||||
import math
|
||||
|
||||
# Simple hacky Matroska generator
|
||||
# Reads mp3 file "q.mp3" and jpeg images from img/0.jpg, img/1.jpg and so on and
|
||||
# writes Matroska file with mjpeg and mp3 to stdout
|
||||
|
||||
# License=MIT
|
||||
|
||||
# unsigned
|
||||
def big_endian_number(number):
|
||||
if(number<0x100):
|
||||
return chr(number)
|
||||
return big_endian_number(number>>8) + chr(number&0xFF)
|
||||
|
||||
ben=big_endian_number
|
||||
|
||||
def ebml_encode_number(number):
|
||||
def trailing_bits(rest_of_number, number_of_bits):
|
||||
# like big_endian_number, but can do padding zeroes
|
||||
if number_of_bits==8:
|
||||
return chr(rest_of_number&0xFF);
|
||||
else:
|
||||
return trailing_bits(rest_of_number>>8, number_of_bits-8) + chr(rest_of_number&0xFF)
|
||||
|
||||
if number == -1:
|
||||
return chr(0xFF)
|
||||
if number < 2**7 - 1:
|
||||
return chr(number|0x80)
|
||||
if number < 2**14 - 1:
|
||||
return chr(0x40 | (number>>8)) + trailing_bits(number, 8)
|
||||
if number < 2**21 - 1:
|
||||
return chr(0x20 | (number>>16)) + trailing_bits(number, 16)
|
||||
if number < 2**28 - 1:
|
||||
return chr(0x10 | (number>>24)) + trailing_bits(number, 24)
|
||||
if number < 2**35 - 1:
|
||||
return chr(0x08 | (number>>32)) + trailing_bits(number, 32)
|
||||
if number < 2**42 - 1:
|
||||
return chr(0x04 | (number>>40)) + trailing_bits(number, 40)
|
||||
if number < 2**49 - 1:
|
||||
return chr(0x02 | (number>>48)) + trailing_bits(number, 48)
|
||||
if number < 2**56 - 1:
|
||||
return chr(0x01) + trailing_bits(number, 56)
|
||||
raise Exception("NUMBER TOO BIG")
|
||||
|
||||
def ebml_element(element_id, data, length=None):
|
||||
if length==None:
|
||||
length = len(data)
|
||||
return big_endian_number(element_id) + ebml_encode_number(length) + data
|
||||
|
||||
|
||||
def write_ebml_header(f, content_type, version, read_version):
|
||||
f.write(
|
||||
ebml_element(0x1A45DFA3, "" # EBML
|
||||
+ ebml_element(0x4286, ben(1)) # EBMLVersion
|
||||
+ ebml_element(0x42F7, ben(1)) # EBMLReadVersion
|
||||
+ ebml_element(0x42F2, ben(4)) # EBMLMaxIDLength
|
||||
+ ebml_element(0x42F3, ben(8)) # EBMLMaxSizeLength
|
||||
+ ebml_element(0x4282, content_type) # DocType
|
||||
+ ebml_element(0x4287, ben(version)) # DocTypeVersion
|
||||
+ ebml_element(0x4285, ben(read_version)) # DocTypeReadVersion
|
||||
))
|
||||
|
||||
def write_infinite_segment_header(f):
|
||||
# write segment element header
|
||||
f.write(ebml_element(0x18538067,"",-1)) # Segment (unknown length)
|
||||
|
||||
def random_uid():
|
||||
def rint():
|
||||
return int(random.random()*(0x100**4))
|
||||
return ben(rint()) + ben(rint()) + ben(rint()) + ben(rint())
|
||||
|
||||
|
||||
def example():
|
||||
write_ebml_header(sys.stdout, "matroska", 2, 2)
|
||||
write_infinite_segment_header(sys.stdout)
|
||||
|
||||
|
||||
# write segment info (optional)
|
||||
sys.stdout.write(ebml_element(0x1549A966, "" # SegmentInfo
|
||||
+ ebml_element(0x73A4, random_uid()) # SegmentUID
|
||||
+ ebml_element(0x7BA9, "mkvgen.py test") # Title
|
||||
+ ebml_element(0x4D80, "mkvgen.py") # MuxingApp
|
||||
+ ebml_element(0x5741, "mkvgen.py") # WritingApp
|
||||
))
|
||||
|
||||
# write trans data (codecs etc.)
|
||||
sys.stdout.write(ebml_element(0x1654AE6B, "" # Tracks
|
||||
+ ebml_element(0xAE, "" # TrackEntry
|
||||
+ ebml_element(0xD7, ben(1)) # TrackNumber
|
||||
+ ebml_element(0x73C5, ben(0x77)) # TrackUID
|
||||
+ ebml_element(0x83, ben(0x01)) # TrackType
|
||||
#0x01 track is a video track
|
||||
#0x02 track is an audio track
|
||||
#0x03 track is a complex track, i.e. a combined video and audio track
|
||||
#0x10 track is a logo track
|
||||
#0x11 track is a subtitle track
|
||||
#0x12 track is a button track
|
||||
#0x20 track is a control track
|
||||
+ ebml_element(0x536E, "mjpeg data") # Name
|
||||
+ ebml_element(0x86, "V_MJPEG") # CodecID
|
||||
#+ ebml_element(0x23E383, ben(100000000)) # DefaultDuration (opt.), nanoseconds
|
||||
#+ ebml_element(0x6DE7, ben(100)) # MinCache
|
||||
+ ebml_element(0xE0, "" # Video
|
||||
+ ebml_element(0xB0, ben(640)) # PixelWidth
|
||||
+ ebml_element(0xBA, ben(480)) # PixelHeight
|
||||
)
|
||||
)
|
||||
+ ebml_element(0xAE, "" # TrackEntry
|
||||
+ ebml_element(0xD7, ben(2)) # TrackNumber
|
||||
+ ebml_element(0x73C5, ben(0x78)) # TrackUID
|
||||
+ ebml_element(0x83, ben(0x02)) # TrackType
|
||||
#0x01 track is a video track
|
||||
#0x02 track is an audio track
|
||||
#0x03 track is a complex track, i.e. a combined video and audio track
|
||||
#0x10 track is a logo track
|
||||
#0x11 track is a subtitle track
|
||||
#0x12 track is a button track
|
||||
#0x20 track is a control track
|
||||
+ ebml_element(0x536E, "content of mp3 file") # Name
|
||||
#+ ebml_element(0x6DE7, ben(100)) # MinCache
|
||||
+ ebml_element(0x86, "A_MPEG/L3") # CodecID
|
||||
#+ ebml_element(0xE1, "") # Audio
|
||||
)
|
||||
))
|
||||
|
||||
|
||||
mp3file = open("q.mp3", "rb")
|
||||
mp3file.read(500000);
|
||||
|
||||
def mp3framesgenerator(f):
|
||||
debt=""
|
||||
while True:
|
||||
for i in xrange(0,len(debt)+1):
|
||||
if i >= len(debt)-1:
|
||||
debt = debt + f.read(8192)
|
||||
break
|
||||
#sys.stderr.write("i="+str(i)+" len="+str(len(debt))+"\n")
|
||||
if ord(debt[i])==0xFF and (ord(debt[i+1]) & 0xF0)==0XF0 and i>700:
|
||||
if i>0:
|
||||
yield debt[0:i]
|
||||
# sys.stderr.write("len="+str(i)+"\n")
|
||||
debt = debt[i:]
|
||||
break
|
||||
|
||||
|
||||
mp3 = mp3framesgenerator(mp3file)
|
||||
mp3.next()
|
||||
|
||||
|
||||
for i in xrange(0,530):
|
||||
framefile = open("img/"+str(i)+".jpg", "rb")
|
||||
framedata = framefile.read()
|
||||
framefile.close()
|
||||
|
||||
# write cluster (actual video data)
|
||||
|
||||
if random.random()<1:
|
||||
sys.stdout.write(ebml_element(0x1F43B675, "" # Cluster
|
||||
+ ebml_element(0xE7, ben(int(i*26*4))) # TimeCode, uint, milliseconds
|
||||
# + ebml_element(0xA7, ben(0)) # Position, uint
|
||||
+ ebml_element(0xA3, "" # SimpleBlock
|
||||
+ ebml_encode_number(1) # track number
|
||||
+ chr(0x00) + chr(0x00) # timecode, relative to Cluster timecode, sint16, in milliseconds
|
||||
+ chr(0x00) # flags
|
||||
+ framedata
|
||||
)))
|
||||
|
||||
for u in xrange(0,4):
|
||||
mp3f=mp3.next()
|
||||
if random.random()<1:
|
||||
sys.stdout.write(ebml_element(0x1F43B675, "" # Cluster
|
||||
+ ebml_element(0xE7, ben(i*26*4+u*26)) # TimeCode, uint, milliseconds
|
||||
+ ebml_element(0xA3, "" # SimpleBlock
|
||||
+ ebml_encode_number(2) # track number
|
||||
+ chr(0x00) + chr(0x00) # timecode, relative to Cluster timecode, sint16, in milliseconds
|
||||
+ chr(0x00) # flags
|
||||
+ mp3f
|
||||
)))
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
example()
|
||||
|
|
@ -0,0 +1,87 @@
|
|||
#!/usr/bin/env python
|
||||
# Copyright (c) 2016, Comma.ai, Inc.
|
||||
|
||||
import sys
|
||||
import re
|
||||
import binascii
|
||||
|
||||
from tools.lib.mkvparse import mkvparse
|
||||
from tools.lib.mkvparse import mkvgen
|
||||
from tools.lib.mkvparse.mkvgen import ben, ebml_element, ebml_encode_number
|
||||
|
||||
class MatroskaIndex(mkvparse.MatroskaHandler):
|
||||
# def __init__(self, banlist, nocluster_mode):
|
||||
# pass
|
||||
def __init__(self):
|
||||
self.frameindex = []
|
||||
|
||||
def tracks_available(self):
|
||||
_, self.config_record = self.tracks[1]['CodecPrivate']
|
||||
|
||||
def frame(self, track_id, timestamp, pos, length, more_laced_frames, duration,
|
||||
keyframe, invisible, discardable):
|
||||
self.frameindex.append((pos, length, keyframe))
|
||||
|
||||
|
||||
|
||||
def mkvindex(f):
|
||||
handler = MatroskaIndex()
|
||||
mkvparse.mkvparse(f, handler)
|
||||
return handler.config_record, handler.frameindex
|
||||
|
||||
|
||||
def simple_gen(of, config_record, w, h, framedata):
|
||||
mkvgen.write_ebml_header(of, "matroska", 2, 2)
|
||||
mkvgen.write_infinite_segment_header(of)
|
||||
|
||||
of.write(ebml_element(0x1654AE6B, "" # Tracks
|
||||
+ ebml_element(0xAE, "" # TrackEntry
|
||||
+ ebml_element(0xD7, ben(1)) # TrackNumber
|
||||
+ ebml_element(0x73C5, ben(1)) # TrackUID
|
||||
+ ebml_element(0x83, ben(1)) # TrackType = video track
|
||||
+ ebml_element(0x86, "V_MS/VFW/FOURCC") # CodecID
|
||||
+ ebml_element(0xE0, "" # Video
|
||||
+ ebml_element(0xB0, ben(w)) # PixelWidth
|
||||
+ ebml_element(0xBA, ben(h)) # PixelHeight
|
||||
)
|
||||
+ ebml_element(0x63A2, config_record) # CodecPrivate (ffv1 configuration record)
|
||||
)
|
||||
))
|
||||
|
||||
blocks = []
|
||||
for fd in framedata:
|
||||
blocks.append(
|
||||
ebml_element(0xA3, "" # SimpleBlock
|
||||
+ ebml_encode_number(1) # track number
|
||||
+ chr(0x00) + chr(0x00) # timecode, relative to Cluster timecode, sint16, in milliseconds
|
||||
+ chr(0x80) # flags (keyframe)
|
||||
+ fd
|
||||
)
|
||||
)
|
||||
|
||||
of.write(ebml_element(0x1F43B675, "" # Cluster
|
||||
+ ebml_element(0xE7, ben(0)) # TimeCode, uint, milliseconds
|
||||
# + ebml_element(0xA7, ben(0)) # Position, uint
|
||||
+ ''.join(blocks)))
|
||||
|
||||
if __name__ == "__main__":
|
||||
import random
|
||||
|
||||
if len(sys.argv) != 2:
|
||||
print("usage: %s mkvpath" % sys.argv[0])
|
||||
with open(sys.argv[1], "rb") as f:
|
||||
cr, index = mkvindex(f)
|
||||
|
||||
# cr = "280000003002000030010000010018004646563100cb070000000000000000000000000000000000".decode("hex")
|
||||
|
||||
def geti(i):
|
||||
pos, length = index[i]
|
||||
with open(sys.argv[1], "rb") as f:
|
||||
f.seek(pos)
|
||||
return f.read(length)
|
||||
|
||||
dats = [geti(random.randrange(200)) for _ in xrange(30)]
|
||||
|
||||
with open("tmpout.mkv", "wb") as of:
|
||||
simple_gen(of, cr, dats)
|
||||
|
||||
|
|
@ -0,0 +1,761 @@
|
|||
# Licence==MIT; Vitaly "_Vi" Shukela 2012
|
||||
|
||||
# Simple easy-to-use hacky matroska parser
|
||||
|
||||
# Supports SimpleBlock and BlockGroup, lacing, TimecodeScale.
|
||||
# Does not support seeking, cues, chapters and other features.
|
||||
# No proper EOF handling unfortunately
|
||||
|
||||
# See "mkvuser.py" for the example
|
||||
|
||||
import traceback
|
||||
from struct import unpack
|
||||
|
||||
import sys
|
||||
import datetime
|
||||
|
||||
if sys.version < '3':
|
||||
range=xrange
|
||||
else:
|
||||
#identity=lambda x:x
|
||||
def ord(something):
|
||||
if type(something)==bytes:
|
||||
if something == b"":
|
||||
raise StopIteration
|
||||
return something[0]
|
||||
else:
|
||||
return something
|
||||
|
||||
def get_major_bit_number(n):
|
||||
'''
|
||||
Takes uint8, returns number of the most significant bit plus the number with that bit cleared.
|
||||
Examples:
|
||||
0b10010101 -> (0, 0b00010101)
|
||||
0b00010101 -> (3, 0b00000101)
|
||||
0b01111111 -> (1, 0b00111111)
|
||||
'''
|
||||
if not n:
|
||||
raise Exception("Bad number")
|
||||
i=0x80;
|
||||
r=0
|
||||
while not n&i:
|
||||
r+=1
|
||||
i>>=1
|
||||
return (r,n&~i);
|
||||
|
||||
def read_matroska_number(f, unmodified=False, signed=False):
|
||||
'''
|
||||
Read ebml number. Unmodified means don't clear the length bit (as in Element IDs)
|
||||
Returns the number and it's length as a tuple
|
||||
|
||||
See examples in "parse_matroska_number" function
|
||||
'''
|
||||
if unmodified and signed:
|
||||
raise Exception("Contradictary arguments")
|
||||
first_byte=f.read(1)
|
||||
if(first_byte==""):
|
||||
raise StopIteration
|
||||
r = ord(first_byte)
|
||||
(n,r2) = get_major_bit_number(r)
|
||||
if not unmodified:
|
||||
r=r2
|
||||
# from now "signed" means "negative"
|
||||
i=n
|
||||
while i:
|
||||
r = r * 0x100 + ord(f.read(1))
|
||||
i-=1
|
||||
if signed:
|
||||
r-=(2**(7*n+7)-1)
|
||||
else:
|
||||
if r==2**(7*n+7)-1:
|
||||
return (-1, n+1)
|
||||
return (r,n+1)
|
||||
|
||||
def parse_matroska_number(data, pos, unmodified=False, signed=False):
|
||||
'''
|
||||
Parse ebml number from buffer[pos:]. Just like read_matroska_number.
|
||||
Unmodified means don't clear the length bit (as in Element IDs)
|
||||
Returns the number plus the new position in input buffer
|
||||
|
||||
Examples:
|
||||
"\x81" -> (1, pos+1)
|
||||
"\x40\x01" -> (1, pos+2)
|
||||
"\x20\x00\x01" -> (1, pos+3)
|
||||
"\x3F\xFF\xFF" -> (0x1FFFFF, pos+3)
|
||||
"\x20\x00\x01" unmodified -> (0x200001, pos+3)
|
||||
"\xBF" signed -> (0, pos+1)
|
||||
"\xBE" signed -> (-1, pos+1)
|
||||
"\xC0" signed -> (1, pos+1)
|
||||
"\x5F\xEF" signed -> (-16, pos+2)
|
||||
'''
|
||||
if unmodified and signed:
|
||||
raise Exception("Contradictary arguments")
|
||||
r = ord(data[pos])
|
||||
pos+=1
|
||||
(n,r2) = get_major_bit_number(r)
|
||||
if not unmodified:
|
||||
r=r2
|
||||
# from now "signed" means "negative"
|
||||
i=n
|
||||
while i:
|
||||
r = r * 0x100 + ord(data[pos])
|
||||
pos+=1
|
||||
i-=1
|
||||
if signed:
|
||||
r-=(2**(7*n+6)-1)
|
||||
else:
|
||||
if r==2**(7*n+7)-1:
|
||||
return (-1, pos)
|
||||
return (r,pos)
|
||||
|
||||
def parse_xiph_number(data, pos):
|
||||
'''
|
||||
Parse the Xiph lacing number from data[pos:]
|
||||
Returns the number plus the new position
|
||||
|
||||
Examples:
|
||||
"\x01" -> (1, pos+1)
|
||||
"\x55" -> (0x55, pos+1)
|
||||
"\xFF\x04" -> (0x103, pos+2)
|
||||
"\xFF\xFF\x04" -> (0x202, pos+3)
|
||||
"\xFF\xFF\x00" -> (0x1FE, pos+3)
|
||||
'''
|
||||
v = ord(data[pos])
|
||||
pos+=1
|
||||
|
||||
r=0
|
||||
while v==255:
|
||||
r+=v
|
||||
v = ord(data[pos])
|
||||
pos+=1
|
||||
|
||||
r+=v
|
||||
return (r, pos)
|
||||
|
||||
|
||||
def parse_fixedlength_number(data, pos, length, signed=False):
|
||||
'''
|
||||
Read the big-endian number from data[pos:pos+length]
|
||||
Returns the number plus the new position
|
||||
|
||||
Examples:
|
||||
"\x01" -> (0x1, pos+1)
|
||||
"\x55" -> (0x55, pos+1)
|
||||
"\x55" signed -> (0x55, pos+1)
|
||||
"\xFF\x04" -> (0xFF04, pos+2)
|
||||
"\xFF\x04" signed -> (-0x00FC, pos+2)
|
||||
'''
|
||||
r=0
|
||||
for i in range(length):
|
||||
r=r*0x100+ord(data[pos+i])
|
||||
if signed:
|
||||
if ord(data[pos]) & 0x80:
|
||||
r-=2**(8*length)
|
||||
return (r, pos+length)
|
||||
|
||||
def read_fixedlength_number(f, length, signed=False):
|
||||
""" Read length bytes and parse (parse_fixedlength_number) it.
|
||||
Returns only the number"""
|
||||
buf = f.read(length)
|
||||
(r, pos) = parse_fixedlength_number(buf, 0, length, signed)
|
||||
return r
|
||||
|
||||
def read_ebml_element_header(f):
|
||||
'''
|
||||
Read Element ID and size
|
||||
Returns id, element size and this header size
|
||||
'''
|
||||
(id_, n) = read_matroska_number(f, unmodified=True)
|
||||
(size, n2) = read_matroska_number(f)
|
||||
return (id_, size, n+n2)
|
||||
|
||||
class EbmlElementType:
|
||||
VOID=0
|
||||
MASTER=1 # read all subelements and return tree. Don't use this too large things like Segment
|
||||
UNSIGNED=2
|
||||
SIGNED=3
|
||||
TEXTA=4
|
||||
TEXTU=5
|
||||
BINARY=6
|
||||
FLOAT=7
|
||||
DATE=8
|
||||
|
||||
JUST_GO_ON=10 # For "Segment".
|
||||
# Actually MASTER, but don't build the tree for all subelements,
|
||||
# interpreting all child elements as if they were top-level elements
|
||||
|
||||
|
||||
EET=EbmlElementType
|
||||
|
||||
# lynx -width=10000 -dump http://matroska.org/technical/specs/index.html
|
||||
# | sed 's/not 0/not0/g; s/> 0/>0/g; s/Sampling Frequency/SamplingFrequency/g'
|
||||
# | awk '{print $1 " " $3 " " $8}'
|
||||
# | grep '\[..\]'
|
||||
# | perl -ne '/(\S+) (\S+) (.)/;
|
||||
# $name=$1; $id=$2; $type=$3;
|
||||
# $id=~s/\[|\]//g;
|
||||
# %types = (m=>"EET.MASTER",
|
||||
# u=>"EET.UNSIGNED",
|
||||
# i=>"EET.SIGNED",
|
||||
# 8=>"EET.TEXTU",
|
||||
# s=>"EET.TEXTA",
|
||||
# b=>"EET.BINARY",
|
||||
# f=>"EET.FLOAT",
|
||||
# d=>"EET.DATE");
|
||||
# $t=$types{$type};
|
||||
# next unless $t;
|
||||
# $t="EET.JUST_GO_ON" if $name eq "Segment" or $name eq "Cluster";
|
||||
# print "\t0x$id: ($t, \"$name\"),\n";'
|
||||
|
||||
element_types_names = {
|
||||
0x1A45DFA3: (EET.MASTER, "EBML"),
|
||||
0x4286: (EET.UNSIGNED, "EBMLVersion"),
|
||||
0x42F7: (EET.UNSIGNED, "EBMLReadVersion"),
|
||||
0x42F2: (EET.UNSIGNED, "EBMLMaxIDLength"),
|
||||
0x42F3: (EET.UNSIGNED, "EBMLMaxSizeLength"),
|
||||
0x4282: (EET.TEXTA, "DocType"),
|
||||
0x4287: (EET.UNSIGNED, "DocTypeVersion"),
|
||||
0x4285: (EET.UNSIGNED, "DocTypeReadVersion"),
|
||||
0xEC: (EET.BINARY, "Void"),
|
||||
0xBF: (EET.BINARY, "CRC-32"),
|
||||
0x1B538667: (EET.MASTER, "SignatureSlot"),
|
||||
0x7E8A: (EET.UNSIGNED, "SignatureAlgo"),
|
||||
0x7E9A: (EET.UNSIGNED, "SignatureHash"),
|
||||
0x7EA5: (EET.BINARY, "SignaturePublicKey"),
|
||||
0x7EB5: (EET.BINARY, "Signature"),
|
||||
0x7E5B: (EET.MASTER, "SignatureElements"),
|
||||
0x7E7B: (EET.MASTER, "SignatureElementList"),
|
||||
0x6532: (EET.BINARY, "SignedElement"),
|
||||
0x18538067: (EET.JUST_GO_ON, "Segment"),
|
||||
0x114D9B74: (EET.MASTER, "SeekHead"),
|
||||
0x4DBB: (EET.MASTER, "Seek"),
|
||||
0x53AB: (EET.BINARY, "SeekID"),
|
||||
0x53AC: (EET.UNSIGNED, "SeekPosition"),
|
||||
0x1549A966: (EET.MASTER, "Info"),
|
||||
0x73A4: (EET.BINARY, "SegmentUID"),
|
||||
0x7384: (EET.TEXTU, "SegmentFilename"),
|
||||
0x3CB923: (EET.BINARY, "PrevUID"),
|
||||
0x3C83AB: (EET.TEXTU, "PrevFilename"),
|
||||
0x3EB923: (EET.BINARY, "NextUID"),
|
||||
0x3E83BB: (EET.TEXTU, "NextFilename"),
|
||||
0x4444: (EET.BINARY, "SegmentFamily"),
|
||||
0x6924: (EET.MASTER, "ChapterTranslate"),
|
||||
0x69FC: (EET.UNSIGNED, "ChapterTranslateEditionUID"),
|
||||
0x69BF: (EET.UNSIGNED, "ChapterTranslateCodec"),
|
||||
0x69A5: (EET.BINARY, "ChapterTranslateID"),
|
||||
0x2AD7B1: (EET.UNSIGNED, "TimecodeScale"),
|
||||
0x4489: (EET.FLOAT, "Duration"),
|
||||
0x4461: (EET.DATE, "DateUTC"),
|
||||
0x7BA9: (EET.TEXTU, "Title"),
|
||||
0x4D80: (EET.TEXTU, "MuxingApp"),
|
||||
0x5741: (EET.TEXTU, "WritingApp"),
|
||||
0x1F43B675: (EET.JUST_GO_ON, "Cluster"),
|
||||
0xE7: (EET.UNSIGNED, "Timecode"),
|
||||
0x5854: (EET.MASTER, "SilentTracks"),
|
||||
0x58D7: (EET.UNSIGNED, "SilentTrackNumber"),
|
||||
0xA7: (EET.UNSIGNED, "Position"),
|
||||
0xAB: (EET.UNSIGNED, "PrevSize"),
|
||||
0xA3: (EET.BINARY, "SimpleBlock"),
|
||||
0xA0: (EET.MASTER, "BlockGroup"),
|
||||
0xA1: (EET.BINARY, "Block"),
|
||||
0xA2: (EET.BINARY, "BlockVirtual"),
|
||||
0x75A1: (EET.MASTER, "BlockAdditions"),
|
||||
0xA6: (EET.MASTER, "BlockMore"),
|
||||
0xEE: (EET.UNSIGNED, "BlockAddID"),
|
||||
0xA5: (EET.BINARY, "BlockAdditional"),
|
||||
0x9B: (EET.UNSIGNED, "BlockDuration"),
|
||||
0xFA: (EET.UNSIGNED, "ReferencePriority"),
|
||||
0xFB: (EET.SIGNED, "ReferenceBlock"),
|
||||
0xFD: (EET.SIGNED, "ReferenceVirtual"),
|
||||
0xA4: (EET.BINARY, "CodecState"),
|
||||
0x8E: (EET.MASTER, "Slices"),
|
||||
0xE8: (EET.MASTER, "TimeSlice"),
|
||||
0xCC: (EET.UNSIGNED, "LaceNumber"),
|
||||
0xCD: (EET.UNSIGNED, "FrameNumber"),
|
||||
0xCB: (EET.UNSIGNED, "BlockAdditionID"),
|
||||
0xCE: (EET.UNSIGNED, "Delay"),
|
||||
0xCF: (EET.UNSIGNED, "SliceDuration"),
|
||||
0xC8: (EET.MASTER, "ReferenceFrame"),
|
||||
0xC9: (EET.UNSIGNED, "ReferenceOffset"),
|
||||
0xCA: (EET.UNSIGNED, "ReferenceTimeCode"),
|
||||
0xAF: (EET.BINARY, "EncryptedBlock"),
|
||||
0x1654AE6B: (EET.MASTER, "Tracks"),
|
||||
0xAE: (EET.MASTER, "TrackEntry"),
|
||||
0xD7: (EET.UNSIGNED, "TrackNumber"),
|
||||
0x73C5: (EET.UNSIGNED, "TrackUID"),
|
||||
0x83: (EET.UNSIGNED, "TrackType"),
|
||||
0xB9: (EET.UNSIGNED, "FlagEnabled"),
|
||||
0x88: (EET.UNSIGNED, "FlagDefault"),
|
||||
0x55AA: (EET.UNSIGNED, "FlagForced"),
|
||||
0x9C: (EET.UNSIGNED, "FlagLacing"),
|
||||
0x6DE7: (EET.UNSIGNED, "MinCache"),
|
||||
0x6DF8: (EET.UNSIGNED, "MaxCache"),
|
||||
0x23E383: (EET.UNSIGNED, "DefaultDuration"),
|
||||
0x23314F: (EET.FLOAT, "TrackTimecodeScale"),
|
||||
0x537F: (EET.SIGNED, "TrackOffset"),
|
||||
0x55EE: (EET.UNSIGNED, "MaxBlockAdditionID"),
|
||||
0x536E: (EET.TEXTU, "Name"),
|
||||
0x22B59C: (EET.TEXTA, "Language"),
|
||||
0x86: (EET.TEXTA, "CodecID"),
|
||||
0x63A2: (EET.BINARY, "CodecPrivate"),
|
||||
0x258688: (EET.TEXTU, "CodecName"),
|
||||
0x7446: (EET.UNSIGNED, "AttachmentLink"),
|
||||
0x3A9697: (EET.TEXTU, "CodecSettings"),
|
||||
0x3B4040: (EET.TEXTA, "CodecInfoURL"),
|
||||
0x26B240: (EET.TEXTA, "CodecDownloadURL"),
|
||||
0xAA: (EET.UNSIGNED, "CodecDecodeAll"),
|
||||
0x6FAB: (EET.UNSIGNED, "TrackOverlay"),
|
||||
0x6624: (EET.MASTER, "TrackTranslate"),
|
||||
0x66FC: (EET.UNSIGNED, "TrackTranslateEditionUID"),
|
||||
0x66BF: (EET.UNSIGNED, "TrackTranslateCodec"),
|
||||
0x66A5: (EET.BINARY, "TrackTranslateTrackID"),
|
||||
0xE0: (EET.MASTER, "Video"),
|
||||
0x9A: (EET.UNSIGNED, "FlagInterlaced"),
|
||||
0x53B8: (EET.UNSIGNED, "StereoMode"),
|
||||
0x53B9: (EET.UNSIGNED, "OldStereoMode"),
|
||||
0xB0: (EET.UNSIGNED, "PixelWidth"),
|
||||
0xBA: (EET.UNSIGNED, "PixelHeight"),
|
||||
0x54AA: (EET.UNSIGNED, "PixelCropBottom"),
|
||||
0x54BB: (EET.UNSIGNED, "PixelCropTop"),
|
||||
0x54CC: (EET.UNSIGNED, "PixelCropLeft"),
|
||||
0x54DD: (EET.UNSIGNED, "PixelCropRight"),
|
||||
0x54B0: (EET.UNSIGNED, "DisplayWidth"),
|
||||
0x54BA: (EET.UNSIGNED, "DisplayHeight"),
|
||||
0x54B2: (EET.UNSIGNED, "DisplayUnit"),
|
||||
0x54B3: (EET.UNSIGNED, "AspectRatioType"),
|
||||
0x2EB524: (EET.BINARY, "ColourSpace"),
|
||||
0x2FB523: (EET.FLOAT, "GammaValue"),
|
||||
0x2383E3: (EET.FLOAT, "FrameRate"),
|
||||
0xE1: (EET.MASTER, "Audio"),
|
||||
0xB5: (EET.FLOAT, "SamplingFrequency"),
|
||||
0x78B5: (EET.FLOAT, "OutputSamplingFrequency"),
|
||||
0x9F: (EET.UNSIGNED, "Channels"),
|
||||
0x7D7B: (EET.BINARY, "ChannelPositions"),
|
||||
0x6264: (EET.UNSIGNED, "BitDepth"),
|
||||
0xE2: (EET.MASTER, "TrackOperation"),
|
||||
0xE3: (EET.MASTER, "TrackCombinePlanes"),
|
||||
0xE4: (EET.MASTER, "TrackPlane"),
|
||||
0xE5: (EET.UNSIGNED, "TrackPlaneUID"),
|
||||
0xE6: (EET.UNSIGNED, "TrackPlaneType"),
|
||||
0xE9: (EET.MASTER, "TrackJoinBlocks"),
|
||||
0xED: (EET.UNSIGNED, "TrackJoinUID"),
|
||||
0xC0: (EET.UNSIGNED, "TrickTrackUID"),
|
||||
0xC1: (EET.BINARY, "TrickTrackSegmentUID"),
|
||||
0xC6: (EET.UNSIGNED, "TrickTrackFlag"),
|
||||
0xC7: (EET.UNSIGNED, "TrickMasterTrackUID"),
|
||||
0xC4: (EET.BINARY, "TrickMasterTrackSegmentUID"),
|
||||
0x6D80: (EET.MASTER, "ContentEncodings"),
|
||||
0x6240: (EET.MASTER, "ContentEncoding"),
|
||||
0x5031: (EET.UNSIGNED, "ContentEncodingOrder"),
|
||||
0x5032: (EET.UNSIGNED, "ContentEncodingScope"),
|
||||
0x5033: (EET.UNSIGNED, "ContentEncodingType"),
|
||||
0x5034: (EET.MASTER, "ContentCompression"),
|
||||
0x4254: (EET.UNSIGNED, "ContentCompAlgo"),
|
||||
0x4255: (EET.BINARY, "ContentCompSettings"),
|
||||
0x5035: (EET.MASTER, "ContentEncryption"),
|
||||
0x47E1: (EET.UNSIGNED, "ContentEncAlgo"),
|
||||
0x47E2: (EET.BINARY, "ContentEncKeyID"),
|
||||
0x47E3: (EET.BINARY, "ContentSignature"),
|
||||
0x47E4: (EET.BINARY, "ContentSigKeyID"),
|
||||
0x47E5: (EET.UNSIGNED, "ContentSigAlgo"),
|
||||
0x47E6: (EET.UNSIGNED, "ContentSigHashAlgo"),
|
||||
0x1C53BB6B: (EET.MASTER, "Cues"),
|
||||
0xBB: (EET.MASTER, "CuePoint"),
|
||||
0xB3: (EET.UNSIGNED, "CueTime"),
|
||||
0xB7: (EET.MASTER, "CueTrackPositions"),
|
||||
0xF7: (EET.UNSIGNED, "CueTrack"),
|
||||
0xF1: (EET.UNSIGNED, "CueClusterPosition"),
|
||||
0x5378: (EET.UNSIGNED, "CueBlockNumber"),
|
||||
0xEA: (EET.UNSIGNED, "CueCodecState"),
|
||||
0xDB: (EET.MASTER, "CueReference"),
|
||||
0x96: (EET.UNSIGNED, "CueRefTime"),
|
||||
0x97: (EET.UNSIGNED, "CueRefCluster"),
|
||||
0x535F: (EET.UNSIGNED, "CueRefNumber"),
|
||||
0xEB: (EET.UNSIGNED, "CueRefCodecState"),
|
||||
0x1941A469: (EET.MASTER, "Attachments"),
|
||||
0x61A7: (EET.MASTER, "AttachedFile"),
|
||||
0x467E: (EET.TEXTU, "FileDescription"),
|
||||
0x466E: (EET.TEXTU, "FileName"),
|
||||
0x4660: (EET.TEXTA, "FileMimeType"),
|
||||
0x465C: (EET.BINARY, "FileData"),
|
||||
0x46AE: (EET.UNSIGNED, "FileUID"),
|
||||
0x4675: (EET.BINARY, "FileReferral"),
|
||||
0x4661: (EET.UNSIGNED, "FileUsedStartTime"),
|
||||
0x4662: (EET.UNSIGNED, "FileUsedEndTime"),
|
||||
0x1043A770: (EET.MASTER, "Chapters"),
|
||||
0x45B9: (EET.MASTER, "EditionEntry"),
|
||||
0x45BC: (EET.UNSIGNED, "EditionUID"),
|
||||
0x45BD: (EET.UNSIGNED, "EditionFlagHidden"),
|
||||
0x45DB: (EET.UNSIGNED, "EditionFlagDefault"),
|
||||
0x45DD: (EET.UNSIGNED, "EditionFlagOrdered"),
|
||||
0xB6: (EET.MASTER, "ChapterAtom"),
|
||||
0x73C4: (EET.UNSIGNED, "ChapterUID"),
|
||||
0x91: (EET.UNSIGNED, "ChapterTimeStart"),
|
||||
0x92: (EET.UNSIGNED, "ChapterTimeEnd"),
|
||||
0x98: (EET.UNSIGNED, "ChapterFlagHidden"),
|
||||
0x4598: (EET.UNSIGNED, "ChapterFlagEnabled"),
|
||||
0x6E67: (EET.BINARY, "ChapterSegmentUID"),
|
||||
0x6EBC: (EET.UNSIGNED, "ChapterSegmentEditionUID"),
|
||||
0x63C3: (EET.UNSIGNED, "ChapterPhysicalEquiv"),
|
||||
0x8F: (EET.MASTER, "ChapterTrack"),
|
||||
0x89: (EET.UNSIGNED, "ChapterTrackNumber"),
|
||||
0x80: (EET.MASTER, "ChapterDisplay"),
|
||||
0x85: (EET.TEXTU, "ChapString"),
|
||||
0x437C: (EET.TEXTA, "ChapLanguage"),
|
||||
0x437E: (EET.TEXTA, "ChapCountry"),
|
||||
0x6944: (EET.MASTER, "ChapProcess"),
|
||||
0x6955: (EET.UNSIGNED, "ChapProcessCodecID"),
|
||||
0x450D: (EET.BINARY, "ChapProcessPrivate"),
|
||||
0x6911: (EET.MASTER, "ChapProcessCommand"),
|
||||
0x6922: (EET.UNSIGNED, "ChapProcessTime"),
|
||||
0x6933: (EET.BINARY, "ChapProcessData"),
|
||||
0x1254C367: (EET.MASTER, "Tags"),
|
||||
0x7373: (EET.MASTER, "Tag"),
|
||||
0x63C0: (EET.MASTER, "Targets"),
|
||||
0x68CA: (EET.UNSIGNED, "TargetTypeValue"),
|
||||
0x63CA: (EET.TEXTA, "TargetType"),
|
||||
0x63C5: (EET.UNSIGNED, "TagTrackUID"),
|
||||
0x63C9: (EET.UNSIGNED, "TagEditionUID"),
|
||||
0x63C4: (EET.UNSIGNED, "TagChapterUID"),
|
||||
0x63C6: (EET.UNSIGNED, "TagAttachmentUID"),
|
||||
0x67C8: (EET.MASTER, "SimpleTag"),
|
||||
0x45A3: (EET.TEXTU, "TagName"),
|
||||
0x447A: (EET.TEXTA, "TagLanguage"),
|
||||
0x4484: (EET.UNSIGNED, "TagDefault"),
|
||||
0x4487: (EET.TEXTU, "TagString"),
|
||||
0x4485: (EET.BINARY, "TagBinary"),
|
||||
0x56AA: (EET.UNSIGNED, "CodecDelay"),
|
||||
0x56BB: (EET.UNSIGNED, "SeekPreRoll"),
|
||||
0xF0: (EET.UNSIGNED, "CueRelativePosition"),
|
||||
0x53C0: (EET.UNSIGNED, "AlphaMode"),
|
||||
0x55B2: (EET.UNSIGNED, "BitsPerChannel"),
|
||||
0x55B5: (EET.UNSIGNED, "CbSubsamplingHorz"),
|
||||
0x55B6: (EET.UNSIGNED, "CbSubsamplingVert"),
|
||||
0x5654: (EET.TEXTU, "ChapterStringUID"),
|
||||
0x55B7: (EET.UNSIGNED, "ChromaSitingHorz"),
|
||||
0x55B8: (EET.UNSIGNED, "ChromaSitingVert"),
|
||||
0x55B3: (EET.UNSIGNED, "ChromaSubsamplingHorz"),
|
||||
0x55B4: (EET.UNSIGNED, "ChromaSubsamplingVert"),
|
||||
0x55B0: (EET.MASTER, "Colour"),
|
||||
0x234E7A: (EET.UNSIGNED, "DefaultDecodedFieldDuration"),
|
||||
0x75A2: (EET.SIGNED, "DiscardPadding"),
|
||||
0x9D: (EET.UNSIGNED, "FieldOrder"),
|
||||
0x55D9: (EET.FLOAT, "LuminanceMax"),
|
||||
0x55DA: (EET.FLOAT, "LuminanceMin"),
|
||||
0x55D0: (EET.MASTER, "MasteringMetadata"),
|
||||
0x55B1: (EET.UNSIGNED, "MatrixCoefficients"),
|
||||
0x55BC: (EET.UNSIGNED, "MaxCLL"),
|
||||
0x55BD: (EET.UNSIGNED, "MaxFALL"),
|
||||
0x55BB: (EET.UNSIGNED, "Primaries"),
|
||||
0x55D5: (EET.FLOAT, "PrimaryBChromaticityX"),
|
||||
0x55D6: (EET.FLOAT, "PrimaryBChromaticityY"),
|
||||
0x55D3: (EET.FLOAT, "PrimaryGChromaticityX"),
|
||||
0x55D4: (EET.FLOAT, "PrimaryGChromaticityY"),
|
||||
0x55D1: (EET.FLOAT, "PrimaryRChromaticityX"),
|
||||
0x55D2: (EET.FLOAT, "PrimaryRChromaticityY"),
|
||||
0x55B9: (EET.UNSIGNED, "Range"),
|
||||
0x55BA: (EET.UNSIGNED, "TransferCharacteristics"),
|
||||
0x55D7: (EET.FLOAT, "WhitePointChromaticityX"),
|
||||
0x55D8: (EET.FLOAT, "WhitePointChromaticityY"),
|
||||
}
|
||||
|
||||
def read_simple_element(f, type_, size):
|
||||
date = None
|
||||
if size==0:
|
||||
return ""
|
||||
|
||||
if type_==EET.UNSIGNED:
|
||||
data=read_fixedlength_number(f, size, False)
|
||||
elif type_==EET.SIGNED:
|
||||
data=read_fixedlength_number(f, size, True)
|
||||
elif type_==EET.TEXTA:
|
||||
data=f.read(size)
|
||||
data = data.replace(b"\x00", b"") # filter out \0, for gstreamer
|
||||
data = data.decode("ascii")
|
||||
elif type_==EET.TEXTU:
|
||||
data=f.read(size)
|
||||
data = data.replace(b"\x00", b"") # filter out \0, for gstreamer
|
||||
data = data.decode("UTF-8")
|
||||
elif type_==EET.MASTER:
|
||||
data=read_ebml_element_tree(f, size)
|
||||
elif type_==EET.DATE:
|
||||
data=read_fixedlength_number(f, size, True)
|
||||
data*= 1e-9
|
||||
data+= (datetime.datetime(2001, 1, 1) - datetime.datetime(1970, 1, 1)).total_seconds()
|
||||
# now should be UNIX date
|
||||
elif type_==EET.FLOAT:
|
||||
if size==4:
|
||||
data = f.read(4)
|
||||
data = unpack(">f", data)[0]
|
||||
elif size==8:
|
||||
data = f.read(8)
|
||||
data = unpack(">d", data)[0]
|
||||
else:
|
||||
data=read_fixedlength_number(f, size, False)
|
||||
sys.stderr.write("mkvparse: Floating point of size %d is not supported\n" % size)
|
||||
data = None
|
||||
else:
|
||||
data=f.read(size)
|
||||
return data
|
||||
|
||||
def read_ebml_element_tree(f, total_size):
|
||||
'''
|
||||
Build tree of elements, reading f until total_size reached
|
||||
Don't use for the whole segment, it's not Haskell
|
||||
|
||||
Returns list of pairs (element_name, element_value).
|
||||
element_value can also be list of pairs
|
||||
'''
|
||||
childs=[]
|
||||
while(total_size>0):
|
||||
(id_, size, hsize) = read_ebml_element_header(f)
|
||||
if size == -1:
|
||||
sys.stderr.write("mkvparse: Element %x without size? Damaged data? Skipping %d bytes\n" % (id_, size, total_size))
|
||||
f.read(total_size);
|
||||
break;
|
||||
if size>total_size:
|
||||
sys.stderr.write("mkvparse: Element %x with size %d? Damaged data? Skipping %d bytes\n" % (id_, size, total_size))
|
||||
f.read(total_size);
|
||||
break
|
||||
type_ = EET.BINARY
|
||||
name = "unknown_%x"%id_
|
||||
if id_ in element_types_names:
|
||||
(type_, name) = element_types_names[id_]
|
||||
data = read_simple_element(f, type_, size)
|
||||
total_size-=(size+hsize)
|
||||
childs.append((name, (type_, data)))
|
||||
return childs
|
||||
|
||||
|
||||
class MatroskaHandler:
|
||||
""" User for mkvparse should override these methods """
|
||||
def tracks_available(self):
|
||||
pass
|
||||
def segment_info_available(self):
|
||||
pass
|
||||
def frame(self, track_id, timestamp, data, more_laced_frames, duration, keyframe, invisible, discardable):
|
||||
pass
|
||||
def ebml_top_element(self, id_, name_, type_, data_):
|
||||
pass
|
||||
def before_handling_an_element(self):
|
||||
pass
|
||||
def begin_handling_ebml_element(self, id_, name, type_, headersize, datasize):
|
||||
return type_
|
||||
def element_data_available(self, id_, name, type_, headersize, data):
|
||||
pass
|
||||
|
||||
def handle_block(buffer, buffer_pos, handler, cluster_timecode, timecode_scale=1000000, duration=None, header_removal_headers_for_tracks={}):
|
||||
'''
|
||||
Decode a block, handling all lacings, send it to handler with appropriate timestamp, track number
|
||||
'''
|
||||
pos=0
|
||||
(tracknum, pos) = parse_matroska_number(buffer, pos, signed=False)
|
||||
(tcode, pos) = parse_fixedlength_number(buffer, pos, 2, signed=True)
|
||||
flags = ord(buffer[pos]); pos+=1
|
||||
f_keyframe = (flags&0x80 == 0x80)
|
||||
f_invisible = (flags&0x08 == 0x08)
|
||||
f_discardable = (flags&0x01 == 0x01)
|
||||
laceflags=flags&0x06
|
||||
|
||||
block_timecode = (cluster_timecode + tcode)*(timecode_scale*0.000000001)
|
||||
|
||||
header_removal_prefix = b""
|
||||
if tracknum in header_removal_headers_for_tracks:
|
||||
# header_removal_prefix = header_removal_headers_for_tracks[tracknum]
|
||||
raise NotImplementedError
|
||||
|
||||
if laceflags == 0x00: # no lacing
|
||||
# buf = buffer[pos:]
|
||||
handler.frame(tracknum, block_timecode, buffer_pos+pos, len(buffer)-pos,
|
||||
0, duration, f_keyframe, f_invisible, f_discardable)
|
||||
return
|
||||
|
||||
numframes = ord(buffer[pos]); pos+=1
|
||||
numframes+=1
|
||||
|
||||
lengths=[]
|
||||
|
||||
if laceflags == 0x02: # Xiph lacing
|
||||
accumlength=0
|
||||
for i in range(numframes-1):
|
||||
(l, pos) = parse_xiph_number(buffer, pos)
|
||||
lengths.append(l)
|
||||
accumlength+=l
|
||||
lengths.append(len(buffer)-pos-accumlength)
|
||||
elif laceflags == 0x06: # EBML lacing
|
||||
accumlength=0
|
||||
if numframes:
|
||||
(flength, pos) = parse_matroska_number(buffer, pos, signed=False)
|
||||
lengths.append(flength)
|
||||
accumlength+=flength
|
||||
for i in range(numframes-2):
|
||||
(l, pos) = parse_matroska_number(buffer, pos, signed=True)
|
||||
flength+=l
|
||||
lengths.append(flength)
|
||||
accumlength+=flength
|
||||
lengths.append(len(buffer)-pos-accumlength)
|
||||
elif laceflags==0x04: # Fixed size lacing
|
||||
fl=int((len(buffer)-pos)/numframes)
|
||||
for i in range(numframes):
|
||||
lengths.append(fl)
|
||||
|
||||
more_laced_frames=numframes-1
|
||||
for i in lengths:
|
||||
# buf = buffer[pos:pos+i]
|
||||
handler.frame(tracknum, block_timecode, buffer_pos+pos, i, more_laced_frames, duration,
|
||||
f_keyframe, f_invisible, f_discardable)
|
||||
pos+=i
|
||||
more_laced_frames-=1
|
||||
|
||||
|
||||
def resync(f):
|
||||
sys.stderr.write("mvkparse: Resyncing\n")
|
||||
while True:
|
||||
b = f.read(1);
|
||||
if b == b"": return (None, None);
|
||||
if b == b"\x1F":
|
||||
b2 = f.read(3);
|
||||
if b2 == b"\x43\xB6\x75":
|
||||
(seglen, x) = read_matroska_number(f)
|
||||
return (0x1F43B675, seglen, x+4) # cluster
|
||||
if b == b"\x18":
|
||||
b2 = f.read(3)
|
||||
if b2 == b"\x53\x80\x67":
|
||||
(seglen, x) = read_matroska_number(f)
|
||||
return (0x18538067, seglen, x+4) # segment
|
||||
if b == b"\x16":
|
||||
b2 = f.read(3)
|
||||
if b2 == b"\x54\xAE\x6B":
|
||||
(seglen ,x )= read_matroska_number(f)
|
||||
return (0x1654AE6B, seglen, x+4) # tracks
|
||||
|
||||
|
||||
|
||||
|
||||
def mkvparse(f, handler):
|
||||
'''
|
||||
Read mkv file f and call handler methods when track or segment information is ready or when frame is read.
|
||||
Handles lacing, timecodes (except of per-track scaling)
|
||||
'''
|
||||
timecode_scale = 1000000
|
||||
current_cluster_timecode = 0
|
||||
resync_element_id = None
|
||||
resync_element_size = None
|
||||
resync_element_headersize = None
|
||||
header_removal_headers_for_tracks = {}
|
||||
while f:
|
||||
(id_, size, hsize) = (None, None, None)
|
||||
tree = None
|
||||
data = None
|
||||
(type_, name) = (None, None)
|
||||
try:
|
||||
if not resync_element_id:
|
||||
try:
|
||||
handler.before_handling_an_element()
|
||||
(id_, size, hsize) = read_ebml_element_header(f)
|
||||
except StopIteration:
|
||||
break;
|
||||
if not (id_ in element_types_names):
|
||||
sys.stderr.write("mkvparse: Unknown element with id %x and size %d\n"%(id_, size))
|
||||
(resync_element_id, resync_element_size, resync_element_headersize) = resync(f)
|
||||
if resync_element_id:
|
||||
continue;
|
||||
else:
|
||||
break;
|
||||
else:
|
||||
id_ = resync_element_id
|
||||
size=resync_element_size
|
||||
hsize=resync_element_headersize
|
||||
resync_element_id = None
|
||||
resync_element_size = None
|
||||
resync_element_headersize = None
|
||||
|
||||
(type_, name) = element_types_names[id_]
|
||||
(type_, name) = element_types_names[id_]
|
||||
type_ = handler.begin_handling_ebml_element(id_, name, type_, hsize, size)
|
||||
|
||||
if type_ == EET.MASTER:
|
||||
tree = read_ebml_element_tree(f, size)
|
||||
data = tree
|
||||
|
||||
except Exception:
|
||||
traceback.print_exc()
|
||||
handler.before_handling_an_element()
|
||||
(resync_element_id, resync_element_size, resync_element_headersize) = resync(f)
|
||||
if resync_element_id:
|
||||
continue;
|
||||
else:
|
||||
break;
|
||||
|
||||
if name=="EBML" and type(data) == list:
|
||||
d = dict(tree)
|
||||
if 'EBMLReadVersion' in d:
|
||||
if d['EBMLReadVersion'][1]>1: sys.stderr.write("mkvparse: Warning: EBMLReadVersion too big\n")
|
||||
if 'DocTypeReadVersion' in d:
|
||||
if d['DocTypeReadVersion'][1]>2: sys.stderr.write("mkvparse: Warning: DocTypeReadVersion too big\n")
|
||||
dt = d['DocType'][1]
|
||||
if dt != "matroska" and dt != "webm":
|
||||
sys.stderr.write("mkvparse: Warning: EBML DocType is not \"matroska\" or \"webm\"")
|
||||
elif name=="Info" and type(data) == list:
|
||||
handler.segment_info = tree
|
||||
handler.segment_info_available()
|
||||
|
||||
d = dict(tree)
|
||||
if "TimecodeScale" in d:
|
||||
timecode_scale = d["TimecodeScale"][1]
|
||||
elif name=="Tracks" and type(data) == list:
|
||||
handler.tracks={}
|
||||
for (ten, (_t, track)) in tree:
|
||||
if ten != "TrackEntry": continue
|
||||
d = dict(track)
|
||||
n = d['TrackNumber'][1]
|
||||
handler.tracks[n]=d
|
||||
tt = d['TrackType'][1]
|
||||
if tt==0x01: d['type']='video'
|
||||
elif tt==0x02: d['type']='audio'
|
||||
elif tt==0x03: d['type']='complex'
|
||||
elif tt==0x10: d['type']='logo'
|
||||
elif tt==0x11: d['type']='subtitle'
|
||||
elif tt==0x12: d['type']='button'
|
||||
elif tt==0x20: d['type']='control'
|
||||
if 'TrackTimecodeScale' in d:
|
||||
sys.stderr.write("mkvparse: Warning: TrackTimecodeScale is not supported\n")
|
||||
if 'ContentEncodings' in d:
|
||||
try:
|
||||
compr = dict(d["ContentEncodings"][1][0][1][1][0][1][1])
|
||||
if compr["ContentCompAlgo"][1] == 3:
|
||||
header_removal_headers_for_tracks[n] = compr["ContentCompSettings"][1]
|
||||
else:
|
||||
sys.stderr.write("mkvparse: Warning: compression other than " \
|
||||
"header removal is not supported\n")
|
||||
except:
|
||||
sys.stderr.write("mkvparse: Warning: unsuccessfully tried " \
|
||||
"to handle header removal compression\n")
|
||||
handler.tracks_available()
|
||||
# cluster contents:
|
||||
elif name=="Timecode" and type_ == EET.UNSIGNED:
|
||||
data=read_fixedlength_number(f, size, False)
|
||||
current_cluster_timecode = data;
|
||||
elif name=="SimpleBlock" and type_ == EET.BINARY:
|
||||
pos = f.tell()
|
||||
data=f.read(size)
|
||||
handle_block(data, pos, handler, current_cluster_timecode, timecode_scale, None, header_removal_headers_for_tracks)
|
||||
elif name=="BlockGroup" and type_ == EET.MASTER:
|
||||
d2 = dict(tree)
|
||||
duration=None
|
||||
raise NotImplementedError
|
||||
# if 'BlockDuration' in d2:
|
||||
# duration = d2['BlockDuration'][1]
|
||||
# duration = duration*0.000000001*timecode_scale
|
||||
# if 'Block' in d2:
|
||||
# handle_block(d2['Block'][1], None, handler, current_cluster_timecode, timecode_scale, duration, header_removal_headers_for_tracks)
|
||||
else:
|
||||
if type_!=EET.JUST_GO_ON and type_!=EET.MASTER:
|
||||
data = read_simple_element(f, type_, size)
|
||||
|
||||
handler.ebml_top_element(id_, name, type_, data);
|
||||
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
print("Run mkvuser.py for the example")
|
||||
|
|
@ -0,0 +1,107 @@
|
|||
import os
|
||||
import select
|
||||
import fcntl
|
||||
from itertools import count
|
||||
from collections import deque
|
||||
|
||||
Empty = OSError
|
||||
Full = OSError
|
||||
ExistentialError = OSError
|
||||
|
||||
class PollableQueue(object):
|
||||
"""A Queue that you can poll().
|
||||
Only works with a single producer.
|
||||
"""
|
||||
def __init__(self, maxlen=None):
|
||||
with open("/proc/sys/fs/pipe-max-size") as f:
|
||||
max_maxlen = int(f.read().rstrip())
|
||||
|
||||
if maxlen is None:
|
||||
maxlen = max_maxlen
|
||||
else:
|
||||
maxlen = min(maxlen, max_maxlen)
|
||||
|
||||
self._maxlen = maxlen
|
||||
self._q = deque()
|
||||
self._get_fd, self._put_fd = os.pipe()
|
||||
fcntl.fcntl(self._get_fd, fcntl.F_SETFL, os.O_NONBLOCK)
|
||||
fcntl.fcntl(self._put_fd, fcntl.F_SETFL, os.O_NONBLOCK)
|
||||
|
||||
fcntl.fcntl(self._get_fd, fcntl.F_SETLEASE + 7, self._maxlen)
|
||||
fcntl.fcntl(self._put_fd, fcntl.F_SETLEASE + 7, self._maxlen)
|
||||
|
||||
get_poller = select.epoll()
|
||||
put_poller = select.epoll()
|
||||
get_poller.register(self._get_fd, select.EPOLLIN)
|
||||
put_poller.register(self._put_fd, select.EPOLLOUT)
|
||||
|
||||
self._get_poll = get_poller.poll
|
||||
self._put_poll = put_poller.poll
|
||||
|
||||
|
||||
def get_fd(self):
|
||||
return self._get_fd
|
||||
|
||||
def put_fd(self):
|
||||
return self._put_fd
|
||||
|
||||
def put(self, item, block=True, timeout=None):
|
||||
if block:
|
||||
while self._put_poll(timeout if timeout is not None else -1):
|
||||
try:
|
||||
# TODO: This is broken for multiple push threads when the queue is full.
|
||||
return self.put_nowait(item)
|
||||
except OSError as e:
|
||||
if e.errno != 11:
|
||||
raise
|
||||
|
||||
raise Full()
|
||||
else:
|
||||
return self.put_nowait(item)
|
||||
|
||||
def put_nowait(self, item):
|
||||
self._q.appendleft(item)
|
||||
os.write(self._put_fd, b"\x00")
|
||||
|
||||
def get(self, block=True, timeout=None):
|
||||
if block:
|
||||
while self._get_poll(timeout if timeout is not None else -1):
|
||||
try:
|
||||
return self.get_nowait()
|
||||
except OSError as e:
|
||||
if e.errno != 11:
|
||||
raise
|
||||
|
||||
raise Empty()
|
||||
else:
|
||||
return self.get_nowait()
|
||||
|
||||
def get_nowait(self):
|
||||
os.read(self._get_fd, 1)
|
||||
return self._q.pop()
|
||||
|
||||
def get_multiple(self, block=True, timeout=None):
|
||||
if block:
|
||||
if self._get_poll(timeout if timeout is not None else -1):
|
||||
return self.get_multiple_nowait()
|
||||
else:
|
||||
raise Empty()
|
||||
else:
|
||||
return self.get_multiple_nowait()
|
||||
|
||||
def get_multiple_nowait(self, max_messages=None):
|
||||
num_read = len(os.read(self._get_fd, max_messages or self._maxlen))
|
||||
return [self._q.pop() for _ in range(num_read)]
|
||||
|
||||
def empty(self):
|
||||
return len(self._q) == 0
|
||||
|
||||
def full(self):
|
||||
return len(self._q) >= self._maxlen
|
||||
|
||||
def close(self):
|
||||
os.close(self._get_fd)
|
||||
os.close(self._put_fd)
|
||||
|
||||
def __len__(self):
|
||||
return len(self._q)
|
||||
|
|
@ -0,0 +1,97 @@
|
|||
import os
|
||||
import re
|
||||
from collections import defaultdict
|
||||
|
||||
SEGMENT_NAME_RE = r'[a-z0-9]{16}[|_][0-9]{4}-[0-9]{2}-[0-9]{2}--[0-9]{2}-[0-9]{2}-[0-9]{2}--[0-9]+'
|
||||
EXPLORER_FILE_RE = r'^({})--([a-z]+\.[a-z0-9]+)$'.format(SEGMENT_NAME_RE)
|
||||
OP_SEGMENT_DIR_RE = r'^({})$'.format(SEGMENT_NAME_RE)
|
||||
|
||||
LOG_FILENAMES = ['rlog.bz2', 'raw_log.bz2', 'log2.gz', 'ilog.7z']
|
||||
CAMERA_FILENAMES = ['fcamera.hevc', 'video.hevc', 'acamera', 'icamera']
|
||||
|
||||
class Route(object):
|
||||
def __init__(self, route_name, data_dir):
|
||||
self.route_name = route_name.replace('_', '|')
|
||||
self._segments = self._get_segments(data_dir)
|
||||
|
||||
@property
|
||||
def segments(self):
|
||||
return self._segments
|
||||
|
||||
def log_paths(self):
|
||||
max_seg_number = self._segments[-1].canonical_name.segment_num
|
||||
log_path_by_seg_num = {s.canonical_name.segment_num: s.log_path for s in self._segments}
|
||||
return [log_path_by_seg_num.get(i, None) for i in range(max_seg_number+1)]
|
||||
|
||||
def camera_paths(self):
|
||||
max_seg_number = self._segments[-1].canonical_name.segment_num
|
||||
camera_path_by_seg_num = {s.canonical_name.segment_num: s.camera_path for s in self._segments}
|
||||
return [camera_path_by_seg_num.get(i, None) for i in range(max_seg_number+1)]
|
||||
|
||||
def _get_segments(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(EXPLORER_FILE_RE, f)
|
||||
op_match = re.match(OP_SEGMENT_DIR_RE, f)
|
||||
|
||||
if explorer_match:
|
||||
segment_name, fn = explorer_match.groups()
|
||||
if segment_name.replace('_', '|').startswith(self.route_name):
|
||||
segment_files[segment_name].append((fullpath, fn))
|
||||
elif op_match and os.path.isdir(fullpath):
|
||||
segment_name, = op_match.groups()
|
||||
if segment_name.startswith(self.route_name):
|
||||
for seg_f in os.listdir(fullpath):
|
||||
segment_files[segment_name].append((os.path.join(fullpath, seg_f), seg_f))
|
||||
elif f == self.route_name:
|
||||
for seg_num in os.listdir(fullpath):
|
||||
if not seg_num.isdigit():
|
||||
continue
|
||||
|
||||
segment_name = '{}--{}'.format(self.route_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 LOG_FILENAMES)
|
||||
except StopIteration:
|
||||
log_path = None
|
||||
|
||||
try:
|
||||
camera_path = next(path for path, filename in files if filename in CAMERA_FILENAMES)
|
||||
except StopIteration:
|
||||
camera_path = None
|
||||
|
||||
segments.append(RouteSegment(segment, log_path, camera_path))
|
||||
|
||||
if len(segments) == 0:
|
||||
raise ValueError('Could not find segments for route {} in data directory {}'.format(self.route_name, data_dir))
|
||||
return sorted(segments, key=lambda seg: seg.canonical_name.segment_num)
|
||||
|
||||
class RouteSegment(object):
|
||||
def __init__(self, name, log_path, camera_path):
|
||||
self._name = RouteSegmentName(name)
|
||||
self.log_path = log_path
|
||||
self.camera_path = camera_path
|
||||
|
||||
@property
|
||||
def name(self): return str(self._name)
|
||||
|
||||
@property
|
||||
def canonical_name(self): return self._name
|
||||
|
||||
class RouteSegmentName(object):
|
||||
def __init__(self, name_str):
|
||||
self._segment_name_str = name_str
|
||||
self._route_name_str, num_str = self._segment_name_str.rsplit("--", 1)
|
||||
self._num = int(num_str)
|
||||
|
||||
@property
|
||||
def segment_num(self): return self._num
|
||||
|
||||
def __str__(self): return self._segment_name_str
|
||||
|
|
@ -0,0 +1,86 @@
|
|||
"""RouteFrameReader indexes and reads frames across routes, by frameId or segment indices."""
|
||||
from tools.lib.framereader import FrameReader
|
||||
|
||||
class _FrameReaderDict(dict):
|
||||
def __init__(self, camera_paths, cache_paths, framereader_kwargs, *args, **kwargs):
|
||||
super(_FrameReaderDict, self).__init__(*args, **kwargs)
|
||||
|
||||
if cache_paths is None:
|
||||
cache_paths = {}
|
||||
if not isinstance(cache_paths, dict):
|
||||
cache_paths = { k: v for k, v in enumerate(cache_paths) }
|
||||
|
||||
self._camera_paths = camera_paths
|
||||
self._cache_paths = cache_paths
|
||||
self._framereader_kwargs = framereader_kwargs
|
||||
|
||||
def __missing__(self, key):
|
||||
if key < len(self._camera_paths) and self._camera_paths[key] is not None:
|
||||
frame_reader = FrameReader(self._camera_paths[key],
|
||||
self._cache_paths.get(key), **self._framereader_kwargs)
|
||||
self[key] = frame_reader
|
||||
return frame_reader
|
||||
else:
|
||||
raise KeyError("Segment index out of bounds: {}".format(key))
|
||||
|
||||
|
||||
class RouteFrameReader(object):
|
||||
"""Reads frames across routes and route segments by frameId."""
|
||||
def __init__(self, camera_paths, cache_paths, frame_id_lookup, **kwargs):
|
||||
"""Create a route framereader.
|
||||
|
||||
Inputs:
|
||||
TODO
|
||||
|
||||
kwargs: Forwarded to the FrameReader function. If cache_prefix is included, that path
|
||||
will also be used for frame position indices.
|
||||
"""
|
||||
self._first_camera_idx = next(i for i in range(len(camera_paths)) if camera_paths[i] is not None)
|
||||
self._frame_readers = _FrameReaderDict(camera_paths, cache_paths, kwargs)
|
||||
self._frame_id_lookup = frame_id_lookup
|
||||
|
||||
@property
|
||||
def w(self):
|
||||
"""Width of each frame in pixels."""
|
||||
return self._frame_readers[self._first_camera_idx].w
|
||||
|
||||
@property
|
||||
def h(self):
|
||||
"""Height of each frame in pixels."""
|
||||
return self._frame_readers[self._first_camera_idx].h
|
||||
|
||||
def get(self, frame_id, **kwargs):
|
||||
"""Get a frame for a route based on frameId.
|
||||
|
||||
Inputs:
|
||||
frame_id: The frameId of the returned frame.
|
||||
kwargs: Forwarded to BaseFrameReader.get. "count" is not implemented.
|
||||
"""
|
||||
segment_num, segment_id = self._frame_id_lookup.get(frame_id, (None, None))
|
||||
if segment_num is None or segment_num == -1 or segment_id == -1:
|
||||
return None
|
||||
else:
|
||||
return self.get_from_segment(segment_num, segment_id, **kwargs)
|
||||
|
||||
def get_from_segment(self, segment_num, segment_id, **kwargs):
|
||||
"""Get a frame from a specific segment with a specific index in that segment (segment_id).
|
||||
|
||||
Inputs:
|
||||
segment_num: The number of the segment.
|
||||
segment_id: The index of the return frame within that segment.
|
||||
kwargs: Forwarded to BaseFrameReader.get. "count" is not implemented.
|
||||
"""
|
||||
if "count" in kwargs:
|
||||
raise NotImplementedError("count")
|
||||
|
||||
return self._frame_readers[segment_num].get(segment_id, **kwargs)[0]
|
||||
|
||||
|
||||
def close(self):
|
||||
frs = self._frame_readers
|
||||
self._frame_readers.clear()
|
||||
for fr in frs:
|
||||
fr.close()
|
||||
|
||||
def __enter__(self): return self
|
||||
def __exit__(self, type, value, traceback): self.close()
|
||||
|
|
@ -0,0 +1,56 @@
|
|||
#!/usr/bin/env python
|
||||
import unittest
|
||||
import requests
|
||||
import tempfile
|
||||
|
||||
from collections import defaultdict
|
||||
import numpy as np
|
||||
from tools.lib.framereader import FrameReader
|
||||
from tools.lib.logreader import LogReader
|
||||
|
||||
class TestReaders(unittest.TestCase):
|
||||
def test_logreader(self):
|
||||
with tempfile.NamedTemporaryFile(suffix=".bz2") as fp:
|
||||
r = requests.get("https://github.com/commaai/comma2k19/blob/master/Example_1/b0c9d2329ad1606b%7C2018-08-02--08-34-47/40/raw_log.bz2?raw=true")
|
||||
fp.write(r.content)
|
||||
fp.flush()
|
||||
|
||||
lr = LogReader(fp.name)
|
||||
hist = defaultdict(int)
|
||||
for l in lr:
|
||||
hist[l.which()] += 1
|
||||
|
||||
self.assertEqual(hist['carControl'], 6000)
|
||||
self.assertEqual(hist['logMessage'], 6857)
|
||||
|
||||
def test_framereader(self):
|
||||
with tempfile.NamedTemporaryFile(suffix=".hevc") as fp:
|
||||
r = requests.get("https://github.com/commaai/comma2k19/blob/master/Example_1/b0c9d2329ad1606b%7C2018-08-02--08-34-47/40/video.hevc?raw=true")
|
||||
fp.write(r.content)
|
||||
fp.flush()
|
||||
|
||||
f = FrameReader(fp.name)
|
||||
|
||||
self.assertEqual(f.frame_count, 1200)
|
||||
self.assertEqual(f.w, 1164)
|
||||
self.assertEqual(f.h, 874)
|
||||
|
||||
|
||||
frame_first_30 = f.get(0, 30)
|
||||
self.assertEqual(len(frame_first_30), 30)
|
||||
|
||||
|
||||
print(frame_first_30[15])
|
||||
|
||||
print("frame_0")
|
||||
frame_0 = f.get(0, 1)
|
||||
frame_15 = f.get(15, 1)
|
||||
|
||||
print(frame_15[0])
|
||||
|
||||
assert np.all(frame_first_30[0] == frame_0[0])
|
||||
assert np.all(frame_first_30[15] == frame_15[0])
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
||||
|
|
@ -0,0 +1 @@
|
|||
vidindex
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
CC := gcc
|
||||
|
||||
vidindex: bitstream.c bitstream.h vidindex.c
|
||||
$(eval $@_TMP := $(shell mktemp))
|
||||
$(CC) -std=c99 bitstream.c vidindex.c -o $($@_TMP)
|
||||
mv $($@_TMP) $@
|
||||
|
|
@ -0,0 +1,118 @@
|
|||
#include <stdbool.h>
|
||||
#include <assert.h>
|
||||
|
||||
#include "bitstream.h"
|
||||
|
||||
static const uint32_t BS_MASKS[33] = {
|
||||
0, 0x1L, 0x3L, 0x7L, 0xFL, 0x1FL,
|
||||
0x3FL, 0x7FL, 0xFFL, 0x1FFL, 0x3FFL, 0x7FFL,
|
||||
0xFFFL, 0x1FFFL, 0x3FFFL, 0x7FFFL, 0xFFFFL, 0x1FFFFL,
|
||||
0x3FFFFL, 0x7FFFFL, 0xFFFFFL, 0x1FFFFFL, 0x3FFFFFL, 0x7FFFFFL,
|
||||
0xFFFFFFL, 0x1FFFFFFL, 0x3FFFFFFL, 0x7FFFFFFL, 0xFFFFFFFL, 0x1FFFFFFFL,
|
||||
0x3FFFFFFFL, 0x7FFFFFFFL, 0xFFFFFFFFL};
|
||||
|
||||
void bs_init(struct bitstream* bs, const uint8_t* buffer, size_t input_size) {
|
||||
bs->buffer_ptr = buffer;
|
||||
bs->buffer_end = buffer + input_size;
|
||||
bs->value = 0;
|
||||
bs->pos = 0;
|
||||
bs->shift = 8;
|
||||
bs->size = input_size * 8;
|
||||
}
|
||||
|
||||
uint32_t bs_get(struct bitstream* bs, int n) {
|
||||
if (n > 32)
|
||||
return 0;
|
||||
|
||||
bs->pos += n;
|
||||
bs->shift += n;
|
||||
while (bs->shift > 8) {
|
||||
if (bs->buffer_ptr < bs->buffer_end) {
|
||||
bs->value <<= 8;
|
||||
bs->value |= *bs->buffer_ptr++;
|
||||
bs->shift -= 8;
|
||||
} else {
|
||||
bs_seek(bs, bs->pos - n);
|
||||
return 0;
|
||||
// bs->value <<= 8;
|
||||
// bs->shift -= 8;
|
||||
}
|
||||
}
|
||||
return (bs->value >> (8 - bs->shift)) & BS_MASKS[n];
|
||||
}
|
||||
|
||||
void bs_seek(struct bitstream* bs, size_t new_pos) {
|
||||
bs->pos = (new_pos / 32) * 32;
|
||||
bs->shift = 8;
|
||||
bs->value = 0;
|
||||
bs_get(bs, new_pos % 32);
|
||||
}
|
||||
|
||||
uint32_t bs_peek(struct bitstream* bs, int n) {
|
||||
struct bitstream bak = *bs;
|
||||
return bs_get(&bak, n);
|
||||
}
|
||||
|
||||
size_t bs_remain(struct bitstream* bs) {
|
||||
return bs->size - bs->pos;
|
||||
}
|
||||
|
||||
int bs_eof(struct bitstream* bs) {
|
||||
return bs_remain(bs) == 0;
|
||||
}
|
||||
|
||||
uint32_t bs_ue(struct bitstream* bs) {
|
||||
static const uint8_t exp_golomb_bits[256] = {
|
||||
8, 7, 6, 6, 5, 5, 5, 5, 4, 4, 4, 4, 4, 4, 4, 4, 3, 3, 3, 3, 3, 3, 3, 3,
|
||||
3, 3, 3, 3, 3, 3, 3, 3, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2,
|
||||
2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 1, 1, 1, 1, 1, 1, 1, 1,
|
||||
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
|
||||
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
|
||||
1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
};
|
||||
uint32_t bits, read = 0;
|
||||
int bits_left;
|
||||
uint8_t coded;
|
||||
int done = 0;
|
||||
bits = 0;
|
||||
// we want to read 8 bits at a time - if we don't have 8 bits,
|
||||
// read what's left, and shift. The exp_golomb_bits calc remains the
|
||||
// same.
|
||||
while (!done) {
|
||||
bits_left = bs_remain(bs);
|
||||
if (bits_left < 8) {
|
||||
read = bs_peek(bs, bits_left) << (8 - bits_left);
|
||||
done = 1;
|
||||
} else {
|
||||
read = bs_peek(bs, 8);
|
||||
if (read == 0) {
|
||||
bs_get(bs, 8);
|
||||
bits += 8;
|
||||
} else {
|
||||
done = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
coded = exp_golomb_bits[read];
|
||||
bs_get(bs, coded);
|
||||
bits += coded;
|
||||
|
||||
// printf("ue - bits %d\n", bits);
|
||||
return bs_get(bs, bits + 1) - 1;
|
||||
}
|
||||
|
||||
int32_t bs_se(struct bitstream* bs) {
|
||||
uint32_t ret;
|
||||
ret = bs_ue(bs);
|
||||
if ((ret & 0x1) == 0) {
|
||||
ret >>= 1;
|
||||
int32_t temp = 0 - ret;
|
||||
return temp;
|
||||
}
|
||||
return (ret + 1) >> 1;
|
||||
}
|
||||
|
|
@ -0,0 +1,26 @@
|
|||
#ifndef bitstream_H
|
||||
#define bitstream_H
|
||||
|
||||
|
||||
#include <stddef.h>
|
||||
#include <stdint.h>
|
||||
|
||||
struct bitstream {
|
||||
const uint8_t *buffer_ptr;
|
||||
const uint8_t *buffer_end;
|
||||
uint64_t value;
|
||||
uint32_t pos;
|
||||
uint32_t shift;
|
||||
size_t size;
|
||||
};
|
||||
|
||||
void bs_init(struct bitstream *bs, const uint8_t *buffer, size_t input_size);
|
||||
void bs_seek(struct bitstream *bs, size_t new_pos);
|
||||
uint32_t bs_get(struct bitstream *bs, int n);
|
||||
uint32_t bs_peek(struct bitstream *bs, int n);
|
||||
size_t bs_remain(struct bitstream *bs);
|
||||
int bs_eof(struct bitstream *bs);
|
||||
uint32_t bs_ue(struct bitstream *bs);
|
||||
int32_t bs_se(struct bitstream *bs);
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,307 @@
|
|||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdbool.h>
|
||||
#include <string.h>
|
||||
#include <assert.h>
|
||||
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <sys/stat.h>
|
||||
#include <sys/mman.h>
|
||||
|
||||
#include "bitstream.h"
|
||||
|
||||
#define START_CODE 0x000001
|
||||
|
||||
static uint32_t read24be(const uint8_t* ptr) {
|
||||
return (ptr[0] << 16) | (ptr[1] << 8) | ptr[2];
|
||||
}
|
||||
static void write32le(FILE *of, uint32_t v) {
|
||||
uint8_t va[4] = {
|
||||
v & 0xff, (v >> 8) & 0xff, (v >> 16) & 0xff, (v >> 24) & 0xff
|
||||
};
|
||||
fwrite(va, 1, sizeof(va), of);
|
||||
}
|
||||
|
||||
// Table 7-1
|
||||
enum hevc_nal_type {
|
||||
HEVC_NAL_TYPE_TRAIL_N = 0,
|
||||
HEVC_NAL_TYPE_TRAIL_R = 1,
|
||||
HEVC_NAL_TYPE_TSA_N = 2,
|
||||
HEVC_NAL_TYPE_TSA_R = 3,
|
||||
HEVC_NAL_TYPE_STSA_N = 4,
|
||||
HEVC_NAL_TYPE_STSA_R = 5,
|
||||
HEVC_NAL_TYPE_RADL_N = 6,
|
||||
HEVC_NAL_TYPE_RADL_R = 7,
|
||||
HEVC_NAL_TYPE_RASL_N = 8,
|
||||
HEVC_NAL_TYPE_RASL_R = 9,
|
||||
HEVC_NAL_TYPE_BLA_W_LP = 16,
|
||||
HEVC_NAL_TYPE_BLA_W_RADL = 17,
|
||||
HEVC_NAL_TYPE_BLA_N_LP = 18,
|
||||
HEVC_NAL_TYPE_IDR_W_RADL = 19,
|
||||
HEVC_NAL_TYPE_IDR_N_LP = 20,
|
||||
HEVC_NAL_TYPE_CRA_NUT = 21,
|
||||
HEVC_NAL_TYPE_RSV_IRAP_VCL23 = 23,
|
||||
HEVC_NAL_TYPE_VPS_NUT = 32,
|
||||
HEVC_NAL_TYPE_SPS_NUT = 33,
|
||||
HEVC_NAL_TYPE_PPS_NUT = 34,
|
||||
HEVC_NAL_TYPE_AUD_NUT = 35,
|
||||
HEVC_NAL_TYPE_EOS_NUT = 36,
|
||||
HEVC_NAL_TYPE_EOB_NUT = 37,
|
||||
HEVC_NAL_TYPE_FD_NUT = 38,
|
||||
HEVC_NAL_TYPE_PREFIX_SEI_NUT = 39,
|
||||
HEVC_NAL_TYPE_SUFFIX_SEI_NUT = 40,
|
||||
};
|
||||
|
||||
// Table 7-7
|
||||
enum hevc_slice_type {
|
||||
HEVC_SLICE_B = 0,
|
||||
HEVC_SLICE_P = 1,
|
||||
HEVC_SLICE_I = 2,
|
||||
};
|
||||
|
||||
static void hevc_index(const uint8_t *data, size_t file_size, FILE *of_prefix, FILE *of_index) {
|
||||
const uint8_t* ptr = data;
|
||||
const uint8_t* ptr_end = data + file_size;
|
||||
|
||||
assert(ptr[0] == 0);
|
||||
ptr++;
|
||||
assert(read24be(ptr) == START_CODE);
|
||||
|
||||
// pps. ignore for now
|
||||
uint32_t num_extra_slice_header_bits = 0;
|
||||
uint32_t dependent_slice_segments_enabled_flag = 0;
|
||||
|
||||
while (ptr < ptr_end) {
|
||||
const uint8_t* next = ptr+1;
|
||||
for (; next < ptr_end-4; next++) {
|
||||
if (read24be(next) == START_CODE) break;
|
||||
}
|
||||
size_t nal_size = next - ptr;
|
||||
if (nal_size < 6) {
|
||||
break;
|
||||
}
|
||||
|
||||
{
|
||||
struct bitstream bs = {0};
|
||||
bs_init(&bs, ptr, nal_size);
|
||||
|
||||
uint32_t start_code = bs_get(&bs, 24);
|
||||
assert(start_code == 0x000001);
|
||||
|
||||
// nal_unit_header
|
||||
uint32_t forbidden_zero_bit = bs_get(&bs, 1);
|
||||
uint32_t nal_unit_type = bs_get(&bs, 6);
|
||||
uint32_t nuh_layer_id = bs_get(&bs, 6);
|
||||
uint32_t nuh_temporal_id_plus1 = bs_get(&bs, 3);
|
||||
|
||||
// if (nal_unit_type != 1) printf("%3d -- %3d %10d %lu\n", nal_unit_type, frame_num, (uint32_t)(ptr-data), nal_size);
|
||||
|
||||
switch (nal_unit_type) {
|
||||
case HEVC_NAL_TYPE_VPS_NUT:
|
||||
case HEVC_NAL_TYPE_SPS_NUT:
|
||||
case HEVC_NAL_TYPE_PPS_NUT:
|
||||
fwrite(ptr, 1, nal_size, of_prefix);
|
||||
break;
|
||||
case HEVC_NAL_TYPE_TRAIL_N:
|
||||
case HEVC_NAL_TYPE_TRAIL_R:
|
||||
case HEVC_NAL_TYPE_TSA_N:
|
||||
case HEVC_NAL_TYPE_TSA_R:
|
||||
case HEVC_NAL_TYPE_STSA_N:
|
||||
case HEVC_NAL_TYPE_STSA_R:
|
||||
case HEVC_NAL_TYPE_RADL_N:
|
||||
case HEVC_NAL_TYPE_RADL_R:
|
||||
case HEVC_NAL_TYPE_RASL_N:
|
||||
case HEVC_NAL_TYPE_RASL_R:
|
||||
case HEVC_NAL_TYPE_BLA_W_LP:
|
||||
case HEVC_NAL_TYPE_BLA_W_RADL:
|
||||
case HEVC_NAL_TYPE_BLA_N_LP:
|
||||
case HEVC_NAL_TYPE_IDR_W_RADL:
|
||||
case HEVC_NAL_TYPE_IDR_N_LP:
|
||||
case HEVC_NAL_TYPE_CRA_NUT: {
|
||||
// slice_segment_header
|
||||
uint32_t first_slice_segment_in_pic_flag = bs_get(&bs, 1);
|
||||
if (nal_unit_type >= HEVC_NAL_TYPE_BLA_W_LP && nal_unit_type <= HEVC_NAL_TYPE_RSV_IRAP_VCL23) {
|
||||
uint32_t no_output_of_prior_pics_flag = bs_get(&bs, 1);
|
||||
}
|
||||
uint32_t slice_pic_parameter_set_id = bs_get(&bs, 1);
|
||||
if (!first_slice_segment_in_pic_flag) {
|
||||
// ...
|
||||
break;
|
||||
}
|
||||
|
||||
if (!dependent_slice_segments_enabled_flag) {
|
||||
for (int i=0; i<num_extra_slice_header_bits; i++) {
|
||||
bs_get(&bs, 1);
|
||||
}
|
||||
uint32_t slice_type = bs_ue(&bs);
|
||||
|
||||
// write the index
|
||||
write32le(of_index, slice_type);
|
||||
write32le(of_index, ptr - data);
|
||||
|
||||
// ...
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
//...
|
||||
// emulation_prevention_three_byte
|
||||
}
|
||||
|
||||
ptr = next;
|
||||
}
|
||||
|
||||
write32le(of_index, -1);
|
||||
write32le(of_index, file_size);
|
||||
}
|
||||
|
||||
// Table 7-1
|
||||
enum h264_nal_type {
|
||||
H264_NAL_SLICE = 1,
|
||||
H264_NAL_DPA = 2,
|
||||
H264_NAL_DPB = 3,
|
||||
H264_NAL_DPC = 4,
|
||||
H264_NAL_IDR_SLICE = 5,
|
||||
H264_NAL_SEI = 6,
|
||||
H264_NAL_SPS = 7,
|
||||
H264_NAL_PPS = 8,
|
||||
H264_NAL_AUD = 9,
|
||||
H264_NAL_END_SEQUENCE = 10,
|
||||
H264_NAL_END_STREAM = 11,
|
||||
H264_NAL_FILLER_DATA = 12,
|
||||
H264_NAL_SPS_EXT = 13,
|
||||
H264_NAL_AUXILIARY_SLICE = 19,
|
||||
};
|
||||
|
||||
enum h264_slice_type {
|
||||
H264_SLICE_P = 0,
|
||||
H264_SLICE_B = 1,
|
||||
H264_SLICE_I = 2,
|
||||
// ...
|
||||
};
|
||||
|
||||
static void h264_index(const uint8_t *data, size_t file_size, FILE *of_prefix, FILE *of_index) {
|
||||
const uint8_t* ptr = data;
|
||||
const uint8_t* ptr_end = data + file_size;
|
||||
|
||||
assert(ptr[0] == 0);
|
||||
ptr++;
|
||||
assert(read24be(ptr) == START_CODE);
|
||||
|
||||
|
||||
uint32_t sps_log2_max_frame_num_minus4;
|
||||
|
||||
|
||||
int last_frame_num = -1;
|
||||
|
||||
while (ptr < ptr_end) {
|
||||
const uint8_t* next = ptr+1;
|
||||
for (; next < ptr_end-4; next++) {
|
||||
if (read24be(next) == START_CODE) break;
|
||||
}
|
||||
size_t nal_size = next - ptr;
|
||||
if (nal_size < 5) {
|
||||
break;
|
||||
}
|
||||
|
||||
{
|
||||
struct bitstream bs = {0};
|
||||
bs_init(&bs, ptr, nal_size);
|
||||
|
||||
uint32_t start_code = bs_get(&bs, 24);
|
||||
assert(start_code == 0x000001);
|
||||
|
||||
// nal_unit_header
|
||||
uint32_t forbidden_zero_bit = bs_get(&bs, 1);
|
||||
uint32_t nal_ref_idx = bs_get(&bs, 2);
|
||||
uint32_t nal_unit_type = bs_get(&bs, 5);
|
||||
|
||||
switch (nal_unit_type) {
|
||||
case H264_NAL_SPS:
|
||||
|
||||
{
|
||||
uint32_t profile_idx = bs_get(&bs, 8);
|
||||
uint32_t constraint_sets = bs_get(&bs, 4);
|
||||
uint32_t reserved = bs_get(&bs, 5);
|
||||
uint32_t level_idc = bs_get(&bs, 5);
|
||||
uint32_t seq_parameter_set_id = bs_ue(&bs);
|
||||
sps_log2_max_frame_num_minus4 = bs_ue(&bs);
|
||||
}
|
||||
|
||||
// fallthrough
|
||||
case H264_NAL_PPS:
|
||||
fwrite(ptr, 1, nal_size, of_prefix);
|
||||
break;
|
||||
|
||||
case H264_NAL_SLICE:
|
||||
case H264_NAL_IDR_SLICE: {
|
||||
// slice header
|
||||
uint32_t first_mb_in_slice = bs_ue(&bs);
|
||||
uint32_t slice_type = bs_ue(&bs);
|
||||
uint32_t pic_parameter_set_id = bs_ue(&bs);
|
||||
|
||||
uint32_t frame_num = bs_get(&bs, sps_log2_max_frame_num_minus4+4);
|
||||
|
||||
if (first_mb_in_slice == 0) {
|
||||
write32le(of_index, slice_type);
|
||||
write32le(of_index, ptr - data);
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
ptr = next;
|
||||
}
|
||||
|
||||
write32le(of_index, -1);
|
||||
write32le(of_index, file_size);
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
if (argc != 5) {
|
||||
fprintf(stderr, "usage: %s h264|hevc file_path out_prefix out_index\n", argv[0]);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
const char* file_type = argv[1];
|
||||
const char* file_path = argv[2];
|
||||
|
||||
int fd = open(file_path, O_RDONLY, 0);
|
||||
if (fd < 0) {
|
||||
fprintf(stderr, "error: couldn't open %s\n", file_path);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
FILE *of_prefix = fopen(argv[3], "wb");
|
||||
assert(of_prefix);
|
||||
FILE *of_index = fopen(argv[4], "wb");
|
||||
assert(of_index);
|
||||
|
||||
off_t file_size = lseek(fd, 0, SEEK_END);
|
||||
lseek(fd, 0, SEEK_SET);
|
||||
|
||||
assert(file_size > 4);
|
||||
|
||||
const uint8_t* data = (const uint8_t*)mmap(NULL, file_size, PROT_READ, MAP_PRIVATE, fd, 0);
|
||||
assert(data != MAP_FAILED);
|
||||
|
||||
if (strcmp(file_type, "hevc") == 0) {
|
||||
hevc_index(data, file_size, of_prefix, of_index);
|
||||
} else if (strcmp(file_type, "h264") == 0) {
|
||||
h264_index(data, file_size, of_prefix, of_index);
|
||||
} else {
|
||||
assert(false);
|
||||
}
|
||||
|
||||
munmap((void*)data, file_size);
|
||||
close(fd);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,30 @@
|
|||
import numpy as np
|
||||
import cv2
|
||||
|
||||
def rot_matrix(roll, pitch, yaw):
|
||||
cr, sr = np.cos(roll), np.sin(roll)
|
||||
cp, sp = np.cos(pitch), np.sin(pitch)
|
||||
cy, sy = np.cos(yaw), np.sin(yaw)
|
||||
rr = np.array([[1,0,0],[0, cr,-sr],[0, sr, cr]])
|
||||
rp = np.array([[cp,0,sp],[0, 1,0],[-sp, 0, cp]])
|
||||
ry = np.array([[cy,-sy,0],[sy, cy,0],[0, 0, 1]])
|
||||
return ry.dot(rp.dot(rr))
|
||||
|
||||
def draw_pose(img, pose, loc, W=160, H=320, xyoffset=(0,0), faceprob=0):
|
||||
rcmat = np.zeros((3,4))
|
||||
rcmat[:,:3] = rot_matrix(*pose[0:3]) * 0.5
|
||||
rcmat[0,3] = (loc[0]+0.5) * W
|
||||
rcmat[1,3] = (loc[1]+0.5) * H
|
||||
rcmat[2,3] = 1.0
|
||||
# draw nose
|
||||
p1 = np.dot(rcmat, [0,0,0,1])[0:2]
|
||||
p2 = np.dot(rcmat, [0,0,100,1])[0:2]
|
||||
tr = tuple([int(round(x + xyoffset[i])) for i,x in enumerate(p1)])
|
||||
pr = tuple([int(round(x + xyoffset[i])) for i,x in enumerate(p2)])
|
||||
if faceprob > 0.4:
|
||||
color = (255,255,0)
|
||||
cv2.line(img, tr, pr, color=(255,255,0), thickness=3)
|
||||
else:
|
||||
color = (64,64,64)
|
||||
cv2.circle(img, tr, 7, color=color)
|
||||
|
||||
|
|
@ -0,0 +1,79 @@
|
|||
#!/usr/bin/env python3
|
||||
import os
|
||||
import argparse
|
||||
import pygame
|
||||
import numpy as np
|
||||
import cv2
|
||||
|
||||
from cereal import log
|
||||
import cereal.messaging as messaging
|
||||
|
||||
from helpers import draw_pose
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
os.environ["ZMQ"] = "1"
|
||||
|
||||
parser = argparse.ArgumentParser(description='Sniff a communcation socket')
|
||||
parser.add_argument('--addr', default='192.168.5.11')
|
||||
args = parser.parse_args()
|
||||
|
||||
messaging.context = messaging.Context()
|
||||
|
||||
poller = messaging.Poller()
|
||||
|
||||
m = 'driverMonitoring'
|
||||
sock = messaging.sub_sock(m, poller, addr=args.addr)
|
||||
|
||||
pygame.init()
|
||||
pygame.display.set_caption('livedm')
|
||||
screen = pygame.display.set_mode((320,640), pygame.DOUBLEBUF)
|
||||
camera_surface = pygame.surface.Surface((160,320), 0, 24).convert()
|
||||
|
||||
while 1:
|
||||
polld = poller.poll(1000)
|
||||
for sock in polld:
|
||||
msg = sock.receive()
|
||||
evt = log.Event.from_bytes(msg)
|
||||
|
||||
faceProb = np.array(evt.driverMonitoring.faceProb)
|
||||
faceOrientation = np.array(evt.driverMonitoring.faceOrientation)
|
||||
facePosition = np.array(evt.driverMonitoring.facePosition)
|
||||
|
||||
print(faceProb)
|
||||
# print(faceOrientation)
|
||||
# print(facePosition)
|
||||
faceOrientation[1] *= -1
|
||||
facePosition[0] *= -1
|
||||
|
||||
img = np.zeros((320,160,3))
|
||||
if faceProb > 0.4:
|
||||
cv2.putText(img, 'you', (int(facePosition[0]*160+40), int(facePosition[1]*320+110)), cv2.FONT_ITALIC, 0.5, (255,255,0))
|
||||
cv2.rectangle(img, (int(facePosition[0]*160+40), int(facePosition[1]*320+120)),\
|
||||
(int(facePosition[0]*160+120), int(facePosition[1]*320+200)), (255,255,0), 1)
|
||||
|
||||
not_blink = evt.driverMonitoring.leftBlinkProb + evt.driverMonitoring.rightBlinkProb < 1
|
||||
|
||||
if evt.driverMonitoring.leftEyeProb > 0.6:
|
||||
cv2.line(img, (int(facePosition[0]*160+95), int(facePosition[1]*320+140)),\
|
||||
(int(facePosition[0]*160+105), int(facePosition[1]*320+140)), (255,255,0), 2)
|
||||
if not_blink:
|
||||
cv2.line(img, (int(facePosition[0]*160+99), int(facePosition[1]*320+143)),\
|
||||
(int(facePosition[0]*160+101), int(facePosition[1]*320+143)), (255,255,0), 2)
|
||||
|
||||
if evt.driverMonitoring.rightEyeProb > 0.6:
|
||||
cv2.line(img, (int(facePosition[0]*160+55), int(facePosition[1]*320+140)),\
|
||||
(int(facePosition[0]*160+65), int(facePosition[1]*320+140)), (255,255,0), 2)
|
||||
if not_blink:
|
||||
cv2.line(img, (int(facePosition[0]*160+59), int(facePosition[1]*320+143)),\
|
||||
(int(facePosition[0]*160+61), int(facePosition[1]*320+143)), (255,255,0), 2)
|
||||
|
||||
else:
|
||||
cv2.putText(img, 'you not found', (int(facePosition[0]*160+40), int(facePosition[1]*320+110)), cv2.FONT_ITALIC, 0.5, (64,64,64))
|
||||
draw_pose(img, faceOrientation, facePosition,
|
||||
W = 160, H = 320, xyoffset = (0, 0), faceprob=faceProb)
|
||||
|
||||
pygame.surfarray.blit_array(camera_surface, img.swapaxes(0,1))
|
||||
camera_surface_2x = pygame.transform.scale2x(camera_surface)
|
||||
screen.blit(camera_surface_2x, (0, 0))
|
||||
pygame.display.flip()
|
||||
|
|
@ -0,0 +1,59 @@
|
|||
#!/usr/bin/env python
|
||||
import argparse
|
||||
import os
|
||||
import sys
|
||||
from common.basedir import BASEDIR
|
||||
from tools.lib.logreader import MultiLogIterator
|
||||
from tools.lib.route import Route
|
||||
|
||||
os.environ['BASEDIR'] = BASEDIR
|
||||
|
||||
|
||||
def get_arg_parser():
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Unlogging and save to file",
|
||||
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
|
||||
|
||||
parser.add_argument("data_dir", nargs='?',
|
||||
help="Path to directory in which log and camera files are located.")
|
||||
parser.add_argument("route_name", type=(lambda x: x.replace("#", "|")), nargs="?",
|
||||
help="The route whose messages will be published.")
|
||||
parser.add_argument("--out_path", nargs='?', default='/data/ubloxRaw.stream',
|
||||
help="Output pickle file path")
|
||||
return parser
|
||||
|
||||
|
||||
def main(argv):
|
||||
args = get_arg_parser().parse_args(sys.argv[1:])
|
||||
if not args.data_dir:
|
||||
print('Data directory invalid.')
|
||||
return
|
||||
|
||||
if not args.route_name:
|
||||
# Extract route name from path
|
||||
args.route_name = os.path.basename(args.data_dir)
|
||||
args.data_dir = os.path.dirname(args.data_dir)
|
||||
|
||||
route = Route(args.route_name, args.data_dir)
|
||||
lr = MultiLogIterator(route.log_paths(), wraparound=False)
|
||||
|
||||
with open(args.out_path, 'wb') as f:
|
||||
try:
|
||||
done = False
|
||||
i = 0
|
||||
while not done:
|
||||
msg = next(lr)
|
||||
if not msg:
|
||||
break
|
||||
smsg = msg.as_builder()
|
||||
typ = smsg.which()
|
||||
if typ == 'ubloxRaw':
|
||||
f.write(smsg.to_bytes())
|
||||
i += 1
|
||||
except StopIteration:
|
||||
print('All done')
|
||||
print('Writed {} msgs'.format(i))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
sys.exit(main(sys.argv[1:]))
|
||||
|
|
@ -0,0 +1,8 @@
|
|||
Makefile
|
||||
.*.swp
|
||||
*.o
|
||||
nui
|
||||
moc_*
|
||||
.qmake.stash
|
||||
nui.app/*
|
||||
|
||||
|
|
@ -0,0 +1,138 @@
|
|||
#include "FileReader.hpp"
|
||||
#include "FrameReader.hpp"
|
||||
|
||||
#include <QtNetwork>
|
||||
|
||||
FileReader::FileReader(const QString& file_) : file(file_) {
|
||||
}
|
||||
|
||||
void FileReader::process() {
|
||||
timer.start();
|
||||
// TODO: Support reading files from the API
|
||||
startRequest(QUrl("http://data.comma.life/"+file));
|
||||
}
|
||||
|
||||
void FileReader::startRequest(const QUrl &url) {
|
||||
qnam = new QNetworkAccessManager;
|
||||
reply = qnam->get(QNetworkRequest(url));
|
||||
connect(reply, &QNetworkReply::finished, this, &FileReader::httpFinished);
|
||||
connect(reply, &QIODevice::readyRead, this, &FileReader::readyRead);
|
||||
qDebug() << "requesting" << url;
|
||||
}
|
||||
|
||||
void FileReader::httpFinished() {
|
||||
if (reply->error()) {
|
||||
qWarning() << reply->errorString();
|
||||
}
|
||||
|
||||
const QVariant redirectionTarget = reply->attribute(QNetworkRequest::RedirectionTargetAttribute);
|
||||
if (!redirectionTarget.isNull()) {
|
||||
const QUrl redirectedUrl = redirectionTarget.toUrl();
|
||||
//qDebug() << "redirected to" << redirectedUrl;
|
||||
startRequest(redirectedUrl);
|
||||
} else {
|
||||
qDebug() << "done in" << timer.elapsed() << "ms";
|
||||
done();
|
||||
}
|
||||
}
|
||||
|
||||
void FileReader::readyRead() {
|
||||
QByteArray dat = reply->readAll();
|
||||
printf("got http ready read: %d\n", dat.size());
|
||||
}
|
||||
|
||||
FileReader::~FileReader() {
|
||||
|
||||
}
|
||||
|
||||
LogReader::LogReader(const QString& file, Events *events_, QReadWriteLock* events_lock_, QMap<int, QPair<int, int> > *eidx_) :
|
||||
FileReader(file), events(events_), events_lock(events_lock_), eidx(eidx_) {
|
||||
bStream.next_in = NULL;
|
||||
bStream.avail_in = 0;
|
||||
bStream.bzalloc = NULL;
|
||||
bStream.bzfree = NULL;
|
||||
bStream.opaque = NULL;
|
||||
|
||||
int ret = BZ2_bzDecompressInit(&bStream, 0, 0);
|
||||
if (ret != BZ_OK) qWarning() << "bz2 init failed";
|
||||
|
||||
// start with 64MB buffer
|
||||
raw.resize(1024*1024*64);
|
||||
|
||||
// auto increment?
|
||||
bStream.next_out = raw.data();
|
||||
bStream.avail_out = raw.size();
|
||||
|
||||
// parsed no events yet
|
||||
event_offset = 0;
|
||||
|
||||
parser = new std::thread([&]() {
|
||||
while (1) {
|
||||
mergeEvents(cdled.get());
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
void LogReader::mergeEvents(int dled) {
|
||||
auto amsg = kj::arrayPtr((const capnp::word*)(raw.data() + event_offset), (dled-event_offset)/sizeof(capnp::word));
|
||||
Events events_local;
|
||||
QMap<int, QPair<int, int> > eidx_local;
|
||||
|
||||
while (amsg.size() > 0) {
|
||||
try {
|
||||
capnp::FlatArrayMessageReader cmsg = capnp::FlatArrayMessageReader(amsg);
|
||||
|
||||
// this needed? it is
|
||||
capnp::FlatArrayMessageReader *tmsg =
|
||||
new capnp::FlatArrayMessageReader(kj::arrayPtr(amsg.begin(), cmsg.getEnd()));
|
||||
|
||||
amsg = kj::arrayPtr(cmsg.getEnd(), amsg.end());
|
||||
|
||||
cereal::Event::Reader event = tmsg->getRoot<cereal::Event>();
|
||||
events_local.insert(event.getLogMonoTime(), event);
|
||||
|
||||
// hack
|
||||
// TODO: rewrite with callback
|
||||
if (event.which() == cereal::Event::ENCODE_IDX) {
|
||||
auto ee = event.getEncodeIdx();
|
||||
eidx_local.insert(ee.getFrameId(), qMakePair(ee.getSegmentNum(), ee.getSegmentId()));
|
||||
}
|
||||
|
||||
// increment
|
||||
event_offset = (char*)cmsg.getEnd() - raw.data();
|
||||
} catch (const kj::Exception& e) {
|
||||
// partial messages trigger this
|
||||
//qDebug() << e.getDescription().cStr();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// merge in events
|
||||
// TODO: add lock
|
||||
events_lock->lockForWrite();
|
||||
*events += events_local;
|
||||
eidx->unite(eidx_local);
|
||||
events_lock->unlock();
|
||||
|
||||
printf("parsed %d into %d events with offset %d\n", dled, events->size(), event_offset);
|
||||
}
|
||||
|
||||
void LogReader::readyRead() {
|
||||
QByteArray dat = reply->readAll();
|
||||
|
||||
bStream.next_in = dat.data();
|
||||
bStream.avail_in = dat.size();
|
||||
|
||||
while (bStream.avail_in > 0) {
|
||||
int ret = BZ2_bzDecompress(&bStream);
|
||||
if (ret != BZ_OK && ret != BZ_STREAM_END) {
|
||||
qWarning() << "bz2 decompress failed";
|
||||
break;
|
||||
}
|
||||
qDebug() << "got" << dat.size() << "with" << bStream.avail_out << "size" << raw.size();
|
||||
}
|
||||
|
||||
int dled = raw.size() - bStream.avail_out;
|
||||
cdled.put(dled);
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,68 @@
|
|||
#ifndef FILEREADER_HPP
|
||||
#define FILEREADER_HPP
|
||||
|
||||
#include <QString>
|
||||
#include <QNetworkAccessManager>
|
||||
#include <QWidget>
|
||||
#include <QVector>
|
||||
#include <QMultiMap>
|
||||
#include <QElapsedTimer>
|
||||
#include <QReadWriteLock>
|
||||
|
||||
#include <bzlib.h>
|
||||
|
||||
#include <kj/io.h>
|
||||
#include <capnp/serialize.h>
|
||||
|
||||
#include "cereal/gen/cpp/log.capnp.h"
|
||||
|
||||
#include <thread>
|
||||
#include "channel.hpp"
|
||||
|
||||
class FileReader : public QObject {
|
||||
Q_OBJECT
|
||||
public:
|
||||
FileReader(const QString& file_);
|
||||
void startRequest(const QUrl &url);
|
||||
~FileReader();
|
||||
virtual void readyRead();
|
||||
void httpFinished();
|
||||
virtual void done() {};
|
||||
public slots:
|
||||
void process();
|
||||
protected:
|
||||
QNetworkReply *reply;
|
||||
private:
|
||||
QNetworkAccessManager *qnam;
|
||||
QElapsedTimer timer;
|
||||
QString file;
|
||||
};
|
||||
|
||||
typedef QMultiMap<uint64_t, cereal::Event::Reader> Events;
|
||||
|
||||
class LogReader : public FileReader {
|
||||
Q_OBJECT
|
||||
public:
|
||||
LogReader(const QString& file, Events *, QReadWriteLock* events_lock_, QMap<int, QPair<int, int> > *eidx_);
|
||||
void readyRead();
|
||||
void done() { is_done = true; };
|
||||
bool is_done = false;
|
||||
private:
|
||||
bz_stream bStream;
|
||||
|
||||
// backing store
|
||||
QByteArray raw;
|
||||
|
||||
std::thread *parser;
|
||||
int event_offset;
|
||||
channel<int> cdled;
|
||||
|
||||
// global
|
||||
void mergeEvents(int dled);
|
||||
Events *events;
|
||||
QReadWriteLock* events_lock;
|
||||
QMap<int, QPair<int, int> > *eidx;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
|
|
@ -0,0 +1,9 @@
|
|||
== Ubuntu ==
|
||||
|
||||
sudo apt-get install capnproto libyaml-cpp-dev qt5-default
|
||||
|
||||
== Mac ==
|
||||
|
||||
brew install qt5 ffmpeg capnp yaml-cpp zmq
|
||||
brew link qt5 --force
|
||||
|
||||
|
|
@ -0,0 +1,182 @@
|
|||
#include <string>
|
||||
#include <vector>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
#include <capnp/dynamic.h>
|
||||
#include <capnp/schema.h>
|
||||
|
||||
// include the dynamic struct
|
||||
#include "cereal/gen/cpp/car.capnp.c++"
|
||||
#include "cereal/gen/cpp/log.capnp.c++"
|
||||
|
||||
#include "Unlogger.hpp"
|
||||
|
||||
#include <stdint.h>
|
||||
#include <time.h>
|
||||
|
||||
static inline uint64_t nanos_since_boot() {
|
||||
struct timespec t;
|
||||
clock_gettime(CLOCK_BOOTTIME, &t);
|
||||
return t.tv_sec * 1000000000ULL + t.tv_nsec;
|
||||
}
|
||||
|
||||
|
||||
Unlogger::Unlogger(Events *events_, QReadWriteLock* events_lock_, QMap<int, FrameReader*> *frs_, int seek)
|
||||
: events(events_), events_lock(events_lock_), frs(frs_) {
|
||||
ctx = Context::create();
|
||||
YAML::Node service_list = YAML::LoadFile("../../cereal/service_list.yaml");
|
||||
|
||||
seek_request = seek*1e9;
|
||||
|
||||
QStringList block = QString(getenv("BLOCK")).split(",");
|
||||
qDebug() << "blocklist" << block;
|
||||
|
||||
QStringList allow = QString(getenv("ALLOW")).split(",");
|
||||
qDebug() << "allowlist" << allow;
|
||||
|
||||
for (const auto& it : service_list) {
|
||||
auto name = it.first.as<std::string>();
|
||||
|
||||
if (allow[0].size() > 0 && !allow.contains(name.c_str())) {
|
||||
qDebug() << "not allowing" << name.c_str();
|
||||
continue;
|
||||
}
|
||||
|
||||
if (block.contains(name.c_str())) {
|
||||
qDebug() << "blocking" << name.c_str();
|
||||
continue;
|
||||
}
|
||||
|
||||
PubSocket *sock = PubSocket::create(ctx, name);
|
||||
if (sock == NULL) {
|
||||
qDebug() << "FAILED" << name.c_str();
|
||||
continue;
|
||||
}
|
||||
|
||||
qDebug() << name.c_str();
|
||||
|
||||
for (auto field: capnp::Schema::from<cereal::Event>().getFields()) {
|
||||
std::string tname = field.getProto().getName();
|
||||
|
||||
if (tname == name) {
|
||||
// TODO: I couldn't figure out how to get the which, only the index, hence this hack
|
||||
int type = field.getIndex();
|
||||
if (type > 67) type--; // valid
|
||||
type--; // logMonoTime
|
||||
|
||||
//qDebug() << "here" << tname.c_str() << type << cereal::Event::CONTROLS_STATE;
|
||||
socks.insert(type, sock);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Unlogger::process() {
|
||||
qDebug() << "hello from unlogger thread";
|
||||
while (events->size() == 0) {
|
||||
qDebug() << "waiting for events";
|
||||
QThread::sleep(1);
|
||||
}
|
||||
qDebug() << "got events";
|
||||
|
||||
// TODO: hack
|
||||
if (seek_request != 0) {
|
||||
seek_request += events->begin().key();
|
||||
while (events->lowerBound(seek_request) == events->end()) {
|
||||
qDebug() << "waiting for desired time";
|
||||
QThread::sleep(1);
|
||||
}
|
||||
}
|
||||
|
||||
QElapsedTimer timer;
|
||||
timer.start();
|
||||
|
||||
uint64_t last_elapsed = 0;
|
||||
|
||||
// loops
|
||||
while (1) {
|
||||
uint64_t t0 = (events->begin()+1).key();
|
||||
uint64_t t0r = timer.nsecsElapsed();
|
||||
qDebug() << "unlogging at" << t0;
|
||||
|
||||
auto eit = events->lowerBound(t0);
|
||||
while (eit != events->end()) {
|
||||
while (paused) {
|
||||
QThread::usleep(1000);
|
||||
t0 = eit->getLogMonoTime();
|
||||
t0r = timer.nsecsElapsed();
|
||||
}
|
||||
|
||||
if (seek_request != 0) {
|
||||
t0 = seek_request;
|
||||
qDebug() << "seeking to" << t0;
|
||||
t0r = timer.nsecsElapsed();
|
||||
eit = events->lowerBound(t0);
|
||||
seek_request = 0;
|
||||
if (eit == events->end()) {
|
||||
qWarning() << "seek off end";
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (abs(((long long)tc-(long long)last_elapsed)) > 50e6) {
|
||||
//qDebug() << "elapsed";
|
||||
emit elapsed();
|
||||
last_elapsed = tc;
|
||||
}
|
||||
|
||||
auto e = *eit;
|
||||
auto type = e.which();
|
||||
uint64_t tm = e.getLogMonoTime();
|
||||
auto it = socks.find(type);
|
||||
tc = tm;
|
||||
if (it != socks.end()) {
|
||||
long etime = tm-t0;
|
||||
long rtime = timer.nsecsElapsed() - t0r;
|
||||
long us_behind = ((etime-rtime)*1e-3)+0.5;
|
||||
if (us_behind > 0) {
|
||||
if (us_behind > 1e6) {
|
||||
qWarning() << "OVER ONE SECOND BEHIND, HACKING" << us_behind;
|
||||
us_behind = 0;
|
||||
t0 = tm;
|
||||
t0r = timer.nsecsElapsed();
|
||||
}
|
||||
QThread::usleep(us_behind);
|
||||
//qDebug() << "sleeping" << us_behind << etime << timer.nsecsElapsed();
|
||||
}
|
||||
|
||||
capnp::MallocMessageBuilder msg;
|
||||
msg.setRoot(e);
|
||||
|
||||
auto ee = msg.getRoot<cereal::Event>();
|
||||
ee.setLogMonoTime(nanos_since_boot());
|
||||
|
||||
if (e.which() == cereal::Event::FRAME) {
|
||||
auto fr = msg.getRoot<cereal::Event>().getFrame();
|
||||
|
||||
// TODO: better way?
|
||||
auto it = eidx.find(fr.getFrameId());
|
||||
if (it != eidx.end()) {
|
||||
auto pp = *it;
|
||||
//qDebug() << fr.getFrameId() << pp;
|
||||
|
||||
if (frs->find(pp.first) != frs->end()) {
|
||||
auto frm = (*frs)[pp.first];
|
||||
auto data = frm->get(pp.second);
|
||||
if (data != NULL) {
|
||||
fr.setImage(kj::arrayPtr(data, frm->getRGBSize()));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
auto words = capnp::messageToFlatArray(msg);
|
||||
auto bytes = words.asBytes();
|
||||
|
||||
// TODO: Can PubSocket take a const char?
|
||||
(*it)->send((char*)bytes.begin(), bytes.size());
|
||||
}
|
||||
++eit;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,36 @@
|
|||
#ifndef UNLOGGER_HPP
|
||||
#define UNLOGGER_HPP
|
||||
|
||||
#include <QThread>
|
||||
#include <QReadWriteLock>
|
||||
#include "messaging.hpp"
|
||||
#include "FileReader.hpp"
|
||||
#include "FrameReader.hpp"
|
||||
|
||||
class Unlogger : public QObject {
|
||||
Q_OBJECT
|
||||
public:
|
||||
Unlogger(Events *events_, QReadWriteLock* events_lock_, QMap<int, FrameReader*> *frs_, int seek);
|
||||
uint64_t getCurrentTime() { return tc; }
|
||||
void setSeekRequest(uint64_t seek_request_) { seek_request = seek_request_; }
|
||||
void setPause(bool pause) { paused = pause; }
|
||||
void togglePause() { paused = !paused; }
|
||||
QMap<int, QPair<int, int> > eidx;
|
||||
public slots:
|
||||
void process();
|
||||
signals:
|
||||
void elapsed();
|
||||
void finished();
|
||||
private:
|
||||
Events *events;
|
||||
QReadWriteLock *events_lock;
|
||||
QMap<int, FrameReader*> *frs;
|
||||
QMap<int, PubSocket*> socks;
|
||||
Context *ctx;
|
||||
uint64_t tc = 0;
|
||||
uint64_t seek_request = 0;
|
||||
bool paused = false;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
|
|
@ -0,0 +1,4 @@
|
|||
#!/bin/bash
|
||||
qmake
|
||||
make -j
|
||||
|
||||
|
|
@ -0,0 +1,216 @@
|
|||
#include <QApplication>
|
||||
#include <QWidget>
|
||||
#include <QString>
|
||||
#include <QTimer>
|
||||
#include <QPushButton>
|
||||
#include <QGraphicsScene>
|
||||
#include <QPainter>
|
||||
#include <QThread>
|
||||
#include <QMouseEvent>
|
||||
#include <QReadWriteLock>
|
||||
#include <QLineEdit>
|
||||
|
||||
#include "FileReader.hpp"
|
||||
#include "Unlogger.hpp"
|
||||
#include "FrameReader.hpp"
|
||||
|
||||
class Window : public QWidget {
|
||||
public:
|
||||
Window(QString route_, int seek);
|
||||
bool addSegment(int i);
|
||||
protected:
|
||||
void keyPressEvent(QKeyEvent *event) override;
|
||||
void mousePressEvent(QMouseEvent *event) override;
|
||||
void paintEvent(QPaintEvent *event) override;
|
||||
uint64_t ct;
|
||||
Unlogger *unlogger;
|
||||
private:
|
||||
int timeToPixel(uint64_t ns);
|
||||
uint64_t pixelToTime(int px);
|
||||
QString route;
|
||||
|
||||
QReadWriteLock events_lock;
|
||||
Events events;
|
||||
int last_event_size = 0;
|
||||
|
||||
QMap<int, LogReader*> lrs;
|
||||
QMap<int, FrameReader*> frs;
|
||||
|
||||
// cache the bar
|
||||
QPixmap *px = NULL;
|
||||
int seg_add = 0;
|
||||
|
||||
QLineEdit *timeLE;
|
||||
};
|
||||
|
||||
Window::Window(QString route_, int seek) : route(route_) {
|
||||
timeLE = new QLineEdit(this);
|
||||
timeLE->setPlaceholderText("Placeholder Text");
|
||||
timeLE->move(50, 650);
|
||||
|
||||
QThread* thread = new QThread;
|
||||
unlogger = new Unlogger(&events, &events_lock, &frs, seek);
|
||||
unlogger->moveToThread(thread);
|
||||
connect(thread, SIGNAL (started()), unlogger, SLOT (process()));
|
||||
connect(unlogger, SIGNAL (elapsed()), this, SLOT (update()));
|
||||
thread->start();
|
||||
|
||||
this->setFocusPolicy(Qt::StrongFocus);
|
||||
|
||||
// add the first segment
|
||||
addSegment(seek/60);
|
||||
}
|
||||
|
||||
bool Window::addSegment(int i) {
|
||||
if (lrs.find(i) == lrs.end()) {
|
||||
QString fn = QString("%1/%2/rlog.bz2").arg(route).arg(i);
|
||||
|
||||
QThread* thread = new QThread;
|
||||
lrs.insert(i, new LogReader(fn, &events, &events_lock, &unlogger->eidx));
|
||||
lrs[i]->moveToThread(thread);
|
||||
connect(thread, SIGNAL (started()), lrs[i], SLOT (process()));
|
||||
thread->start();
|
||||
//connect(lrs[i], SIGNAL (finished()), this, SLOT (update()));
|
||||
|
||||
QString frn = QString("%1/%2/fcamera.hevc").arg(route).arg(i);
|
||||
frs.insert(i, new FrameReader(qPrintable(frn)));
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
#define PIXELS_PER_SEC 0.5
|
||||
|
||||
int Window::timeToPixel(uint64_t ns) {
|
||||
// TODO: make this dynamic
|
||||
return int(ns*1e-9*PIXELS_PER_SEC+0.5);
|
||||
}
|
||||
|
||||
uint64_t Window::pixelToTime(int px) {
|
||||
// TODO: make this dynamic
|
||||
//printf("%d\n", px);
|
||||
return ((px+0.5)/PIXELS_PER_SEC) * 1e9;
|
||||
}
|
||||
|
||||
void Window::keyPressEvent(QKeyEvent *event) {
|
||||
printf("keypress: %x\n", event->key());
|
||||
if (event->key() == Qt::Key_Space) unlogger->togglePause();
|
||||
}
|
||||
|
||||
void Window::mousePressEvent(QMouseEvent *event) {
|
||||
//printf("mouse event\n");
|
||||
if (event->button() == Qt::LeftButton) {
|
||||
uint64_t t0 = events.begin().key();
|
||||
uint64_t tt = pixelToTime(event->x());
|
||||
int seg = int((tt*1e-9)/60);
|
||||
printf("segment %d\n", seg);
|
||||
addSegment(seg);
|
||||
|
||||
//printf("seek to %lu\n", t0+tt);
|
||||
unlogger->setSeekRequest(t0+tt);
|
||||
}
|
||||
this->update();
|
||||
}
|
||||
|
||||
void Window::paintEvent(QPaintEvent *event) {
|
||||
if (events.size() == 0) return;
|
||||
|
||||
QElapsedTimer timer;
|
||||
timer.start();
|
||||
|
||||
uint64_t t0 = events.begin().key();
|
||||
uint64_t t1 = (events.end()-1).key();
|
||||
|
||||
//p.drawRect(0, 0, 600, 100);
|
||||
|
||||
// TODO: we really don't have to redraw this every time, only on updates to events
|
||||
int this_event_size = events.size();
|
||||
if (last_event_size != this_event_size) {
|
||||
if (px != NULL) delete px;
|
||||
px = new QPixmap(1920, 600);
|
||||
px->fill(QColor(0xd8, 0xd8, 0xd8));
|
||||
|
||||
QPainter tt(px);
|
||||
tt.setBrush(Qt::cyan);
|
||||
|
||||
int lt = -1;
|
||||
int lvv = 0;
|
||||
for (auto e : events) {
|
||||
auto type = e.which();
|
||||
//printf("%lld %d\n", e.getLogMonoTime()-t0, type);
|
||||
if (type == cereal::Event::CONTROLS_STATE) {
|
||||
auto controlsState = e.getControlsState();
|
||||
uint64_t t = (e.getLogMonoTime()-t0);
|
||||
float vEgo = controlsState.getVEgo();
|
||||
int enabled = controlsState.getState() == cereal::ControlsState::OpenpilotState::ENABLED;
|
||||
int rt = timeToPixel(t); // 250 ms per pixel
|
||||
if (rt != lt) {
|
||||
int vv = vEgo*8.0;
|
||||
if (lt != -1) {
|
||||
tt.setPen(Qt::red);
|
||||
tt.drawLine(lt, 300-lvv, rt, 300-vv);
|
||||
|
||||
if (enabled) {
|
||||
tt.setPen(Qt::green);
|
||||
} else {
|
||||
tt.setPen(Qt::blue);
|
||||
}
|
||||
|
||||
tt.drawLine(rt, 300, rt, 600);
|
||||
}
|
||||
lt = rt;
|
||||
lvv = vv;
|
||||
}
|
||||
}
|
||||
}
|
||||
tt.end();
|
||||
last_event_size = this_event_size;
|
||||
if (lrs.find(seg_add) != lrs.end() && lrs[seg_add]->is_done) {
|
||||
while (!addSegment(++seg_add));
|
||||
}
|
||||
}
|
||||
|
||||
QPainter p(this);
|
||||
if (px != NULL) p.drawPixmap(0, 0, 1920, 600, *px);
|
||||
|
||||
p.setBrush(Qt::cyan);
|
||||
|
||||
uint64_t ct = unlogger->getCurrentTime();
|
||||
if (ct != 0) {
|
||||
addSegment((((ct-t0)*1e-9)/60)+1);
|
||||
int rrt = timeToPixel(ct-t0);
|
||||
p.drawRect(rrt-1, 0, 2, 600);
|
||||
|
||||
timeLE->setText(QString("%1").arg((ct-t0)*1e-9, '8', 'f', 2));
|
||||
}
|
||||
|
||||
p.end();
|
||||
|
||||
if (timer.elapsed() > 50) {
|
||||
qDebug() << "paint in" << timer.elapsed() << "ms";
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[]) {
|
||||
QApplication app(argc, argv);
|
||||
|
||||
QString route(argv[1]);
|
||||
int seek = QString(argv[2]).toInt();
|
||||
printf("seek: %d\n", seek);
|
||||
route = route.replace("|", "/");
|
||||
if (route == "") {
|
||||
printf("usage %s: <route>\n", argv[0]);
|
||||
exit(0);
|
||||
//route = "3a5d6ac1c23e5536/2019-10-29--10-06-58";
|
||||
//route = "0006c839f32a6f99/2019-02-18--06-21-29";
|
||||
//route = "02ec6bea180a4d36/2019-10-25--10-18-09";
|
||||
}
|
||||
|
||||
Window window(route, seek);
|
||||
window.resize(1920, 800);
|
||||
window.setWindowTitle("nui unlogger");
|
||||
window.show();
|
||||
|
||||
return app.exec();
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,37 @@
|
|||
######################################################################
|
||||
# Automatically generated by qmake (3.0) Tue Oct 29 11:15:25 2019
|
||||
######################################################################
|
||||
|
||||
TEMPLATE = app
|
||||
TARGET = nui
|
||||
INCLUDEPATH += .
|
||||
|
||||
# Input
|
||||
SOURCES += main.cpp FileReader.cpp Unlogger.cpp ../clib/FrameReader.cpp
|
||||
HEADERS = FileReader.hpp Unlogger.hpp ../clib/FrameReader.hpp
|
||||
|
||||
CONFIG += c++14
|
||||
CONFIG += debug
|
||||
|
||||
QT += widgets network core
|
||||
|
||||
BASEDIR = "../../"
|
||||
PHONELIBS = $$BASEDIR"phonelibs"
|
||||
|
||||
INCLUDEPATH = $$PHONELIBS/capnp-cpp/include $$PHONELIBS/yaml-cpp/include ../clib/
|
||||
|
||||
unix:!macx {
|
||||
LIBS += -L$$PHONELIBS/capnp-cpp/x64/lib -L$$PHONELIBS/yaml-cpp/x64/lib -Wl,-rpath=$$PHONELIBS/capnp-cpp/x64/lib
|
||||
}
|
||||
|
||||
macx: {
|
||||
LIBS += -L$$PHONELIBS/capnp-cpp/mac/lib -L$$PHONELIBS/yaml-cpp/mac/lib
|
||||
}
|
||||
|
||||
LIBS += -lcapnp -lkj -lyaml-cpp
|
||||
|
||||
INCLUDEPATH += /usr/local/include
|
||||
INCLUDEPATH += $$PHONELIBS/capnp-cpp/include $$BASEDIR $$BASEDIR/cereal/messaging $$PHONELIBS/yaml-cpp/include
|
||||
LIBS += -L/usr/local/lib -lavformat -lavcodec -lavutil -lswscale
|
||||
LIBS += -lbz2 $$BASEDIR/cereal/libmessaging.a -lzmq
|
||||
|
||||
|
|
@ -0,0 +1 @@
|
|||
test
|
||||
|
|
@ -0,0 +1,14 @@
|
|||
#include "FrameReader.hpp"
|
||||
#include "TestFrameReader.hpp"
|
||||
|
||||
void TestFrameReader::frameread() {
|
||||
QElapsedTimer t;
|
||||
t.start();
|
||||
FrameReader fr("3a5d6ac1c23e5536/2019-10-29--10-06-58/2/fcamera.hevc");
|
||||
fr.get(2);
|
||||
//QThread::sleep(10);
|
||||
qDebug() << t.nsecsElapsed()*1e-9 << "seconds";
|
||||
}
|
||||
|
||||
QTEST_MAIN(TestFrameReader)
|
||||
|
||||
|
|
@ -0,0 +1,8 @@
|
|||
#include <QtTest/QtTest>
|
||||
|
||||
class TestFrameReader : public QObject {
|
||||
Q_OBJECT
|
||||
private slots:
|
||||
void frameread();
|
||||
};
|
||||
|
||||
|
|
@ -0,0 +1,16 @@
|
|||
######################################################################
|
||||
# Automatically generated by qmake (3.0) Thu Oct 31 16:05:48 2019
|
||||
######################################################################
|
||||
|
||||
QT += testlib
|
||||
TEMPLATE = app
|
||||
TARGET = test
|
||||
INCLUDEPATH += . ../
|
||||
|
||||
# Input
|
||||
SOURCES += TestFrameReader.cpp ../FrameReader.cpp
|
||||
HEADERS = TestFrameReader.hpp ../FrameReader.hpp
|
||||
|
||||
CONFIG += c++14
|
||||
|
||||
LIBS += -lavformat -lavcodec -lavutil -lswscale
|
||||
|
|
@ -0,0 +1,112 @@
|
|||
#!/usr/bin/env python
|
||||
import os
|
||||
|
||||
from common.basedir import BASEDIR
|
||||
os.environ['BASEDIR'] = BASEDIR
|
||||
SCALE = 3
|
||||
|
||||
import argparse
|
||||
import zmq
|
||||
import pygame
|
||||
import numpy as np
|
||||
import cv2
|
||||
import sys
|
||||
import traceback
|
||||
from collections import namedtuple
|
||||
from cereal import car
|
||||
from common.params import Params
|
||||
from tools.lib.lazy_property import lazy_property
|
||||
from cereal.messaging import sub_sock, recv_one_or_none, recv_one
|
||||
from cereal.services import service_list
|
||||
|
||||
_BB_OFFSET = 0, 0
|
||||
_BB_TO_FULL_FRAME = np.asarray([[1., 0., _BB_OFFSET[0]], [0., 1., _BB_OFFSET[1]],
|
||||
[0., 0., 1.]])
|
||||
_FULL_FRAME_TO_BB = np.linalg.inv(_BB_TO_FULL_FRAME)
|
||||
_FULL_FRAME_SIZE = 1164, 874
|
||||
|
||||
|
||||
|
||||
def pygame_modules_have_loaded():
|
||||
return pygame.display.get_init() and pygame.font.get_init()
|
||||
|
||||
|
||||
def ui_thread(addr, frame_address):
|
||||
context = zmq.Context.instance()
|
||||
|
||||
pygame.init()
|
||||
pygame.font.init()
|
||||
assert pygame_modules_have_loaded()
|
||||
|
||||
size = (_FULL_FRAME_SIZE[0] * SCALE, _FULL_FRAME_SIZE[1] * SCALE)
|
||||
pygame.display.set_caption("comma one debug UI")
|
||||
screen = pygame.display.set_mode(size, pygame.DOUBLEBUF)
|
||||
|
||||
camera_surface = pygame.surface.Surface((_FULL_FRAME_SIZE[0] * SCALE, _FULL_FRAME_SIZE[1] * SCALE), 0, 24).convert()
|
||||
|
||||
frame = context.socket(zmq.SUB)
|
||||
frame.connect(frame_address or "tcp://%s:%d" % (addr, service_list['frame'].port))
|
||||
frame.setsockopt(zmq.SUBSCRIBE, "")
|
||||
|
||||
img = np.zeros((_FULL_FRAME_SIZE[1], _FULL_FRAME_SIZE[0], 3), dtype='uint8')
|
||||
imgff = np.zeros((_FULL_FRAME_SIZE[1], _FULL_FRAME_SIZE[0], 3), dtype=np.uint8)
|
||||
|
||||
while 1:
|
||||
list(pygame.event.get())
|
||||
screen.fill((64, 64, 64))
|
||||
|
||||
# ***** frame *****
|
||||
fpkt = recv_one(frame)
|
||||
yuv_img = fpkt.frame.image
|
||||
|
||||
if fpkt.frame.transform:
|
||||
yuv_transform = np.array(fpkt.frame.transform).reshape(3, 3)
|
||||
else:
|
||||
# assume frame is flipped
|
||||
yuv_transform = np.array([[-1.0, 0.0, _FULL_FRAME_SIZE[0] - 1],
|
||||
[0.0, -1.0, _FULL_FRAME_SIZE[1] - 1], [0.0, 0.0, 1.0]])
|
||||
|
||||
if yuv_img and len(yuv_img) == _FULL_FRAME_SIZE[0] * _FULL_FRAME_SIZE[1] * 3 // 2:
|
||||
yuv_np = np.frombuffer(
|
||||
yuv_img, dtype=np.uint8).reshape(_FULL_FRAME_SIZE[1] * 3 // 2, -1)
|
||||
cv2.cvtColor(yuv_np, cv2.COLOR_YUV2RGB_I420, dst=imgff)
|
||||
cv2.warpAffine(
|
||||
imgff,
|
||||
np.dot(yuv_transform, _BB_TO_FULL_FRAME)[:2], (img.shape[1], img.shape[0]),
|
||||
dst=img,
|
||||
flags=cv2.WARP_INVERSE_MAP)
|
||||
else:
|
||||
img.fill(0)
|
||||
|
||||
height, width = img.shape[:2]
|
||||
img_resized = cv2.resize(
|
||||
img, (SCALE * width, SCALE * height), interpolation=cv2.INTER_CUBIC)
|
||||
# *** blits ***
|
||||
pygame.surfarray.blit_array(camera_surface, img_resized.swapaxes(0, 1))
|
||||
screen.blit(camera_surface, (0, 0))
|
||||
|
||||
# this takes time...vsync or something
|
||||
pygame.display.flip()
|
||||
|
||||
|
||||
def get_arg_parser():
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Show replay data in a UI.",
|
||||
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
|
||||
|
||||
parser.add_argument(
|
||||
"ip_address",
|
||||
nargs="?",
|
||||
default="127.0.0.1",
|
||||
help="The ip address on which to receive zmq messages.")
|
||||
|
||||
parser.add_argument(
|
||||
"--frame-address",
|
||||
default=None,
|
||||
help="The ip address on which to receive zmq messages.")
|
||||
return parser
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
args = get_arg_parser().parse_args(sys.argv[1:])
|
||||
ui_thread(args.ip_address, args.frame_address)
|
||||
|
|
@ -0,0 +1,314 @@
|
|||
import platform
|
||||
from collections import namedtuple
|
||||
|
||||
import matplotlib
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
import pygame
|
||||
|
||||
from tools.lib.lazy_property import lazy_property
|
||||
from selfdrive.config import UIParams as UP
|
||||
from selfdrive.config import RADAR_TO_CAMERA
|
||||
from selfdrive.controls.lib.lane_planner import (compute_path_pinv,
|
||||
model_polyfit)
|
||||
|
||||
RED = (255, 0, 0)
|
||||
GREEN = (0, 255, 0)
|
||||
BLUE = (0, 0, 255)
|
||||
YELLOW = (255, 255, 0)
|
||||
BLACK = (0, 0, 0)
|
||||
WHITE = (255, 255, 255)
|
||||
|
||||
_PATH_X = np.arange(192.)
|
||||
_PATH_XD = np.arange(192.)
|
||||
_PATH_PINV = compute_path_pinv(50)
|
||||
#_BB_OFFSET = 290, 332
|
||||
_BB_OFFSET = 0,0
|
||||
_BB_SCALE = 1164/640.
|
||||
_BB_TO_FULL_FRAME = np.asarray([
|
||||
[_BB_SCALE, 0., _BB_OFFSET[0]],
|
||||
[0., _BB_SCALE, _BB_OFFSET[1]],
|
||||
[0., 0., 1.]])
|
||||
_FULL_FRAME_TO_BB = np.linalg.inv(_BB_TO_FULL_FRAME)
|
||||
|
||||
METER_WIDTH = 20
|
||||
|
||||
ModelUIData = namedtuple("ModelUIData", ["cpath", "lpath", "rpath", "lead", "lead_future"])
|
||||
|
||||
_COLOR_CACHE = {}
|
||||
def find_color(lidar_surface, color):
|
||||
if color in _COLOR_CACHE:
|
||||
return _COLOR_CACHE[color]
|
||||
tcolor = 0
|
||||
ret = 255
|
||||
for x in lidar_surface.get_palette():
|
||||
#print tcolor, x
|
||||
if x[0:3] == color:
|
||||
ret = tcolor
|
||||
break
|
||||
tcolor += 1
|
||||
_COLOR_CACHE[color] = ret
|
||||
return ret
|
||||
|
||||
def warp_points(pt_s, warp_matrix):
|
||||
# pt_s are the source points, nxm array.
|
||||
pt_d = np.dot(warp_matrix[:, :-1], pt_s.T) + warp_matrix[:, -1, None]
|
||||
|
||||
# Divide by last dimension for representation in image space.
|
||||
return (pt_d[:-1, :] / pt_d[-1, :]).T
|
||||
|
||||
def to_lid_pt(y, x):
|
||||
px, py = -x * UP.lidar_zoom + UP.lidar_car_x, -y * UP.lidar_zoom + UP.lidar_car_y
|
||||
if px > 0 and py > 0 and px < UP.lidar_x and py < UP.lidar_y:
|
||||
return int(px), int(py)
|
||||
return -1, -1
|
||||
|
||||
|
||||
def draw_path(y, x, color, img, calibration, top_down, lid_color=None):
|
||||
# TODO: Remove big box.
|
||||
uv_model_real = warp_points(np.column_stack((x, y)), calibration.car_to_model)
|
||||
uv_model = np.round(uv_model_real).astype(int)
|
||||
|
||||
uv_model_dots = uv_model[np.logical_and.reduce((np.all( # pylint: disable=no-member
|
||||
uv_model > 0, axis=1), uv_model[:, 0] < img.shape[1] - 1, uv_model[:, 1] <
|
||||
img.shape[0] - 1))]
|
||||
|
||||
for i, j in ((-1, 0), (0, -1), (0, 0), (0, 1), (1, 0)):
|
||||
img[uv_model_dots[:, 1] + i, uv_model_dots[:, 0] + j] = color
|
||||
|
||||
# draw lidar path point on lidar
|
||||
# find color in 8 bit
|
||||
if lid_color is not None and top_down is not None:
|
||||
tcolor = find_color(top_down[0], lid_color)
|
||||
for i in range(len(x)):
|
||||
px, py = to_lid_pt(x[i], y[i])
|
||||
if px != -1:
|
||||
top_down[1][px, py] = tcolor
|
||||
|
||||
def draw_steer_path(speed_ms, curvature, color, img,
|
||||
calibration, top_down, VM, lid_color=None):
|
||||
path_x = np.arange(101.)
|
||||
path_y = np.multiply(path_x, np.tan(np.arcsin(np.clip(path_x * curvature, -0.999, 0.999)) / 2.))
|
||||
|
||||
draw_path(path_y, path_x, color, img, calibration, top_down, lid_color)
|
||||
|
||||
def draw_lead_car(closest, top_down):
|
||||
if closest != None:
|
||||
closest_y = int(round(UP.lidar_car_y - closest * UP.lidar_zoom))
|
||||
if closest_y > 0:
|
||||
top_down[1][int(round(UP.lidar_car_x - METER_WIDTH * 2)):int(
|
||||
round(UP.lidar_car_x + METER_WIDTH * 2)), closest_y] = find_color(
|
||||
top_down[0], (255, 0, 0))
|
||||
|
||||
def draw_lead_on(img, closest_x_m, closest_y_m, calibration, color, sz=10, img_offset=(0, 0)):
|
||||
uv = warp_points(np.asarray([closest_x_m, closest_y_m]), calibration.car_to_bb)[0]
|
||||
u, v = int(uv[0] + img_offset[0]), int(uv[1] + img_offset[1])
|
||||
if u > 0 and u < 640 and v > 0 and v < 480 - 5:
|
||||
img[v - 5 - sz:v - 5 + sz, u] = color
|
||||
img[v - 5, u - sz:u + sz] = color
|
||||
return u, v
|
||||
|
||||
|
||||
if platform.system() != 'Darwin':
|
||||
matplotlib.use('QT4Agg')
|
||||
|
||||
|
||||
def init_plots(arr, name_to_arr_idx, plot_xlims, plot_ylims, plot_names, plot_colors, plot_styles, bigplots=False):
|
||||
color_palette = { "r": (1,0,0),
|
||||
"g": (0,1,0),
|
||||
"b": (0,0,1),
|
||||
"k": (0,0,0),
|
||||
"y": (1,1,0),
|
||||
"p": (0,1,1),
|
||||
"m": (1,0,1) }
|
||||
|
||||
if bigplots == True:
|
||||
fig = plt.figure(figsize=(6.4, 7.0))
|
||||
elif bigplots == False:
|
||||
fig = plt.figure()
|
||||
else:
|
||||
fig = plt.figure(figsize=bigplots)
|
||||
|
||||
fig.set_facecolor((0.2,0.2,0.2))
|
||||
|
||||
axs = []
|
||||
for pn in range(len(plot_ylims)):
|
||||
ax = fig.add_subplot(len(plot_ylims),1,len(axs)+1)
|
||||
ax.set_xlim(plot_xlims[pn][0], plot_xlims[pn][1])
|
||||
ax.set_ylim(plot_ylims[pn][0], plot_ylims[pn][1])
|
||||
ax.patch.set_facecolor((0.4, 0.4, 0.4))
|
||||
axs.append(ax)
|
||||
|
||||
plots = [] ;idxs = [] ;plot_select = []
|
||||
for i, pl_list in enumerate(plot_names):
|
||||
for j, item in enumerate(pl_list):
|
||||
plot, = axs[i].plot(arr[:, name_to_arr_idx[item]],
|
||||
label=item,
|
||||
color=color_palette[plot_colors[i][j]],
|
||||
linestyle=plot_styles[i][j])
|
||||
plots.append(plot)
|
||||
idxs.append(name_to_arr_idx[item])
|
||||
plot_select.append(i)
|
||||
axs[i].set_title(", ".join("%s (%s)" % (nm, cl)
|
||||
for (nm, cl) in zip(pl_list, plot_colors[i])), fontsize=10)
|
||||
if i < len(plot_ylims) - 1:
|
||||
axs[i].set_xticks([])
|
||||
|
||||
fig.canvas.draw()
|
||||
|
||||
renderer = fig.canvas.get_renderer()
|
||||
|
||||
if matplotlib.get_backend() == "MacOSX":
|
||||
fig.draw(renderer)
|
||||
|
||||
def draw_plots(arr):
|
||||
for ax in axs:
|
||||
ax.draw_artist(ax.patch)
|
||||
for i in range(len(plots)):
|
||||
plots[i].set_ydata(arr[:, idxs[i]])
|
||||
axs[plot_select[i]].draw_artist(plots[i])
|
||||
|
||||
if matplotlib.get_backend() == "QT4Agg":
|
||||
fig.canvas.update()
|
||||
fig.canvas.flush_events()
|
||||
|
||||
raw_data = renderer.tostring_rgb()
|
||||
#print fig.canvas.get_width_height()
|
||||
plot_surface = pygame.image.frombuffer(raw_data, fig.canvas.get_width_height(), "RGB").convert()
|
||||
return plot_surface
|
||||
|
||||
return draw_plots
|
||||
|
||||
|
||||
def draw_mpc(liveMpc, top_down):
|
||||
mpc_color = find_color(top_down[0], (0, 255, 0))
|
||||
for p in zip(liveMpc.x, liveMpc.y):
|
||||
px, py = to_lid_pt(*p)
|
||||
top_down[1][px, py] = mpc_color
|
||||
|
||||
|
||||
|
||||
class CalibrationTransformsForWarpMatrix(object):
|
||||
def __init__(self, model_to_full_frame, K, E):
|
||||
self._model_to_full_frame = model_to_full_frame
|
||||
self._K = K
|
||||
self._E = E
|
||||
|
||||
@property
|
||||
def model_to_bb(self):
|
||||
return _FULL_FRAME_TO_BB.dot(self._model_to_full_frame)
|
||||
|
||||
@lazy_property
|
||||
def model_to_full_frame(self):
|
||||
return self._model_to_full_frame
|
||||
|
||||
@lazy_property
|
||||
def car_to_model(self):
|
||||
return np.linalg.inv(self._model_to_full_frame).dot(self._K).dot(
|
||||
self._E[:, [0, 1, 3]])
|
||||
|
||||
@lazy_property
|
||||
def car_to_bb(self):
|
||||
return _BB_TO_FULL_FRAME.dot(self._K).dot(self._E[:, [0, 1, 3]])
|
||||
|
||||
|
||||
def pygame_modules_have_loaded():
|
||||
return pygame.display.get_init() and pygame.font.get_init()
|
||||
|
||||
def draw_var(y, x, var, color, img, calibration, top_down):
|
||||
# otherwise drawing gets stupid
|
||||
var = max(1e-1, min(var, 0.7))
|
||||
|
||||
varcolor = tuple(np.array(color)*0.5)
|
||||
draw_path(y - var, x, varcolor, img, calibration, top_down)
|
||||
draw_path(y + var, x, varcolor, img, calibration, top_down)
|
||||
|
||||
|
||||
class ModelPoly(object):
|
||||
def __init__(self, model_path):
|
||||
if len(model_path.points) == 0 and len(model_path.poly) == 0:
|
||||
self.valid = False
|
||||
return
|
||||
|
||||
if len(model_path.poly):
|
||||
self.poly = np.array(model_path.poly)
|
||||
else:
|
||||
self.poly = model_polyfit(model_path.points, _PATH_PINV)
|
||||
|
||||
self.prob = model_path.prob
|
||||
self.std = model_path.std
|
||||
self.y = np.polyval(self.poly, _PATH_XD)
|
||||
self.valid = True
|
||||
|
||||
def extract_model_data(md):
|
||||
return ModelUIData(
|
||||
cpath=ModelPoly(md.path),
|
||||
lpath=ModelPoly(md.leftLane),
|
||||
rpath=ModelPoly(md.rightLane),
|
||||
lead=md.lead,
|
||||
lead_future=md.leadFuture,
|
||||
)
|
||||
|
||||
def plot_model(m, VM, v_ego, curvature, imgw, calibration, top_down, d_poly, top_down_color=216):
|
||||
if calibration is None or top_down is None:
|
||||
return
|
||||
|
||||
for lead in [m.lead, m.lead_future]:
|
||||
if lead.prob < 0.5:
|
||||
continue
|
||||
|
||||
lead_dist_from_radar = lead.dist - RADAR_TO_CAMERA
|
||||
_, py_top = to_lid_pt(lead_dist_from_radar + lead.std, lead.relY)
|
||||
px, py_bottom = to_lid_pt(lead_dist_from_radar - lead.std, lead.relY)
|
||||
top_down[1][int(round(px - 4)):int(round(px + 4)), py_top:py_bottom] = top_down_color
|
||||
|
||||
color = (0, int(255 * m.lpath.prob), 0)
|
||||
for path in [m.cpath, m.lpath, m.rpath]:
|
||||
if path.valid:
|
||||
draw_path(path.y, _PATH_XD, color, imgw, calibration, top_down, YELLOW)
|
||||
draw_var(path.y, _PATH_XD, path.std, color, imgw, calibration, top_down)
|
||||
|
||||
if d_poly is not None:
|
||||
dpath_y = np.polyval(d_poly, _PATH_X)
|
||||
draw_path(dpath_y, _PATH_X, RED, imgw, calibration, top_down, RED)
|
||||
|
||||
# draw user path from curvature
|
||||
draw_steer_path(v_ego, curvature, BLUE, imgw, calibration, top_down, VM, BLUE)
|
||||
|
||||
|
||||
def maybe_update_radar_points(lt, lid_overlay):
|
||||
ar_pts = []
|
||||
if lt is not None:
|
||||
ar_pts = {}
|
||||
for track in lt:
|
||||
ar_pts[track.trackId] = [track.dRel, track.yRel, track.vRel, track.aRel, track.oncoming, track.stationary]
|
||||
for ids, pt in ar_pts.items():
|
||||
px, py = to_lid_pt(pt[0], pt[1])
|
||||
if px != -1:
|
||||
if pt[-1]:
|
||||
color = 240
|
||||
elif pt[-2]:
|
||||
color = 230
|
||||
else:
|
||||
color = 255
|
||||
if int(ids) == 1:
|
||||
lid_overlay[px - 2:px + 2, py - 10:py + 10] = 100
|
||||
else:
|
||||
lid_overlay[px - 2:px + 2, py - 2:py + 2] = color
|
||||
|
||||
def get_blank_lid_overlay(UP):
|
||||
lid_overlay = np.zeros((UP.lidar_x, UP.lidar_y), 'uint8')
|
||||
# Draw the car.
|
||||
lid_overlay[int(round(UP.lidar_car_x - UP.car_hwidth)):int(
|
||||
round(UP.lidar_car_x + UP.car_hwidth)), int(round(UP.lidar_car_y -
|
||||
UP.car_front))] = UP.car_color
|
||||
lid_overlay[int(round(UP.lidar_car_x - UP.car_hwidth)):int(
|
||||
round(UP.lidar_car_x + UP.car_hwidth)), int(round(UP.lidar_car_y +
|
||||
UP.car_back))] = UP.car_color
|
||||
lid_overlay[int(round(UP.lidar_car_x - UP.car_hwidth)), int(
|
||||
round(UP.lidar_car_y - UP.car_front)):int(round(
|
||||
UP.lidar_car_y + UP.car_back))] = UP.car_color
|
||||
lid_overlay[int(round(UP.lidar_car_x + UP.car_hwidth)), int(
|
||||
round(UP.lidar_car_y - UP.car_front)):int(round(
|
||||
UP.lidar_car_y + UP.car_back))] = UP.car_color
|
||||
return lid_overlay
|
||||
|
|
@ -0,0 +1,68 @@
|
|||
#!/usr/bin/env python
|
||||
|
||||
import matplotlib
|
||||
matplotlib.use('TkAgg')
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
import numpy as np
|
||||
import zmq
|
||||
from cereal.services import service_list
|
||||
from selfdrive.config import Conversions as CV
|
||||
import cereal.messaging as messaging
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
live_map_sock = messaging.sub_sock(service_list['liveMapData'].port, conflate=True)
|
||||
plan_sock = messaging.sub_sock(service_list['plan'].port, conflate=True)
|
||||
|
||||
plt.ion()
|
||||
fig = plt.figure(figsize=(8, 16))
|
||||
ax = fig.add_subplot(2, 1, 1)
|
||||
ax.set_title('Map')
|
||||
|
||||
SCALE = 1000
|
||||
ax.set_xlim([-SCALE, SCALE])
|
||||
ax.set_ylim([-SCALE, SCALE])
|
||||
ax.set_xlabel('x [m]')
|
||||
ax.set_ylabel('y [m]')
|
||||
ax.grid(True)
|
||||
|
||||
points_plt, = ax.plot([0.0], [0.0], "--xk")
|
||||
cur, = ax.plot([0.0], [0.0], "xr")
|
||||
|
||||
speed_txt = ax.text(-500, 900, '')
|
||||
curv_txt = ax.text(-500, 775, '')
|
||||
|
||||
ax = fig.add_subplot(2, 1, 2)
|
||||
ax.set_title('Curvature')
|
||||
curvature_plt, = ax.plot([0.0], [0.0], "--xk")
|
||||
ax.set_xlim([0, 500])
|
||||
ax.set_ylim([0, 1e-2])
|
||||
ax.set_xlabel('Distance along path [m]')
|
||||
ax.set_ylabel('Curvature [1/m]')
|
||||
ax.grid(True)
|
||||
|
||||
plt.show()
|
||||
|
||||
while True:
|
||||
m = messaging.recv_one_or_none(live_map_sock)
|
||||
p = messaging.recv_one_or_none(plan_sock)
|
||||
if p is not None:
|
||||
v = p.plan.vCurvature * CV.MS_TO_MPH
|
||||
speed_txt.set_text('Desired curvature speed: %.2f mph' % v)
|
||||
|
||||
if m is not None:
|
||||
print("Current way id: %d" % m.liveMapData.wayId)
|
||||
curv_txt.set_text('Curvature valid: %s Dist: %03.0f m\nSpeedlimit valid: %s Speed: %.0f mph' %
|
||||
(str(m.liveMapData.curvatureValid),
|
||||
m.liveMapData.distToTurn,
|
||||
str(m.liveMapData.speedLimitValid),
|
||||
m.liveMapData.speedLimit * CV.MS_TO_MPH))
|
||||
|
||||
points_plt.set_xdata(m.liveMapData.roadX)
|
||||
points_plt.set_ydata(m.liveMapData.roadY)
|
||||
curvature_plt.set_xdata(m.liveMapData.roadCurvatureX)
|
||||
curvature_plt.set_ydata(m.liveMapData.roadCurvature)
|
||||
|
||||
fig.canvas.draw()
|
||||
fig.canvas.flush_events()
|
||||
|
|
@ -0,0 +1,83 @@
|
|||
#!/usr/bin/env python
|
||||
import os
|
||||
import sys
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
import cereal.messaging as messaging
|
||||
import time
|
||||
|
||||
|
||||
# tool to plot one or more signals live. Call ex:
|
||||
#./rqplot.py log.carState.vEgo log.carState.aEgo
|
||||
|
||||
# TODO: can this tool consume 10x less cpu?
|
||||
|
||||
def recursive_getattr(x, name):
|
||||
l = name.split('.')
|
||||
if len(l) == 1:
|
||||
return getattr(x, name)
|
||||
else:
|
||||
return recursive_getattr(getattr(x, l[0]), ".".join(l[1:]) )
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
poller = messaging.Poller()
|
||||
|
||||
services = []
|
||||
fields = []
|
||||
subs = []
|
||||
values = []
|
||||
|
||||
plt.ion()
|
||||
fig, ax = plt.subplots()
|
||||
#fig = plt.figure(figsize=(10, 15))
|
||||
#ax = fig.add_subplot(111)
|
||||
ax.grid(True)
|
||||
fig.canvas.draw()
|
||||
|
||||
subs_name = sys.argv[1:]
|
||||
lines = []
|
||||
x, y = [], []
|
||||
LEN = 500
|
||||
|
||||
for i, sub in enumerate(subs_name):
|
||||
sub_split = sub.split(".")
|
||||
services.append(sub_split[0])
|
||||
fields.append(".".join(sub_split[1:]))
|
||||
subs.append(messaging.sub_sock(sub_split[0], poller))
|
||||
|
||||
x.append(np.ones(LEN)*np.nan)
|
||||
y.append(np.ones(LEN)*np.nan)
|
||||
lines.append(ax.plot(x[i], y[i])[0])
|
||||
|
||||
for l in lines:
|
||||
l.set_marker("*")
|
||||
|
||||
cur_t = 0.
|
||||
ax.legend(subs_name)
|
||||
ax.set_xlabel('time [s]')
|
||||
|
||||
while 1:
|
||||
print(1./(time.time() - cur_t))
|
||||
cur_t = time.time()
|
||||
for i, s in enumerate(subs):
|
||||
msg = messaging.recv_sock(s)
|
||||
#msg = messaging.recv_one_or_none(s)
|
||||
if msg is not None:
|
||||
x[i] = np.append(x[i], getattr(msg, 'logMonoTime') / float(1e9))
|
||||
x[i] = np.delete(x[i], 0)
|
||||
y[i] = np.append(y[i], recursive_getattr(msg, subs_name[i]))
|
||||
y[i] = np.delete(y[i], 0)
|
||||
|
||||
lines[i].set_xdata(x[i])
|
||||
lines[i].set_ydata(y[i])
|
||||
|
||||
ax.relim()
|
||||
ax.autoscale_view(True, scaley=True, scalex=True)
|
||||
|
||||
fig.canvas.blit(ax.bbox)
|
||||
fig.canvas.flush_events()
|
||||
|
||||
# just a bit of wait to avoid 100% CPU usage
|
||||
time.sleep(0.001)
|
||||
|
||||
|
|
@ -0,0 +1,278 @@
|
|||
#!/usr/bin/env python
|
||||
import argparse
|
||||
import os
|
||||
import sys
|
||||
|
||||
os.environ["OMP_NUM_THREADS"] = "1"
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
import pygame
|
||||
|
||||
from common.basedir import BASEDIR
|
||||
from common.transformations.camera import FULL_FRAME_SIZE, eon_intrinsics
|
||||
from common.transformations.model import (MODEL_CX, MODEL_CY, MODEL_INPUT_SIZE,
|
||||
get_camera_frame_from_model_frame)
|
||||
from selfdrive.car.toyota.interface import CarInterface as ToyotaInterface
|
||||
from selfdrive.config import UIParams as UP
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
import cereal.messaging as messaging
|
||||
from tools.replay.lib.ui_helpers import (_BB_TO_FULL_FRAME, BLACK, BLUE, GREEN,
|
||||
YELLOW, RED,
|
||||
CalibrationTransformsForWarpMatrix,
|
||||
draw_lead_car, draw_lead_on, draw_mpc,
|
||||
extract_model_data,
|
||||
get_blank_lid_overlay, init_plots,
|
||||
maybe_update_radar_points, plot_model,
|
||||
pygame_modules_have_loaded,
|
||||
warp_points)
|
||||
|
||||
os.environ['BASEDIR'] = BASEDIR
|
||||
|
||||
ANGLE_SCALE = 5.0
|
||||
HOR = os.getenv("HORIZONTAL") is not None
|
||||
|
||||
|
||||
def ui_thread(addr, frame_address):
|
||||
# TODO: Detect car from replay and use that to select carparams
|
||||
CP = ToyotaInterface.get_params("TOYOTA PRIUS 2017")
|
||||
VM = VehicleModel(CP)
|
||||
|
||||
CalP = np.asarray([[0, 0], [MODEL_INPUT_SIZE[0], 0], [MODEL_INPUT_SIZE[0], MODEL_INPUT_SIZE[1]], [0, MODEL_INPUT_SIZE[1]]])
|
||||
vanishing_point = np.asarray([[MODEL_CX, MODEL_CY]])
|
||||
|
||||
pygame.init()
|
||||
pygame.font.init()
|
||||
assert pygame_modules_have_loaded()
|
||||
|
||||
if HOR:
|
||||
size = (640+384+640, 960)
|
||||
write_x = 5
|
||||
write_y = 680
|
||||
else:
|
||||
size = (640+384, 960+300)
|
||||
write_x = 645
|
||||
write_y = 970
|
||||
|
||||
pygame.display.set_caption("openpilot debug UI")
|
||||
screen = pygame.display.set_mode(size, pygame.DOUBLEBUF)
|
||||
|
||||
alert1_font = pygame.font.SysFont("arial", 30)
|
||||
alert2_font = pygame.font.SysFont("arial", 20)
|
||||
info_font = pygame.font.SysFont("arial", 15)
|
||||
|
||||
camera_surface = pygame.surface.Surface((640, 480), 0, 24).convert()
|
||||
cameraw_surface = pygame.surface.Surface(MODEL_INPUT_SIZE, 0, 24).convert()
|
||||
cameraw_test_surface = pygame.surface.Surface(MODEL_INPUT_SIZE, 0, 24)
|
||||
top_down_surface = pygame.surface.Surface((UP.lidar_x, UP.lidar_y),0,8)
|
||||
|
||||
frame = messaging.sub_sock('frame', addr=addr, conflate=True)
|
||||
sm = messaging.SubMaster(['carState', 'plan', 'carControl', 'radarState', 'liveCalibration', 'controlsState', 'liveTracks', 'model', 'liveMpc', 'liveParameters', 'pathPlan'], addr=addr)
|
||||
|
||||
calibration = None
|
||||
img = np.zeros((480, 640, 3), dtype='uint8')
|
||||
imgff = np.zeros((FULL_FRAME_SIZE[1], FULL_FRAME_SIZE[0], 3), dtype=np.uint8)
|
||||
imgw = np.zeros((160, 320, 3), dtype=np.uint8) # warped image
|
||||
lid_overlay_blank = get_blank_lid_overlay(UP)
|
||||
|
||||
# plots
|
||||
name_to_arr_idx = { "gas": 0,
|
||||
"computer_gas": 1,
|
||||
"user_brake": 2,
|
||||
"computer_brake": 3,
|
||||
"v_ego": 4,
|
||||
"v_pid": 5,
|
||||
"angle_steers_des": 6,
|
||||
"angle_steers": 7,
|
||||
"angle_steers_k": 8,
|
||||
"steer_torque": 9,
|
||||
"v_override": 10,
|
||||
"v_cruise": 11,
|
||||
"a_ego": 12,
|
||||
"a_target": 13,
|
||||
"accel_override": 14}
|
||||
|
||||
plot_arr = np.zeros((100, len(name_to_arr_idx.values())))
|
||||
|
||||
plot_xlims = [(0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0])]
|
||||
plot_ylims = [(-0.1, 1.1), (-ANGLE_SCALE, ANGLE_SCALE), (0., 75.), (-3.0, 2.0)]
|
||||
plot_names = [["gas", "computer_gas", "user_brake", "computer_brake", "accel_override"],
|
||||
["angle_steers", "angle_steers_des", "angle_steers_k", "steer_torque"],
|
||||
["v_ego", "v_override", "v_pid", "v_cruise"],
|
||||
["a_ego", "a_target"]]
|
||||
plot_colors = [["b", "b", "g", "r", "y"],
|
||||
["b", "g", "y", "r"],
|
||||
["b", "g", "r", "y"],
|
||||
["b", "r"]]
|
||||
plot_styles = [["-", "-", "-", "-", "-"],
|
||||
["-", "-", "-", "-"],
|
||||
["-", "-", "-", "-"],
|
||||
["-", "-"]]
|
||||
|
||||
draw_plots = init_plots(plot_arr, name_to_arr_idx, plot_xlims, plot_ylims, plot_names, plot_colors, plot_styles, bigplots=True)
|
||||
|
||||
counter = 0
|
||||
while 1:
|
||||
list(pygame.event.get())
|
||||
|
||||
screen.fill((64,64,64))
|
||||
lid_overlay = lid_overlay_blank.copy()
|
||||
top_down = top_down_surface, lid_overlay
|
||||
|
||||
# ***** frame *****
|
||||
fpkt = messaging.recv_one(frame)
|
||||
rgb_img_raw = fpkt.frame.image
|
||||
|
||||
if fpkt.frame.transform:
|
||||
img_transform = np.array(fpkt.frame.transform).reshape(3,3)
|
||||
else:
|
||||
# assume frame is flipped
|
||||
img_transform = np.array([
|
||||
[-1.0, 0.0, FULL_FRAME_SIZE[0]-1],
|
||||
[ 0.0, -1.0, FULL_FRAME_SIZE[1]-1],
|
||||
[ 0.0, 0.0, 1.0]
|
||||
])
|
||||
|
||||
|
||||
if rgb_img_raw and len(rgb_img_raw) == FULL_FRAME_SIZE[0] * FULL_FRAME_SIZE[1] * 3:
|
||||
imgff = np.frombuffer(rgb_img_raw, dtype=np.uint8).reshape((FULL_FRAME_SIZE[1], FULL_FRAME_SIZE[0], 3))
|
||||
imgff = imgff[:, :, ::-1] # Convert BGR to RGB
|
||||
cv2.warpAffine(imgff, np.dot(img_transform, _BB_TO_FULL_FRAME)[:2],
|
||||
(img.shape[1], img.shape[0]), dst=img, flags=cv2.WARP_INVERSE_MAP)
|
||||
|
||||
intrinsic_matrix = eon_intrinsics
|
||||
else:
|
||||
img.fill(0)
|
||||
intrinsic_matrix = np.eye(3)
|
||||
|
||||
if calibration is not None:
|
||||
transform = np.dot(img_transform, calibration.model_to_full_frame)
|
||||
imgw = cv2.warpAffine(imgff, transform[:2], (MODEL_INPUT_SIZE[0], MODEL_INPUT_SIZE[1]), flags=cv2.WARP_INVERSE_MAP)
|
||||
else:
|
||||
imgw.fill(0)
|
||||
|
||||
sm.update()
|
||||
|
||||
w = sm['controlsState'].lateralControlState.which()
|
||||
if w == 'lqrState':
|
||||
angle_steers_k = sm['controlsState'].lateralControlState.lqrState.steerAngle
|
||||
elif w == 'indiState':
|
||||
angle_steers_k = sm['controlsState'].lateralControlState.indiState.steerAngle
|
||||
else:
|
||||
angle_steers_k = np.inf
|
||||
|
||||
plot_arr[:-1] = plot_arr[1:]
|
||||
plot_arr[-1, name_to_arr_idx['angle_steers']] = sm['controlsState'].angleSteers
|
||||
plot_arr[-1, name_to_arr_idx['angle_steers_des']] = sm['carControl'].actuators.steerAngle
|
||||
plot_arr[-1, name_to_arr_idx['angle_steers_k']] = angle_steers_k
|
||||
plot_arr[-1, name_to_arr_idx['gas']] = sm['carState'].gas
|
||||
plot_arr[-1, name_to_arr_idx['computer_gas']] = sm['carControl'].actuators.gas
|
||||
plot_arr[-1, name_to_arr_idx['user_brake']] = sm['carState'].brake
|
||||
plot_arr[-1, name_to_arr_idx['steer_torque']] = sm['carControl'].actuators.steer * ANGLE_SCALE
|
||||
plot_arr[-1, name_to_arr_idx['computer_brake']] = sm['carControl'].actuators.brake
|
||||
plot_arr[-1, name_to_arr_idx['v_ego']] = sm['controlsState'].vEgo
|
||||
plot_arr[-1, name_to_arr_idx['v_pid']] = sm['controlsState'].vPid
|
||||
plot_arr[-1, name_to_arr_idx['v_override']] = sm['carControl'].cruiseControl.speedOverride
|
||||
plot_arr[-1, name_to_arr_idx['v_cruise']] = sm['carState'].cruiseState.speed
|
||||
plot_arr[-1, name_to_arr_idx['a_ego']] = sm['carState'].aEgo
|
||||
plot_arr[-1, name_to_arr_idx['a_target']] = sm['plan'].aTarget
|
||||
plot_arr[-1, name_to_arr_idx['accel_override']] = sm['carControl'].cruiseControl.accelOverride
|
||||
|
||||
# ***** model ****
|
||||
if len(sm['model'].path.poly) > 0:
|
||||
model_data = extract_model_data(sm['model'])
|
||||
plot_model(model_data, VM, sm['controlsState'].vEgo, sm['controlsState'].curvature, imgw, calibration,
|
||||
top_down, np.array(sm['pathPlan'].dPoly))
|
||||
|
||||
# MPC
|
||||
if sm.updated['liveMpc']:
|
||||
draw_mpc(sm['liveMpc'], top_down)
|
||||
|
||||
# draw all radar points
|
||||
maybe_update_radar_points(sm['liveTracks'], top_down[1])
|
||||
|
||||
|
||||
if sm.updated['liveCalibration']:
|
||||
extrinsic_matrix = np.asarray(sm['liveCalibration'].extrinsicMatrix).reshape(3, 4)
|
||||
ke = intrinsic_matrix.dot(extrinsic_matrix)
|
||||
warp_matrix = get_camera_frame_from_model_frame(ke)
|
||||
calibration = CalibrationTransformsForWarpMatrix(warp_matrix, intrinsic_matrix, extrinsic_matrix)
|
||||
|
||||
# draw red pt for lead car in the main img
|
||||
for lead in [sm['radarState'].leadOne, sm['radarState'].leadTwo]:
|
||||
if lead.status:
|
||||
if calibration is not None:
|
||||
draw_lead_on(img, lead.dRel, lead.yRel, calibration, color=(192,0,0))
|
||||
|
||||
draw_lead_car(lead.dRel, top_down)
|
||||
|
||||
# *** blits ***
|
||||
pygame.surfarray.blit_array(camera_surface, img.swapaxes(0,1))
|
||||
screen.blit(camera_surface, (0, 0))
|
||||
|
||||
# display alerts
|
||||
alert_line1 = alert1_font.render(sm['controlsState'].alertText1, True, (255,0,0))
|
||||
alert_line2 = alert2_font.render(sm['controlsState'].alertText2, True, (255,0,0))
|
||||
screen.blit(alert_line1, (180, 150))
|
||||
screen.blit(alert_line2, (180, 190))
|
||||
|
||||
if calibration is not None and img is not None:
|
||||
cpw = warp_points(CalP, calibration.model_to_bb)
|
||||
vanishing_pointw = warp_points(vanishing_point, calibration.model_to_bb)
|
||||
pygame.draw.polygon(screen, BLUE, tuple(map(tuple, cpw)), 1)
|
||||
pygame.draw.circle(screen, BLUE, list(map(int, map(round, vanishing_pointw[0]))), 2)
|
||||
|
||||
if HOR:
|
||||
screen.blit(draw_plots(plot_arr), (640+384, 0))
|
||||
else:
|
||||
screen.blit(draw_plots(plot_arr), (0, 600))
|
||||
|
||||
pygame.surfarray.blit_array(cameraw_surface, imgw.swapaxes(0, 1))
|
||||
screen.blit(cameraw_surface, (320, 480))
|
||||
|
||||
pygame.surfarray.blit_array(*top_down)
|
||||
screen.blit(top_down[0], (640,0))
|
||||
|
||||
i = 0
|
||||
SPACING = 25
|
||||
|
||||
lines = [
|
||||
info_font.render("ENABLED", True, GREEN if sm['controlsState'].enabled else BLACK),
|
||||
info_font.render("BRAKE LIGHTS", True, RED if sm['carState'].brakeLights else BLACK),
|
||||
info_font.render("SPEED: " + str(round(sm['carState'].vEgo, 1)) + " m/s", True, YELLOW),
|
||||
info_font.render("LONG CONTROL STATE: " + str(sm['controlsState'].longControlState), True, YELLOW),
|
||||
info_font.render("LONG MPC SOURCE: " + str(sm['plan'].longitudinalPlanSource), True, YELLOW),
|
||||
None,
|
||||
info_font.render("ANGLE OFFSET (AVG): " + str(round(sm['liveParameters'].angleOffsetAverage, 2)) + " deg", True, YELLOW),
|
||||
info_font.render("ANGLE OFFSET (INSTANT): " + str(round(sm['liveParameters'].angleOffset, 2)) + " deg", True, YELLOW),
|
||||
info_font.render("STIFFNESS: " + str(round(sm['liveParameters'].stiffnessFactor * 100., 2)) + " %", True, YELLOW),
|
||||
info_font.render("STEER RATIO: " + str(round(sm['liveParameters'].steerRatio, 2)), True, YELLOW)
|
||||
]
|
||||
|
||||
for i, line in enumerate(lines):
|
||||
if line is not None:
|
||||
screen.blit(line, (write_x, write_y + i * SPACING))
|
||||
|
||||
# this takes time...vsync or something
|
||||
pygame.display.flip()
|
||||
|
||||
def get_arg_parser():
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Show replay data in a UI.",
|
||||
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
|
||||
|
||||
parser.add_argument("ip_address", nargs="?", default="127.0.0.1",
|
||||
help="The ip address on which to receive zmq messages.")
|
||||
|
||||
parser.add_argument("--frame-address", default=None,
|
||||
help="The frame address (fully qualified ZMQ endpoint for frames) on which to receive zmq messages.")
|
||||
return parser
|
||||
|
||||
if __name__ == "__main__":
|
||||
args = get_arg_parser().parse_args(sys.argv[1:])
|
||||
|
||||
if args.ip_address != "127.0.0.1":
|
||||
os.environ["ZMQ"] = "1"
|
||||
messaging.context = messaging.Context()
|
||||
|
||||
ui_thread(args.ip_address, args.frame_address)
|
||||
|
|
@ -0,0 +1,446 @@
|
|||
#!/usr/bin/env python
|
||||
import argparse
|
||||
import os
|
||||
import sys
|
||||
import zmq
|
||||
import time
|
||||
import gc
|
||||
import signal
|
||||
from threading import Thread
|
||||
import numpy as np
|
||||
from uuid import uuid4
|
||||
from collections import namedtuple
|
||||
from collections import deque
|
||||
from multiprocessing import Process, TimeoutError
|
||||
from datetime import datetime
|
||||
|
||||
# strat 1: script to copy files
|
||||
# strat 2: build pip packages around these
|
||||
# could be its own pip package, which we'd need to build and release
|
||||
from cereal import log as capnp_log
|
||||
from cereal.services import service_list
|
||||
from cereal.messaging import pub_sock, MultiplePublishersError
|
||||
from common import realtime
|
||||
|
||||
from tools.lib.file_helpers import mkdirs_exists_ok
|
||||
from tools.lib.kbhit import KBHit
|
||||
from tools.lib.logreader import MultiLogIterator
|
||||
from tools.lib.route import Route
|
||||
from tools.lib.route_framereader import RouteFrameReader
|
||||
|
||||
# Commands.
|
||||
SetRoute = namedtuple("SetRoute", ("name", "start_time", "data_dir"))
|
||||
SeekAbsoluteTime = namedtuple("SeekAbsoluteTime", ("secs",))
|
||||
SeekRelativeTime = namedtuple("SeekRelativeTime", ("secs",))
|
||||
TogglePause = namedtuple("TogglePause", ())
|
||||
StopAndQuit = namedtuple("StopAndQuit", ())
|
||||
|
||||
|
||||
class UnloggerWorker(object):
|
||||
def __init__(self):
|
||||
self._frame_reader = None
|
||||
self._cookie = None
|
||||
self._readahead = deque()
|
||||
|
||||
def run(self, commands_address, data_address, pub_types):
|
||||
zmq.Context._instance = None
|
||||
commands_socket = zmq.Context.instance().socket(zmq.PULL)
|
||||
commands_socket.connect(commands_address)
|
||||
|
||||
data_socket = zmq.Context.instance().socket(zmq.PUSH)
|
||||
data_socket.connect(data_address)
|
||||
|
||||
poller = zmq.Poller()
|
||||
poller.register(commands_socket, zmq.POLLIN)
|
||||
|
||||
# We can't publish frames without encodeIdx, so add when it's missing.
|
||||
if "frame" in pub_types:
|
||||
pub_types["encodeIdx"] = None
|
||||
|
||||
# gc.set_debug(gc.DEBUG_LEAK | gc.DEBUG_OBJECTS | gc.DEBUG_STATS | gc.DEBUG_SAVEALL |
|
||||
# gc.DEBUG_UNCOLLECTABLE)
|
||||
|
||||
# TODO: WARNING pycapnp leaks memory all over the place after unlogger runs for a while, gc
|
||||
# pauses become huge because there are so many tracked objects solution will be to switch to new
|
||||
# cython capnp
|
||||
try:
|
||||
route = None
|
||||
while True:
|
||||
while poller.poll(0.) or route is None:
|
||||
cookie, cmd = commands_socket.recv_pyobj()
|
||||
route = self._process_commands(cmd, route)
|
||||
|
||||
# **** get message ****
|
||||
self._read_logs(cookie, pub_types)
|
||||
self._send_logs(data_socket)
|
||||
finally:
|
||||
if self._frame_reader is not None:
|
||||
self._frame_reader.close()
|
||||
data_socket.close()
|
||||
commands_socket.close()
|
||||
|
||||
def _read_logs(self, cookie, pub_types):
|
||||
fullHEVC = capnp_log.EncodeIndex.Type.fullHEVC
|
||||
lr = self._lr
|
||||
while len(self._readahead) < 1000:
|
||||
route_time = lr.tell()
|
||||
msg = next(lr)
|
||||
typ = msg.which()
|
||||
if typ not in pub_types:
|
||||
continue
|
||||
|
||||
# **** special case certain message types ****
|
||||
if typ == "encodeIdx" and msg.encodeIdx.type == fullHEVC:
|
||||
# this assumes the encodeIdx always comes before the frame
|
||||
self._frame_id_lookup[
|
||||
msg.encodeIdx.frameId] = msg.encodeIdx.segmentNum, msg.encodeIdx.segmentId
|
||||
#print "encode", msg.encodeIdx.frameId, len(self._readahead), route_time
|
||||
self._readahead.appendleft((typ, msg, route_time, cookie))
|
||||
|
||||
def _send_logs(self, data_socket):
|
||||
while len(self._readahead) > 500:
|
||||
typ, msg, route_time, cookie = self._readahead.pop()
|
||||
smsg = msg.as_builder()
|
||||
|
||||
if typ == "frame":
|
||||
frame_id = msg.frame.frameId
|
||||
|
||||
# Frame exists, make sure we have a framereader.
|
||||
# load the frame readers as needed
|
||||
s1 = time.time()
|
||||
img = self._frame_reader.get(frame_id, pix_fmt="rgb24")
|
||||
fr_time = time.time() - s1
|
||||
if fr_time > 0.05:
|
||||
print("FRAME(%d) LAG -- %.2f ms" % (frame_id, fr_time*1000.0))
|
||||
|
||||
if img is not None:
|
||||
img = img[:, :, ::-1] # Convert RGB to BGR, which is what the camera outputs
|
||||
img = img.flatten()
|
||||
smsg.frame.image = img.tobytes()
|
||||
|
||||
data_socket.send_pyobj((cookie, typ, msg.logMonoTime, route_time), flags=zmq.SNDMORE)
|
||||
data_socket.send(smsg.to_bytes(), copy=False)
|
||||
|
||||
def _process_commands(self, cmd, route):
|
||||
seek_to = None
|
||||
if route is None or (isinstance(cmd, SetRoute) and route.name != cmd.name):
|
||||
seek_to = cmd.start_time
|
||||
route = Route(cmd.name, cmd.data_dir)
|
||||
self._lr = MultiLogIterator(route.log_paths(), wraparound=True)
|
||||
if self._frame_reader is not None:
|
||||
self._frame_reader.close()
|
||||
# reset frames for a route
|
||||
self._frame_id_lookup = {}
|
||||
self._frame_reader = RouteFrameReader(
|
||||
route.camera_paths(), None, self._frame_id_lookup, readahead=True)
|
||||
|
||||
# always reset this on a seek
|
||||
if isinstance(cmd, SeekRelativeTime):
|
||||
seek_to = self._lr.tell() + cmd.secs
|
||||
elif isinstance(cmd, SeekAbsoluteTime):
|
||||
seek_to = cmd.secs
|
||||
elif isinstance(cmd, StopAndQuit):
|
||||
exit()
|
||||
|
||||
if seek_to is not None:
|
||||
print("seeking", seek_to)
|
||||
if not self._lr.seek(seek_to):
|
||||
print("Can't seek: time out of bounds")
|
||||
else:
|
||||
next(self._lr) # ignore one
|
||||
return route
|
||||
|
||||
def _get_address_send_func(address):
|
||||
sock = pub_sock(address)
|
||||
return sock.send
|
||||
|
||||
|
||||
def unlogger_thread(command_address, forward_commands_address, data_address, run_realtime,
|
||||
address_mapping, publish_time_length, bind_early, no_loop):
|
||||
# Clear context to avoid problems with multiprocessing.
|
||||
zmq.Context._instance = None
|
||||
context = zmq.Context.instance()
|
||||
|
||||
command_sock = context.socket(zmq.PULL)
|
||||
command_sock.bind(command_address)
|
||||
|
||||
forward_commands_socket = context.socket(zmq.PUSH)
|
||||
forward_commands_socket.bind(forward_commands_address)
|
||||
|
||||
data_socket = context.socket(zmq.PULL)
|
||||
data_socket.bind(data_address)
|
||||
|
||||
# Set readahead to a reasonable number.
|
||||
data_socket.setsockopt(zmq.RCVHWM, 10000)
|
||||
|
||||
poller = zmq.Poller()
|
||||
poller.register(command_sock, zmq.POLLIN)
|
||||
poller.register(data_socket, zmq.POLLIN)
|
||||
|
||||
if bind_early:
|
||||
send_funcs = {
|
||||
typ: _get_address_send_func(address)
|
||||
for typ, address in address_mapping.items()
|
||||
}
|
||||
|
||||
# Give subscribers a chance to connect.
|
||||
time.sleep(0.1)
|
||||
else:
|
||||
send_funcs = {}
|
||||
|
||||
start_time = float("inf")
|
||||
printed_at = 0
|
||||
generation = 0
|
||||
paused = False
|
||||
reset_time = True
|
||||
prev_msg_time = None
|
||||
while True:
|
||||
evts = dict(poller.poll())
|
||||
if command_sock in evts:
|
||||
cmd = command_sock.recv_pyobj()
|
||||
if isinstance(cmd, TogglePause):
|
||||
paused = not paused
|
||||
if paused:
|
||||
poller.modify(data_socket, 0)
|
||||
else:
|
||||
poller.modify(data_socket, zmq.POLLIN)
|
||||
else:
|
||||
# Forward the command the the log data thread.
|
||||
# TODO: Remove everything on data_socket.
|
||||
generation += 1
|
||||
forward_commands_socket.send_pyobj((generation, cmd))
|
||||
if isinstance(cmd, StopAndQuit):
|
||||
return
|
||||
|
||||
reset_time = True
|
||||
elif data_socket in evts:
|
||||
msg_generation, typ, msg_time, route_time = data_socket.recv_pyobj(flags=zmq.RCVMORE)
|
||||
msg_bytes = data_socket.recv()
|
||||
if msg_generation < generation:
|
||||
# Skip packets.
|
||||
continue
|
||||
|
||||
if no_loop and prev_msg_time is not None and prev_msg_time > msg_time + 1e9:
|
||||
generation += 1
|
||||
forward_commands_socket.send_pyobj((generation, StopAndQuit()))
|
||||
return
|
||||
prev_msg_time = msg_time
|
||||
|
||||
msg_time_seconds = msg_time * 1e-9
|
||||
if reset_time:
|
||||
msg_start_time = msg_time_seconds
|
||||
real_start_time = realtime.sec_since_boot()
|
||||
start_time = min(start_time, msg_start_time)
|
||||
reset_time = False
|
||||
|
||||
if publish_time_length and msg_time_seconds - start_time > publish_time_length:
|
||||
generation += 1
|
||||
forward_commands_socket.send_pyobj((generation, StopAndQuit()))
|
||||
return
|
||||
|
||||
# Print time.
|
||||
if abs(printed_at - route_time) > 5.:
|
||||
print("at", route_time)
|
||||
printed_at = route_time
|
||||
|
||||
if typ not in send_funcs:
|
||||
if typ in address_mapping:
|
||||
# Remove so we don't keep printing warnings.
|
||||
address = address_mapping.pop(typ)
|
||||
try:
|
||||
print("binding", typ)
|
||||
send_funcs[typ] = _get_address_send_func(address)
|
||||
except Exception as e:
|
||||
print("couldn't replay {}: {}".format(typ, e))
|
||||
continue
|
||||
else:
|
||||
# Skip messages that we are not registered to publish.
|
||||
continue
|
||||
|
||||
# Sleep as needed for real time playback.
|
||||
if run_realtime:
|
||||
msg_time_offset = msg_time_seconds - msg_start_time
|
||||
real_time_offset = realtime.sec_since_boot() - real_start_time
|
||||
lag = msg_time_offset - real_time_offset
|
||||
if lag > 0 and lag < 30: # a large jump is OK, likely due to an out of order segment
|
||||
if lag > 1:
|
||||
print("sleeping for", lag)
|
||||
time.sleep(lag)
|
||||
elif lag < -1:
|
||||
# Relax the real time schedule when we slip far behind.
|
||||
reset_time = True
|
||||
|
||||
# Send message.
|
||||
try:
|
||||
send_funcs[typ](msg_bytes)
|
||||
except MultiplePublishersError:
|
||||
del send_funcs[typ]
|
||||
|
||||
def timestamp_to_s(tss):
|
||||
return time.mktime(datetime.strptime(tss, '%Y-%m-%d--%H-%M-%S').timetuple())
|
||||
|
||||
def absolute_time_str(s, start_time):
|
||||
try:
|
||||
# first try if it's a float
|
||||
return float(s)
|
||||
except ValueError:
|
||||
# now see if it's a timestamp
|
||||
return timestamp_to_s(s) - start_time
|
||||
|
||||
def _get_address_mapping(args):
|
||||
if args.min is not None:
|
||||
services_to_mock = [
|
||||
'thermal', 'can', 'health', 'sensorEvents', 'gpsNMEA', 'frame', 'encodeIdx',
|
||||
'model', 'features', 'liveLocation', 'gpsLocation'
|
||||
]
|
||||
elif args.enabled is not None:
|
||||
services_to_mock = args.enabled
|
||||
else:
|
||||
services_to_mock = service_list.keys()
|
||||
|
||||
address_mapping = {service_name: service_name for service_name in services_to_mock}
|
||||
address_mapping.update(dict(args.address_mapping))
|
||||
|
||||
for k in args.disabled:
|
||||
address_mapping.pop(k, None)
|
||||
|
||||
non_services = set(address_mapping) - set(service_list)
|
||||
if non_services:
|
||||
print("WARNING: Unknown services {}".format(list(non_services)))
|
||||
|
||||
return address_mapping
|
||||
|
||||
def keyboard_controller_thread(q, route_start_time):
|
||||
print("keyboard waiting for input")
|
||||
kb = KBHit()
|
||||
while 1:
|
||||
c = kb.getch()
|
||||
if c=='m': # Move forward by 1m
|
||||
q.send_pyobj(SeekRelativeTime(60))
|
||||
elif c=='M': # Move backward by 1m
|
||||
q.send_pyobj(SeekRelativeTime(-60))
|
||||
elif c=='s': # Move forward by 10s
|
||||
q.send_pyobj(SeekRelativeTime(10))
|
||||
elif c=='S': # Move backward by 10s
|
||||
q.send_pyobj(SeekRelativeTime(-10))
|
||||
elif c=='G': # Move backward by 10s
|
||||
q.send_pyobj(SeekAbsoluteTime(0.))
|
||||
elif c=="\x20": # Space bar.
|
||||
q.send_pyobj(TogglePause())
|
||||
elif c=="\n":
|
||||
try:
|
||||
seek_time_input = raw_input('time: ')
|
||||
seek_time = absolute_time_str(seek_time_input, route_start_time)
|
||||
|
||||
q.send_pyobj(SeekAbsoluteTime(seek_time))
|
||||
except Exception as e:
|
||||
print("Time not understood: {}".format(e))
|
||||
|
||||
def get_arg_parser():
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Mock openpilot components by publishing logged messages.",
|
||||
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
|
||||
|
||||
parser.add_argument("route_name", type=(lambda x: x.replace("#", "|")), nargs="?",
|
||||
help="The route whose messages will be published.")
|
||||
parser.add_argument("data_dir", nargs='?', default=os.getenv('UNLOGGER_DATA_DIR'),
|
||||
help="Path to directory in which log and camera files are located.")
|
||||
|
||||
parser.add_argument("--no-loop", action="store_true", help="Stop at the end of the replay.")
|
||||
|
||||
key_value_pair = lambda x: x.split("=")
|
||||
parser.add_argument("address_mapping", nargs="*", type=key_value_pair,
|
||||
help="Pairs <service>=<zmq_addr> to publish <service> on <zmq_addr>.")
|
||||
|
||||
comma_list = lambda x: x.split(",")
|
||||
to_mock_group = parser.add_mutually_exclusive_group()
|
||||
to_mock_group.add_argument("--min", action="store_true", default=os.getenv("MIN"))
|
||||
to_mock_group.add_argument("--enabled", default=os.getenv("ENABLED"), type=comma_list)
|
||||
|
||||
parser.add_argument("--disabled", type=comma_list, default=os.getenv("DISABLED") or ())
|
||||
|
||||
parser.add_argument(
|
||||
"--tl", dest="publish_time_length", type=float, default=None,
|
||||
help="Length of interval in event time for which messages should be published.")
|
||||
|
||||
parser.add_argument(
|
||||
"--no-realtime", dest="realtime", action="store_false", default=True,
|
||||
help="Publish messages as quickly as possible instead of realtime.")
|
||||
|
||||
parser.add_argument(
|
||||
"--no-interactive", dest="interactive", action="store_false", default=True,
|
||||
help="Disable interactivity.")
|
||||
|
||||
parser.add_argument(
|
||||
"--bind-early", action="store_true", default=False,
|
||||
help="Bind early to avoid dropping messages.")
|
||||
|
||||
return parser
|
||||
|
||||
def main(argv):
|
||||
args = get_arg_parser().parse_args(sys.argv[1:])
|
||||
|
||||
command_address = "ipc:///tmp/{}".format(uuid4())
|
||||
forward_commands_address = "ipc:///tmp/{}".format(uuid4())
|
||||
data_address = "ipc:///tmp/{}".format(uuid4())
|
||||
|
||||
address_mapping = _get_address_mapping(args)
|
||||
|
||||
command_sock = zmq.Context.instance().socket(zmq.PUSH)
|
||||
command_sock.connect(command_address)
|
||||
|
||||
if args.route_name is not None:
|
||||
route_name_split = args.route_name.split("|")
|
||||
if len(route_name_split) > 1:
|
||||
route_start_time = timestamp_to_s(route_name_split[1])
|
||||
else:
|
||||
route_start_time = 0
|
||||
command_sock.send_pyobj(
|
||||
SetRoute(args.route_name, 0, args.data_dir))
|
||||
else:
|
||||
print("waiting for external command...")
|
||||
route_start_time = 0
|
||||
|
||||
subprocesses = {}
|
||||
try:
|
||||
subprocesses["data"] = Process(
|
||||
target=UnloggerWorker().run,
|
||||
args=(forward_commands_address, data_address, address_mapping.copy()))
|
||||
|
||||
subprocesses["control"] = Process(
|
||||
target=unlogger_thread,
|
||||
args=(command_address, forward_commands_address, data_address, args.realtime,
|
||||
_get_address_mapping(args), args.publish_time_length, args.bind_early, args.no_loop))
|
||||
|
||||
for p in subprocesses.values():
|
||||
p.daemon = True
|
||||
|
||||
subprocesses["data"].start()
|
||||
subprocesses["control"].start()
|
||||
|
||||
# Exit if any of the children die.
|
||||
def exit_if_children_dead(*_):
|
||||
for name, p in subprocesses.items():
|
||||
if not p.is_alive():
|
||||
[p.terminate() for p in subprocesses.values()]
|
||||
exit()
|
||||
signal.signal(signal.SIGCHLD, signal.SIGIGN)
|
||||
signal.signal(signal.SIGCHLD, exit_if_children_dead)
|
||||
|
||||
if args.interactive:
|
||||
keyboard_controller_thread(command_sock, route_start_time)
|
||||
else:
|
||||
# Wait forever for children.
|
||||
while True:
|
||||
time.sleep(10000.)
|
||||
finally:
|
||||
for p in subprocesses.values():
|
||||
if p.is_alive():
|
||||
try:
|
||||
p.join(3.)
|
||||
except TimeoutError:
|
||||
p.terminate()
|
||||
continue
|
||||
|
||||
if __name__ == "__main__":
|
||||
sys.exit(main(sys.argv[1:]))
|
||||
|
|
@ -0,0 +1,11 @@
|
|||
aenum
|
||||
atomicwrites
|
||||
futures
|
||||
libarchive
|
||||
lru-dict
|
||||
matplotlib==2.0.2
|
||||
numpy
|
||||
opencv-python
|
||||
pygame
|
||||
hexdump==3.3
|
||||
av==0.5.0
|
||||
|
|
@ -0,0 +1,3 @@
|
|||
CARLA_*.tar.gz
|
||||
carla
|
||||
|
||||
|
|
@ -0,0 +1,100 @@
|
|||
#!/usr/bin/env python3
|
||||
import time
|
||||
import cereal.messaging as messaging
|
||||
from opendbc.can.parser import CANParser
|
||||
from opendbc.can.packer import CANPacker
|
||||
from selfdrive.boardd.boardd_api_impl import can_list_to_can_capnp
|
||||
from selfdrive.car.honda.values import FINGERPRINTS, CAR
|
||||
from selfdrive.car import crc8_pedal
|
||||
|
||||
from selfdrive.test.longitudinal_maneuvers.plant import get_car_can_parser
|
||||
cp = get_car_can_parser()
|
||||
#cp = CANParser("honda_civic_touring_2016_can_generated")
|
||||
|
||||
packer = CANPacker("honda_civic_touring_2016_can_generated")
|
||||
rpacker = CANPacker("acura_ilx_2016_nidec")
|
||||
|
||||
def can_function(pm, speed, angle, idx, engage):
|
||||
msg = []
|
||||
msg.append(packer.make_can_msg("ENGINE_DATA", 0, {"XMISSION_SPEED": speed}, idx))
|
||||
msg.append(packer.make_can_msg("WHEEL_SPEEDS", 0,
|
||||
{"WHEEL_SPEED_FL": speed,
|
||||
"WHEEL_SPEED_FR": speed,
|
||||
"WHEEL_SPEED_RL": speed,
|
||||
"WHEEL_SPEED_RR": speed}, -1))
|
||||
|
||||
if engage:
|
||||
msg.append(packer.make_can_msg("SCM_BUTTONS", 0, {"CRUISE_BUTTONS": 3}, idx))
|
||||
else:
|
||||
msg.append(packer.make_can_msg("SCM_BUTTONS", 0, {"CRUISE_BUTTONS": 0}, idx))
|
||||
|
||||
values = {"COUNTER_PEDAL": idx&0xF}
|
||||
checksum = crc8_pedal(packer.make_can_msg("GAS_SENSOR", 0, {"COUNTER_PEDAL": idx&0xF}, -1)[2][:-1])
|
||||
values["CHECKSUM_PEDAL"] = checksum
|
||||
msg.append(packer.make_can_msg("GAS_SENSOR", 0, values, -1))
|
||||
|
||||
msg.append(packer.make_can_msg("GEARBOX", 0, {"GEAR": 4, "GEAR_SHIFTER": 8}, idx))
|
||||
msg.append(packer.make_can_msg("GAS_PEDAL_2", 0, {}, idx))
|
||||
msg.append(packer.make_can_msg("SEATBELT_STATUS", 0, {"SEATBELT_DRIVER_LATCHED": 1}, idx))
|
||||
msg.append(packer.make_can_msg("STEER_STATUS", 0, {}, idx))
|
||||
msg.append(packer.make_can_msg("STEERING_SENSORS", 0, {"STEER_ANGLE": angle}, idx))
|
||||
msg.append(packer.make_can_msg("POWERTRAIN_DATA", 0, {}, idx))
|
||||
msg.append(packer.make_can_msg("VSA_STATUS", 0, {}, idx))
|
||||
msg.append(packer.make_can_msg("STANDSTILL", 0, {}, idx))
|
||||
msg.append(packer.make_can_msg("STEER_MOTOR_TORQUE", 0, {}, idx))
|
||||
msg.append(packer.make_can_msg("EPB_STATUS", 0, {}, idx))
|
||||
msg.append(packer.make_can_msg("DOORS_STATUS", 0, {}, idx))
|
||||
msg.append(packer.make_can_msg("CRUISE_PARAMS", 0, {}, idx))
|
||||
msg.append(packer.make_can_msg("CRUISE", 0, {}, idx))
|
||||
msg.append(packer.make_can_msg("SCM_FEEDBACK", 0, {"MAIN_ON": 1}, idx))
|
||||
#print(msg)
|
||||
|
||||
# cam bus
|
||||
msg.append(packer.make_can_msg("STEERING_CONTROL", 2, {}, idx))
|
||||
msg.append(packer.make_can_msg("ACC_HUD", 2, {}, idx))
|
||||
msg.append(packer.make_can_msg("BRAKE_COMMAND", 2, {}, idx))
|
||||
|
||||
# radar
|
||||
if idx%5 == 0:
|
||||
msg.append(rpacker.make_can_msg("RADAR_DIAGNOSTIC", 1, {"RADAR_STATE": 0x79}, -1))
|
||||
for i in range(16):
|
||||
msg.append(rpacker.make_can_msg("TRACK_%d" % i, 1, {"LONG_DIST": 255.5}, -1))
|
||||
|
||||
# fill in the rest for fingerprint
|
||||
done = set([x[0] for x in msg])
|
||||
for k,v in FINGERPRINTS[CAR.CIVIC][0].items():
|
||||
if k not in done and k not in [0xE4, 0x194]:
|
||||
msg.append([k, 0, b'\x00'*v, 0])
|
||||
pm.send('can', can_list_to_can_capnp(msg))
|
||||
|
||||
def sendcan_function(sendcan):
|
||||
sc = messaging.drain_sock_raw(sendcan)
|
||||
cp.update_strings(sc, sendcan=True)
|
||||
|
||||
if cp.vl[0x1fa]['COMPUTER_BRAKE_REQUEST']:
|
||||
brake = cp.vl[0x1fa]['COMPUTER_BRAKE'] * 0.003906248
|
||||
else:
|
||||
brake = 0.0
|
||||
|
||||
if cp.vl[0x200]['GAS_COMMAND'] > 0:
|
||||
gas = cp.vl[0x200]['GAS_COMMAND'] / 256.0
|
||||
else:
|
||||
gas = 0.0
|
||||
|
||||
if cp.vl[0xe4]['STEER_TORQUE_REQUEST']:
|
||||
steer_torque = cp.vl[0xe4]['STEER_TORQUE']*1.0/0x1000
|
||||
else:
|
||||
steer_torque = 0.0
|
||||
|
||||
return (gas, brake, steer_torque)
|
||||
|
||||
if __name__ == "__main__":
|
||||
pm = messaging.PubMaster(['can'])
|
||||
sendcan = messaging.sub_sock('sendcan')
|
||||
idx = 0
|
||||
while 1:
|
||||
sendcan_function(sendcan)
|
||||
can_function(pm, 10.0, idx)
|
||||
time.sleep(0.01)
|
||||
idx += 1
|
||||
|
||||
|
|
@ -0,0 +1,155 @@
|
|||
#!/usr/bin/env python3
|
||||
import os
|
||||
import time
|
||||
import math
|
||||
import atexit
|
||||
import numpy as np
|
||||
import threading
|
||||
import carla
|
||||
import random
|
||||
import cereal.messaging as messaging
|
||||
from common.params import Params
|
||||
from common.realtime import Ratekeeper
|
||||
from can import can_function, sendcan_function
|
||||
import queue
|
||||
|
||||
pm = messaging.PubMaster(['frame', 'sensorEvents', 'can'])
|
||||
|
||||
W,H = 1164, 874
|
||||
|
||||
def cam_callback(image):
|
||||
img = np.frombuffer(image.raw_data, dtype=np.dtype("uint8"))
|
||||
img = np.reshape(img, (H, W, 4))
|
||||
img = img[:, :, [0,1,2]].copy()
|
||||
|
||||
dat = messaging.new_message()
|
||||
dat.init('frame')
|
||||
dat.frame = {
|
||||
"frameId": image.frame,
|
||||
"image": img.tostring(),
|
||||
}
|
||||
pm.send('frame', dat)
|
||||
|
||||
def imu_callback(imu):
|
||||
#print(imu, imu.accelerometer)
|
||||
|
||||
dat = messaging.new_message()
|
||||
dat.init('sensorEvents', 2)
|
||||
dat.sensorEvents[0].sensor = 4
|
||||
dat.sensorEvents[0].type = 0x10
|
||||
dat.sensorEvents[0].init('acceleration')
|
||||
dat.sensorEvents[0].acceleration.v = [imu.accelerometer.x, imu.accelerometer.y, imu.accelerometer.z]
|
||||
# copied these numbers from locationd
|
||||
dat.sensorEvents[1].sensor = 5
|
||||
dat.sensorEvents[1].type = 0x10
|
||||
dat.sensorEvents[1].init('gyroUncalibrated')
|
||||
dat.sensorEvents[1].gyroUncalibrated.v = [imu.gyroscope.x, imu.gyroscope.y, imu.gyroscope.z]
|
||||
pm.send('sensorEvents', dat)
|
||||
|
||||
def health_function():
|
||||
pm = messaging.PubMaster(['health'])
|
||||
rk = Ratekeeper(1.0)
|
||||
while 1:
|
||||
dat = messaging.new_message()
|
||||
dat.init('health')
|
||||
dat.valid = True
|
||||
dat.health = {
|
||||
'ignitionLine': True,
|
||||
'hwType': "whitePanda",
|
||||
'controlsAllowed': True
|
||||
}
|
||||
pm.send('health', dat)
|
||||
rk.keep_time()
|
||||
|
||||
def fake_driver_monitoring():
|
||||
pm = messaging.PubMaster(['driverMonitoring'])
|
||||
while 1:
|
||||
dat = messaging.new_message()
|
||||
dat.init('driverMonitoring')
|
||||
dat.driverMonitoring.faceProb = 1.0
|
||||
pm.send('driverMonitoring', dat)
|
||||
time.sleep(0.1)
|
||||
|
||||
def go():
|
||||
client = carla.Client("127.0.0.1", 2000)
|
||||
client.set_timeout(5.0)
|
||||
world = client.load_world('Town03')
|
||||
|
||||
settings = world.get_settings()
|
||||
settings.fixed_delta_seconds = 0.05
|
||||
world.apply_settings(settings)
|
||||
|
||||
weather = carla.WeatherParameters(
|
||||
cloudyness=0.0,
|
||||
precipitation=0.0,
|
||||
precipitation_deposits=0.0,
|
||||
wind_intensity=0.0,
|
||||
sun_azimuth_angle=0.0,
|
||||
sun_altitude_angle=0.0)
|
||||
world.set_weather(weather)
|
||||
|
||||
blueprint_library = world.get_blueprint_library()
|
||||
"""
|
||||
for blueprint in blueprint_library.filter('sensor.*'):
|
||||
print(blueprint.id)
|
||||
exit(0)
|
||||
"""
|
||||
|
||||
world_map = world.get_map()
|
||||
|
||||
vehicle_bp = random.choice(blueprint_library.filter('vehicle.bmw.*'))
|
||||
vehicle = world.spawn_actor(vehicle_bp, random.choice(world_map.get_spawn_points()))
|
||||
#vehicle.set_autopilot(True)
|
||||
|
||||
blueprint = blueprint_library.find('sensor.camera.rgb')
|
||||
blueprint.set_attribute('image_size_x', str(W))
|
||||
blueprint.set_attribute('image_size_y', str(H))
|
||||
blueprint.set_attribute('fov', '70')
|
||||
blueprint.set_attribute('sensor_tick', '0.05')
|
||||
transform = carla.Transform(carla.Location(x=0.8, z=1.45))
|
||||
camera = world.spawn_actor(blueprint, transform, attach_to=vehicle)
|
||||
camera.listen(cam_callback)
|
||||
|
||||
# TODO: wait for carla 0.9.7
|
||||
imu_bp = blueprint_library.find('sensor.other.imu')
|
||||
imu = world.spawn_actor(imu_bp, transform, attach_to=vehicle)
|
||||
imu.listen(imu_callback)
|
||||
|
||||
def destroy():
|
||||
print("clean exit")
|
||||
imu.destroy()
|
||||
camera.destroy()
|
||||
vehicle.destroy()
|
||||
print("done")
|
||||
atexit.register(destroy)
|
||||
|
||||
threading.Thread(target=health_function).start()
|
||||
threading.Thread(target=fake_driver_monitoring).start()
|
||||
|
||||
# can loop
|
||||
sendcan = messaging.sub_sock('sendcan')
|
||||
rk = Ratekeeper(100)
|
||||
steer_angle = 0
|
||||
while 1:
|
||||
vel = vehicle.get_velocity()
|
||||
speed = math.sqrt(vel.x**2 + vel.y**2 + vel.z**2)
|
||||
|
||||
can_function(pm, speed, steer_angle, rk.frame, rk.frame%500 == 499)
|
||||
if rk.frame%5 == 0:
|
||||
throttle, brake, steer = sendcan_function(sendcan)
|
||||
steer_angle += steer/10000.0 # torque
|
||||
vc = carla.VehicleControl(throttle=throttle, steer=steer_angle, brake=brake)
|
||||
vehicle.apply_control(vc)
|
||||
print(speed, steer_angle, vc)
|
||||
|
||||
rk.keep_time()
|
||||
|
||||
if __name__ == "__main__":
|
||||
params = Params()
|
||||
params.delete("Offroad_ConnectivityNeeded")
|
||||
from selfdrive.version import terms_version, training_version
|
||||
params.put("HasAcceptedTerms", terms_version)
|
||||
params.put("CompletedTrainingVersion", training_version)
|
||||
|
||||
go()
|
||||
|
||||
|
|
@ -0,0 +1,10 @@
|
|||
#!/bin/bash -e
|
||||
FILE=CARLA_0.9.7.tar.gz
|
||||
if [ ! -f $FILE ]; then
|
||||
curl -O http://carla-assets-internal.s3.amazonaws.com/Releases/Linux/$FILE
|
||||
fi
|
||||
mkdir -p carla
|
||||
cd carla
|
||||
tar xvf ../$FILE
|
||||
easy_install PythonAPI/carla/dist/carla-0.9.7-py3.5-linux-x86_64.egg
|
||||
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
#!/bin/bash
|
||||
cd ~/one/tools/nui
|
||||
# vision, boardd, sensorsd, gpsd
|
||||
ALLOW=frame,can,ubloxRaw,health,sensorEvents,gpsNMEA,gpsLocation ./nui "02ec6bea180a4d36/2019-10-25--10-18-09"
|
||||
|
||||
|
|
@ -0,0 +1,4 @@
|
|||
#!/bin/bash
|
||||
cd carla
|
||||
./CarlaUE4.sh
|
||||
|
||||
|
|
@ -0,0 +1,9 @@
|
|||
Host EON-smays
|
||||
HostName 192.168.5.11
|
||||
Port 8022
|
||||
IdentityFile key/id_rsa
|
||||
|
||||
Host EON-wifi
|
||||
HostName 192.168.43.1
|
||||
Port 8022
|
||||
IdentityFile key/id_rsa
|
||||
|
|
@ -0,0 +1,28 @@
|
|||
-----BEGIN PRIVATE KEY-----
|
||||
MIIEvAIBADANBgkqhkiG9w0BAQEFAASCBKYwggSiAgEAAoIBAQC+iXXq30Tq+J5N
|
||||
Kat3KWHCzcmwZ55nGh6WggAqECa5CasBlM9VeROpVu3beA+5h0MibRgbD4DMtVXB
|
||||
t6gEvZ8nd04E7eLA9LTZyFDZ7SkSOVj4oXOQsT0GnJmKrASW5KslTWqVzTfo2XCt
|
||||
Z+004ikLxmyFeBO8NOcErW1pa8gFdQDToH9FrA7kgysic/XVESTOoe7XlzRoe/eZ
|
||||
acEQ+jtnmFd21A4aEADkk00Ahjr0uKaJiLUAPatxs2icIXWpgYtfqqtaKF23wSt6
|
||||
1OTu6cAwXbOWr3m+IUSRUO0IRzEIQS3z1jfd1svgzSgSSwZ1Lhj4AoKxIEAIc8qJ
|
||||
rO4uymCJAgMBAAECggEBAISFevxHGdoL3Z5xkw6oO5SQKO2GxEeVhRzNgmu/HA+q
|
||||
x8OryqD6O1CWY4037kft6iWxlwiLOdwna2P25ueVM3LxqdQH2KS4DmlCx+kq6FwC
|
||||
gv063fQPMhC9LpWimvaQSPEC7VUPjQlo4tPY6sTTYBUOh0A1ihRm/x7juKuQCWix
|
||||
Cq8C/DVnB1X4mGj+W3nJc5TwVJtgJbbiBrq6PWrhvB/3qmkxHRL7dU2SBb2iNRF1
|
||||
LLY30dJx/cD73UDKNHrlrsjk3UJc29Mp4/MladKvUkRqNwlYxSuAtJV0nZ3+iFkL
|
||||
s3adSTHdJpClQer45R51rFDlVsDz2ZBpb/hRNRoGDuECgYEA6A1EixLq7QYOh3cb
|
||||
Xhyh3W4kpVvA/FPfKH1OMy3ONOD/Y9Oa+M/wthW1wSoRL2n+uuIW5OAhTIvIEivj
|
||||
6bAZsTT3twrvOrvYu9rx9aln4p8BhyvdjeW4kS7T8FP5ol6LoOt2sTP3T1LOuJPO
|
||||
uQvOjlKPKIMh3c3RFNWTnGzMPa0CgYEA0jNiPLxP3A2nrX0keKDI+VHuvOY88gdh
|
||||
0W5BuLMLovOIDk9aQFIbBbMuW1OTjHKv9NK+Lrw+YbCFqOGf1dU/UN5gSyE8lX/Q
|
||||
FsUGUqUZx574nJZnOIcy3ONOnQLcvHAQToLFAGUd7PWgP3CtHkt9hEv2koUwL4vo
|
||||
ikTP1u9Gkc0CgYEA2apoWxPZrY963XLKBxNQecYxNbLFaWq67t3rFnKm9E8BAICi
|
||||
4zUaE5J1tMVi7Vi9iks9Ml9SnNyZRQJKfQ+kaebHXbkyAaPmfv+26rqHKboA0uxA
|
||||
nDOZVwXX45zBkp6g1sdHxJx8JLoGEnkC9eyvSi0C//tRLx86OhLErXwYcNkCf1it
|
||||
VMRKrWYoXJTUNo6tRhvodM88UnnIo3u3CALjhgU4uC1RTMHV4ZCGBwiAOb8GozSl
|
||||
s5YD1E1iKwEULloHnK6BIh6P5v8q7J6uf/xdqoKMjlWBHgq6/roxKvkSPA1DOZ3l
|
||||
jTadcgKFnRUmc+JT9p/ZbCxkA/ALFg8++G+0ghECgYA8vG3M/utweLvq4RI7l7U7
|
||||
b+i2BajfK2OmzNi/xugfeLjY6k2tfQGRuv6ppTjehtji2uvgDWkgjJUgPfZpir3I
|
||||
RsVMUiFgloWGHETOy0Qvc5AwtqTJFLTD1Wza2uBilSVIEsg6Y83Gickh+ejOmEsY
|
||||
6co17RFaAZHwGfCFFjO76Q==
|
||||
-----END PRIVATE KEY-----
|
||||
|
|
@ -0,0 +1,3 @@
|
|||
# enp5s0 is the smays network name. Change it appropriately if you are using an ethernet adapter (type ifconfig to get the proper network name)
|
||||
sudo ifconfig enp5s0 192.168.5.1 netmask 255.255.255.0
|
||||
ssh -F config EON-smays
|
||||
|
|
@ -0,0 +1 @@
|
|||
ssh -F config EON-wifi
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 7.0 MiB |
Binary file not shown.
|
After Width: | Height: | Size: 4.4 MiB |
|
|
@ -0,0 +1,93 @@
|
|||
#!/usr/bin/env python
|
||||
import os
|
||||
import sys
|
||||
import argparse
|
||||
import zmq
|
||||
import json
|
||||
import cv2
|
||||
import numpy as np
|
||||
from hexdump import hexdump
|
||||
import scipy.misc
|
||||
import struct
|
||||
from collections import deque
|
||||
|
||||
# sudo pip install git+git://github.com/mikeboers/PyAV.git
|
||||
import av
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from cereal.services import service_list
|
||||
|
||||
PYGAME = os.getenv("PYGAME") is not None
|
||||
if PYGAME:
|
||||
import pygame
|
||||
imgff = np.zeros((874, 1164, 3), dtype=np.uint8)
|
||||
|
||||
# first 74 bytes in any stream
|
||||
start = "0000000140010c01ffff016000000300b0000003000003005dac5900000001420101016000000300b0000003000003005da0025080381c5c665aee4c92ec80000000014401c0f1800420"
|
||||
|
||||
def receiver_thread():
|
||||
if PYGAME:
|
||||
pygame.init()
|
||||
pygame.display.set_caption("vnet debug UI")
|
||||
screen = pygame.display.set_mode((1164,874), pygame.DOUBLEBUF)
|
||||
camera_surface = pygame.surface.Surface((1164,874), 0, 24).convert()
|
||||
|
||||
addr = "192.168.5.11"
|
||||
if len(sys.argv) >= 2:
|
||||
addr = sys.argv[1]
|
||||
|
||||
context = zmq.Context()
|
||||
s = messaging.sub_sock(context, 9002, addr=addr)
|
||||
frame_sock = messaging.pub_sock(context, service_list['frame'].port)
|
||||
|
||||
ctx = av.codec.codec.Codec('hevc', 'r').create()
|
||||
ctx.decode(av.packet.Packet(start.decode("hex")))
|
||||
|
||||
import time
|
||||
while 1:
|
||||
t1 = time.time()
|
||||
ts, raw = s.recv_multipart()
|
||||
ts = struct.unpack('q', ts)[0] * 1000
|
||||
t1, t2 = time.time(), t1
|
||||
#print 'ms to get frame:', (t1-t2)*1000
|
||||
|
||||
pkt = av.packet.Packet(raw)
|
||||
f = ctx.decode(pkt)
|
||||
if not f:
|
||||
continue
|
||||
f = f[0]
|
||||
t1, t2 = time.time(), t1
|
||||
#print 'ms to decode:', (t1-t2)*1000
|
||||
|
||||
y_plane = np.frombuffer(f.planes[0], np.uint8).reshape((874, 1216))[:, 0:1164]
|
||||
u_plane = np.frombuffer(f.planes[1], np.uint8).reshape((437, 608))[:, 0:582]
|
||||
v_plane = np.frombuffer(f.planes[2], np.uint8).reshape((437, 608))[:, 0:582]
|
||||
yuv_img = y_plane.tobytes() + u_plane.tobytes() + v_plane.tobytes()
|
||||
t1, t2 = time.time(), t1
|
||||
#print 'ms to make yuv:', (t1-t2)*1000
|
||||
#print 'tsEof:', ts
|
||||
|
||||
dat = messaging.new_message()
|
||||
dat.init('frame')
|
||||
dat.frame.image = yuv_img
|
||||
dat.frame.timestampEof = ts
|
||||
dat.frame.transform = map(float, list(np.eye(3).flatten()))
|
||||
frame_sock.send(dat.to_bytes())
|
||||
|
||||
if PYGAME:
|
||||
yuv_np = np.frombuffer(yuv_img, dtype=np.uint8).reshape(874 * 3 // 2, -1)
|
||||
cv2.cvtColor(yuv_np, cv2.COLOR_YUV2RGB_I420, dst=imgff)
|
||||
#print yuv_np.shape, imgff.shape
|
||||
|
||||
#scipy.misc.imsave("tmp.png", imgff)
|
||||
|
||||
pygame.surfarray.blit_array(camera_surface, imgff.swapaxes(0,1))
|
||||
screen.blit(camera_surface, (0, 0))
|
||||
pygame.display.flip()
|
||||
|
||||
|
||||
def main(gctx=None):
|
||||
receiver_thread()
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Loading…
Reference in New Issue