def _get_libmpc(mpc_id):
libmpc_fn = os.path.join(mpc_dir, "libmpc%d%s" % (mpc_id, suffix()))
ffi = FFI()
ffi.cdef("""
typedef struct {
double x_ego, v_ego, a_ego, x_l, v_l, a_l;
} state_t;
typedef struct {
double x_ego[21];
double v_ego[21];
double a_ego[21];
double j_ego[20];
double x_l[21];
double v_l[21];
double a_l[21];
double t[21];
double cost;
} log_t;
void init(double ttcCost, double distanceCost, double accelerationCost, double jerkCost);
void init_with_simulation(double v_ego, double x_l, double v_l, double a_l, double l);
void change_costs(double ttcCost, double distanceCost, double accelerationCost, double jerkCost);
int run_mpc(state_t * x0, log_t * solution,
double l, double a_l_0, double TR);
""")
return (ffi, ffi.dlopen(libmpc_fn))
def get_ffi():
lib = os.path.join(BASEDIR, "selfdrive", "ui", "qt",
"libpython_helpers" + suffix())
ffi = FFI()
ffi.cdef("void set_main_window(void *w);")
return ffi, ffi.dlopen(lib)
def get_ffi():
lib = os.path.join(BASEDIR, "selfdrive", "ui", "navd",
"libmap_renderer" + suffix())
ffi = FFI()
ffi.cdef("""
void* map_renderer_init();
void map_renderer_update_position(void *inst, float lat, float lon, float bearing);
void map_renderer_update_route(void *inst, char *polyline);
void map_renderer_update(void *inst);
void map_renderer_process(void *inst);
bool map_renderer_loaded(void *inst);
uint8_t* map_renderer_get_image(void *inst);
void map_renderer_free_image(void *inst, uint8_t *buf);
""")
return ffi, ffi.dlopen(lib)
import os
from cffi import FFI
from common.ffi_wrapper import suffix
mpc_dir = os.path.dirname(os.path.abspath(__file__))
libmpc_fn = os.path.join(mpc_dir, "libmpc" + suffix())
ffi = FFI()
ffi.cdef("""
typedef struct {
double x, y, psi, curvature, curvature_rate;
} state_t;
int N = 16;
typedef struct {
double x[N+1];
double y[N+1];
double psi[N+1];
double curvature[N+1];
double curvature_rate[N];
double cost;
} log_t;
void init();
void set_weights(double pathCost, double headingCost, double steerRateCost);
int run_mpc(state_t * x0, log_t * solution,
double v_ego, double rotation_radius,
double target_y[N+1], double target_psi[N+1]);
""")
import os
import numpy as np
from cffi import FFI
from common.ffi_wrapper import suffix
cluster_dir = os.path.join(os.path.dirname(os.path.abspath(__file__)))
cluster_fn = os.path.join(cluster_dir, "libfastcluster" + suffix())
ffi = FFI()
ffi.cdef("""
int hclust_fast(int n, double* distmat, int method, int* merge, double* height);
void cutree_cdist(int n, const int* merge, double* height, double cdist, int* labels);
void hclust_pdist(int n, int m, double* pts, double* out);
void cluster_points_centroid(int n, int m, double* pts, double dist, int* idx);
""")
hclust = ffi.dlopen(cluster_fn)
def cluster_points_centroid(pts, dist):
pts = np.ascontiguousarray(pts, dtype=np.float64)
pts_ptr = ffi.cast("double *", pts.ctypes.data)
n, m = pts.shape
labels_ptr = ffi.new("int[]", n)
hclust.cluster_points_centroid(n, m, pts_ptr, dist**2, labels_ptr)
return list(labels_ptr)
