George Hotz
5 years ago
parent
80d6953862
commit
29ac3da7b8
79 changed files with 7577 additions and 0 deletions
@ -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,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 |
||||||
|
After Width: | Height: | Size: 7.0 MiB |
|
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