Vehicle Researcher
8 years ago
parent
9d3963559a
commit
99cb610b12
29 changed files with 558 additions and 179 deletions
@ -1 +1 @@ |
|||||||
Subproject commit b77861eb00d45e25af501f78bbed155d2df06159 |
Subproject commit 008089045e371a9eebbe79d978ff22ae3e70bdba |
||||||
@ -1 +1 @@ |
|||||||
Subproject commit 5409c51041cfe8f139650a4e0decf4f6d863eb07 |
Subproject commit ff8a4bd4e844c2b0a0dd7efb321b31781d8034c8 |
||||||
@ -1 +1 @@ |
|||||||
#define OPENPILOT_VERSION "0.3.5" |
#define OPENPILOT_VERSION "0.3.6" |
||||||
|
|||||||
@ -0,0 +1,87 @@ |
|||||||
|
#!/usr/bin/env python |
||||||
|
import numpy as np |
||||||
|
from numpy.linalg import inv |
||||||
|
from selfdrive.car.honda.interface import CarInterface |
||||||
|
|
||||||
|
## dynamic bycicle model from "The Science of Vehicle Dynamics (2014), M. Guiggiani"## |
||||||
|
|
||||||
|
# Xdot = A*X + B*U |
||||||
|
# where X = [v, r], with v and r lateral speed and rotational speed, respectively |
||||||
|
# and U is the steering angle (controller input) |
||||||
|
# |
||||||
|
# A depends on longitudinal speed, u, and vehicle parameters CP |
||||||
|
|
||||||
|
def create_dyn_state_matrices(u, CP): |
||||||
|
A = np.zeros((2,2)) |
||||||
|
B = np.zeros((2,1)) |
||||||
|
A[0,0] = - (CP.cF + CP.cR)/(CP.m*u) |
||||||
|
A[0,1] = - (CP.cF*CP.aF - CP.cR*CP.aR) / (CP.m*u) - u |
||||||
|
A[1,0] = - (CP.cF*CP.aF - CP.cR*CP.aR) / (CP.j*u) |
||||||
|
A[1,1] = - (CP.cF*CP.aF**2 + CP.cR*CP.aR**2) / (CP.j*u) |
||||||
|
B[0,0] = (CP.cF + CP.chi*CP.cR) / CP.m / CP.sR |
||||||
|
B[1,0] = (CP.cF*CP.aF - CP.chi*CP.cR*CP.aR) / CP.j / CP.sR |
||||||
|
return A, B |
||||||
|
|
||||||
|
|
||||||
|
def kin_ss_sol(sa, u, CP): |
||||||
|
# kinematic solution, useful when speed ~ 0 |
||||||
|
K = np.zeros((2,1)) |
||||||
|
K[0,0] = CP.aR / CP.sR / CP.l * u |
||||||
|
K[1,0] = 1. / CP.sR / CP.l * u |
||||||
|
return K * sa |
||||||
|
|
||||||
|
|
||||||
|
def dyn_ss_sol(sa, u, CP): |
||||||
|
# Dynamic solution, useful when speed > 0 |
||||||
|
A, B = create_dyn_state_matrices(u, CP) |
||||||
|
return - np.matmul(inv(A), B) * sa |
||||||
|
|
||||||
|
def calc_slip_factor(CP): |
||||||
|
# the slip factor is a measure of how the curvature changes with speed |
||||||
|
# it's positive for Oversteering vehicle, negative (usual case) otherwise |
||||||
|
return CP.m * (CP.cF * CP.aF - CP.cR * CP.aR) / (CP.l**2 * CP.cF * CP.cR) |
||||||
|
|
||||||
|
class VehicleModel(object): |
||||||
|
def __init__(self, CP, init_state=np.asarray([[0.],[0.]])): |
||||||
|
self.dt = 0.1 |
||||||
|
lookahead = 2. # s |
||||||
|
self.steps = int(lookahead / self.dt) |
||||||
|
self.update_state(init_state) |
||||||
|
self.state_pred = np.zeros((self.steps, self.state.shape[0])) |
||||||
|
self.CP = CP |
||||||
|
|
||||||
|
def update_state(self, state): |
||||||
|
self.state = state |
||||||
|
|
||||||
|
def steady_state_sol(self, sa, u): |
||||||
|
# if the speed is too small we can't use the dynamic model |
||||||
|
# (tire slip is undefined), we then use the kinematic model |
||||||
|
if u > 0.1: |
||||||
|
return dyn_ss_sol(sa, u, self.CP) |
||||||
|
else: |
||||||
|
return kin_ss_sol(sa, u, self.CP) |
||||||
|
|
||||||
|
def calc_curvature(self, sa, u): |
||||||
|
# this formula can be derived from state equations in steady state conditions |
||||||
|
sf = calc_slip_factor(self.CP) |
||||||
|
return (1. - self.CP.chi)/(1. - sf * u**2) * sa / self.CP.sR / self.CP.l |
||||||
|
|
||||||
|
def get_steer_from_curvature(self, curv, u): |
||||||
|
# this function is the exact inverse of calc_curvature, returning steer angle given curvature |
||||||
|
sf = calc_slip_factor(self.CP) |
||||||
|
return self.CP.l * self.CP.sR * (1. - sf * u**2) / (1. - self.CP.chi) * curv |
||||||
|
|
||||||
|
def state_prediction(self, sa, u): |
||||||
|
# U is the matrix of the controls |
||||||
|
# u is the long speed |
||||||
|
A, B = create_dyn_state_matrices(u, self.CP) |
||||||
|
return np.matmul((A * self.dt + np.identity(2)), self.state) + B * sa * self.dt |
||||||
|
|
||||||
|
|
||||||
|
if __name__ == '__main__': |
||||||
|
# load car params |
||||||
|
CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING", {}) |
||||||
|
print CP |
||||||
|
VM = VehicleModel(CP) |
||||||
|
print VM.steady_state_sol(.1, 0.15) |
||||||
|
|
||||||
@ -0,0 +1,26 @@ |
|||||||
|
CC = clang
|
||||||
|
CXX = clang++
|
||||||
|
|
||||||
|
WARN_FLAGS = -Werror=implicit-function-declaration \
|
||||||
|
-Werror=incompatible-pointer-types \
|
||||||
|
-Werror=int-conversion \
|
||||||
|
-Werror=return-type \
|
||||||
|
-Werror=format-extra-args
|
||||||
|
|
||||||
|
CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS)
|
||||||
|
|
||||||
|
.PHONY: all |
||||||
|
all: libvisionipc.so |
||||||
|
|
||||||
|
visionipc.o: ../../common/visionipc.c ../../common/visionipc.h |
||||||
|
@echo "[ CC ] $@"
|
||||||
|
$(CC) $(CFLAGS) -MMD \
|
||||||
|
-I../.. -I../../.. \
|
||||||
|
-c -o '$@' ../../common/visionipc.c
|
||||||
|
|
||||||
|
libvisionipc.so: visionipc.o |
||||||
|
$(CC) -shared -fPIC -o '$@' visionipc.o
|
||||||
|
|
||||||
|
.PHONY: clean |
||||||
|
clean: |
||||||
|
rm visionipc.o libvisionipc.so
|
||||||
@ -0,0 +1,93 @@ |
|||||||
|
#!/usr/bin/env python |
||||||
|
import os |
||||||
|
import time |
||||||
|
import subprocess |
||||||
|
from cffi import FFI |
||||||
|
import ctypes |
||||||
|
|
||||||
|
import numpy as np |
||||||
|
|
||||||
|
gf_dir = os.path.dirname(os.path.abspath(__file__)) |
||||||
|
|
||||||
|
subprocess.check_call(["make"], cwd=gf_dir) |
||||||
|
|
||||||
|
|
||||||
|
ffi = FFI() |
||||||
|
ffi.cdef(""" |
||||||
|
|
||||||
|
typedef enum VisionStreamType { |
||||||
|
VISION_STREAM_UI_BACK, |
||||||
|
VISION_STREAM_UI_FRONT, |
||||||
|
VISION_STREAM_YUV, |
||||||
|
VISION_STREAM_MAX, |
||||||
|
} VisionStreamType; |
||||||
|
|
||||||
|
typedef struct VisionUIInfo { |
||||||
|
int big_box_x, big_box_y; |
||||||
|
int big_box_width, big_box_height; |
||||||
|
int transformed_width, transformed_height; |
||||||
|
|
||||||
|
int front_box_x, front_box_y; |
||||||
|
int front_box_width, front_box_height; |
||||||
|
} VisionUIInfo; |
||||||
|
|
||||||
|
typedef struct VisionStreamBufs { |
||||||
|
VisionStreamType type; |
||||||
|
|
||||||
|
int width, height, stride; |
||||||
|
size_t buf_len; |
||||||
|
|
||||||
|
union { |
||||||
|
VisionUIInfo ui_info; |
||||||
|
} buf_info; |
||||||
|
} VisionStreamBufs; |
||||||
|
|
||||||
|
typedef struct VisionBuf { |
||||||
|
int fd; |
||||||
|
size_t len; |
||||||
|
void* addr; |
||||||
|
} VisionBuf; |
||||||
|
|
||||||
|
typedef struct VisionBufExtra { |
||||||
|
uint32_t frame_id; // only for yuv |
||||||
|
} VisionBufExtra; |
||||||
|
|
||||||
|
typedef struct VisionStream { |
||||||
|
int ipc_fd; |
||||||
|
int last_idx; |
||||||
|
int num_bufs; |
||||||
|
VisionStreamBufs bufs_info; |
||||||
|
VisionBuf *bufs; |
||||||
|
} VisionStream; |
||||||
|
|
||||||
|
int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, VisionStreamBufs *out_bufs_info); |
||||||
|
VisionBuf* visionstream_get(VisionStream *s, VisionBufExtra *out_extra); |
||||||
|
void visionstream_destroy(VisionStream *s); |
||||||
|
|
||||||
|
""" |
||||||
|
) |
||||||
|
|
||||||
|
clib = ffi.dlopen(os.path.join(gf_dir, "libvisionipc.so")) |
||||||
|
|
||||||
|
|
||||||
|
def getframes(): |
||||||
|
s = ffi.new("VisionStream*") |
||||||
|
buf_info = ffi.new("VisionStreamBufs*") |
||||||
|
err = clib.visionstream_init(s, clib.VISION_STREAM_UI_BACK, True, buf_info) |
||||||
|
assert err == 0 |
||||||
|
|
||||||
|
w = buf_info.width |
||||||
|
h = buf_info.height |
||||||
|
assert buf_info.stride == w*3 |
||||||
|
assert buf_info.buf_len == w*h*3 |
||||||
|
|
||||||
|
while True: |
||||||
|
buf = clib.visionstream_get(s, ffi.NULL) |
||||||
|
|
||||||
|
pbuf = ffi.buffer(buf.addr, buf.len) |
||||||
|
yield np.frombuffer(pbuf, dtype=np.uint8).reshape((h, w, 3)) |
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__": |
||||||
|
for buf in getframes(): |
||||||
|
print buf.shape, buf[101, 101] |
||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Loading…
Reference in new issue