dragonpilot beta3

date: 2023-10-09T10:55:55
commit: 91b6e3aecd7170f24bccacb10c515ec281c30295

selfdrive/modeld/models/commonmodel.pxd

@@ -0,0 +1,20 @@
+# distutils: language = c++
+
+from cereal.visionipc.visionipc cimport cl_device_id, cl_context, cl_mem
+
+cdef extern from "common/mat.h":
+  cdef struct mat3:
+    float v[9]
+
+cdef extern from "common/clutil.h":
+  cdef unsigned long CL_DEVICE_TYPE_DEFAULT
+  cl_device_id cl_get_device_id(unsigned long)
+  cl_context cl_create_context(cl_device_id)
+
+cdef extern from "selfdrive/modeld/models/commonmodel.h":
+  float sigmoid(float)
+
+  cppclass ModelFrame:
+    int buf_size
+    ModelFrame(cl_device_id, cl_context)
+    float * prepare(cl_mem, int, int, int, int, mat3, cl_mem*)

selfdrive/modeld/models/commonmodel_pyx.pxd

@@ -0,0 +1,13 @@
+# distutils: language = c++
+
+from cereal.visionipc.visionipc cimport cl_mem
+from cereal.visionipc.visionipc_pyx cimport CLContext as BaseCLContext
+
+cdef class CLContext(BaseCLContext):
+  pass
+
+cdef class CLMem:
+  cdef cl_mem * mem;
+
+  @staticmethod
+  cdef create(void*)

selfdrive/modeld/models/commonmodel_pyx.pyx

@@ -0,0 +1,43 @@
+# distutils: language = c++
+# cython: c_string_encoding=ascii
+
+import numpy as np
+cimport numpy as cnp
+from libc.string cimport memcpy
+
+from cereal.visionipc.visionipc cimport cl_mem
+from cereal.visionipc.visionipc_pyx cimport VisionBuf, CLContext as BaseCLContext
+from .commonmodel cimport CL_DEVICE_TYPE_DEFAULT, cl_get_device_id, cl_create_context
+from .commonmodel cimport mat3, sigmoid as cppSigmoid, ModelFrame as cppModelFrame
+
+def sigmoid(x):
+  return cppSigmoid(x)
+
+cdef class CLContext(BaseCLContext):
+  def __cinit__(self):
+    self.device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT)
+    self.context = cl_create_context(self.device_id)
+
+cdef class CLMem:
+  @staticmethod
+  cdef create(void * cmem):
+    mem = CLMem()
+    mem.mem = <cl_mem*> cmem
+    return mem
+
+cdef class ModelFrame:
+  cdef cppModelFrame * frame
+
+  def __cinit__(self, CLContext context):
+    self.frame = new cppModelFrame(context.device_id, context.context)
+
+  def __dealloc__(self):
+    del self.frame
+
+  def prepare(self, VisionBuf buf, float[:] projection, CLMem output):
+    cdef mat3 cprojection
+    memcpy(cprojection.v, &projection[0], 9*sizeof(float))
+    cdef float * data = self.frame.prepare(buf.buf.buf_cl, buf.width, buf.height, buf.stride, buf.uv_offset, cprojection, output.mem)
+    if not data:
+      return None
+    return np.asarray(<cnp.float32_t[:self.frame.buf_size]> data)

selfdrive/modeld/models/dmonitoring.h

@@ -1,50 +0,0 @@
-#pragma once
-
-#include <vector>
-
-#include "cereal/messaging/messaging.h"
-#include "common/util.h"
-#include "selfdrive/modeld/models/commonmodel.h"
-#include "selfdrive/modeld/runners/run.h"
-
-#define CALIB_LEN 3
-
-#define OUTPUT_SIZE 84
-#define REG_SCALE 0.25f
-
-typedef struct DriverStateResult {
-  float face_orientation[3];
-  float face_orientation_std[3];
-  float face_position[2];
-  float face_position_std[2];
-  float face_prob;
-  float left_eye_prob;
-  float right_eye_prob;
-  float left_blink_prob;
-  float right_blink_prob;
-  float sunglasses_prob;
-  float occluded_prob;
-  float ready_prob[4];
-  float not_ready_prob[2];
-} DriverStateResult;
-
-typedef struct DMonitoringModelResult {
-  DriverStateResult driver_state_lhd;
-  DriverStateResult driver_state_rhd;
-  float poor_vision_prob;
-  float wheel_on_right_prob;
-  float dsp_execution_time;
-} DMonitoringModelResult;
-
-typedef struct DMonitoringModelState {
-  RunModel *m;
-  float output[OUTPUT_SIZE];
-  std::vector<uint8_t> net_input_buf;
-  float calib[CALIB_LEN];
-} DMonitoringModelState;
-
-void dmonitoring_init(DMonitoringModelState* s);
-DMonitoringModelResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, int stride, int uv_offset, float *calib);
-void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringModelResult &model_res, float execution_time, kj::ArrayPtr<const float> raw_pred);
-void dmonitoring_free(DMonitoringModelState* s);
-

selfdrive/modeld/models/driving.h

@@ -4,25 +4,18 @@
 #include <memory>
 
 #include "cereal/messaging/messaging.h"
-#include "cereal/visionipc/visionipc_client.h"
-#include "common/mat.h"
 #include "common/modeldata.h"
 #include "common/util.h"
 #include "selfdrive/modeld/models/commonmodel.h"
-#include "selfdrive/modeld/models/nav.h"
 #include "selfdrive/modeld/runners/run.h"
 
-// gate this here
-#define TEMPORAL
-#define DESIRE
-#define TRAFFIC_CONVENTION
-#define NAV
-
-constexpr int FEATURE_LEN = 128;
+constexpr int FEATURE_LEN = 512;
 constexpr int HISTORY_BUFFER_LEN = 99;
 constexpr int DESIRE_LEN = 8;
 constexpr int DESIRE_PRED_LEN = 4;
 constexpr int TRAFFIC_CONVENTION_LEN = 2;
+constexpr int NAV_FEATURE_LEN = 256;
+constexpr int NAV_INSTRUCTION_LEN = 150;
 constexpr int DRIVING_STYLE_LEN = 12;
 constexpr int MODEL_FREQ = 20;
 
@@ -31,10 +24,8 @@ constexpr int BLINKER_LEN = 6;
 constexpr int META_STRIDE = 7;
 
 constexpr int PLAN_MHP_N = 5;
-
 constexpr int LEAD_MHP_N = 2;
 constexpr int LEAD_TRAJ_LEN = 6;
-constexpr int LEAD_PRED_DIM = 4;
 constexpr int LEAD_MHP_SELECTION = 3;
 // Padding to get output shape as multiple of 4
 constexpr int PAD_SIZE = 2;
@@ -253,49 +244,14 @@ struct ModelOutput {
 };
 
 constexpr int OUTPUT_SIZE = sizeof(ModelOutput) / sizeof(float);
-
-#ifdef TEMPORAL
-  constexpr int TEMPORAL_SIZE = HISTORY_BUFFER_LEN * FEATURE_LEN;
-#else
-  constexpr int TEMPORAL_SIZE = 0;
-#endif
 constexpr int NET_OUTPUT_SIZE = OUTPUT_SIZE + FEATURE_LEN + PAD_SIZE;
 
-// TODO: convert remaining arrays to std::array and update model runners
-struct ModelState {
-  ModelFrame *frame = nullptr;
-  ModelFrame *wide_frame = nullptr;
-  std::array<float, HISTORY_BUFFER_LEN * FEATURE_LEN> feature_buffer = {};
-  std::array<float, NET_OUTPUT_SIZE> output = {};
-  std::unique_ptr<RunModel> m;
-#ifdef DESIRE
-  float prev_desire[DESIRE_LEN] = {};
-  float pulse_desire[DESIRE_LEN*(HISTORY_BUFFER_LEN+1)] = {};
-#endif
-#ifdef TRAFFIC_CONVENTION
-  float traffic_convention[TRAFFIC_CONVENTION_LEN] = {};
-#endif
-#ifdef DRIVING_STYLE
-  float driving_style[DRIVING_STYLE_LEN] = {};
-#endif
-#ifdef NAV
-  float nav_features[NAV_FEATURE_LEN] = {};
-  float nav_instructions[NAV_INSTRUCTION_LEN] = {};
-#endif
-};
-
 struct PublishState {
   std::array<float, DISENGAGE_LEN * DISENGAGE_LEN> disengage_buffer = {};
   std::array<float, 5> prev_brake_5ms2_probs = {};
   std::array<float, 3> prev_brake_3ms2_probs = {};
 };
 
-void model_init(ModelState* s, cl_device_id device_id, cl_context context);
-ModelOutput *model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* buf_wide, const mat3 &transform, const mat3 &transform_wide,
-                              float *desire_in, bool is_rhd, float *driving_style, float *nav_features, float *nav_instructions, bool prepare_only);
-void model_free(ModelState* s);
-void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop,
-                   const ModelOutput &net_outputs, ModelState &s, PublishState &ps, uint64_t timestamp_eof, uint64_t timestamp_llk,
-                   float model_execution_time, const bool nav_enabled, const bool valid);
-void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames,
-                     const ModelOutput &net_outputs, uint64_t timestamp_eof, const bool valid);
+void fill_model_msg(MessageBuilder &msg, float *net_output_data, PublishState &ps, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop,
+                    uint64_t timestamp_eof, uint64_t timestamp_llk, float model_execution_time, const bool nav_enabled, const bool valid);
+void fill_pose_msg(MessageBuilder &msg, float *net_outputs, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames, uint64_t timestamp_eof, const bool valid);

selfdrive/modeld/models/driving.pxd

@@ -0,0 +1,25 @@
+# distutils: language = c++
+
+from libcpp cimport bool
+from libc.stdint cimport uint32_t, uint64_t
+
+cdef extern from "cereal/messaging/messaging.h":
+  cdef cppclass MessageBuilder:
+    size_t getSerializedSize()
+    int serializeToBuffer(unsigned char *, size_t)
+
+cdef extern from "selfdrive/modeld/models/driving.h":
+  cdef int FEATURE_LEN
+  cdef int HISTORY_BUFFER_LEN
+  cdef int DESIRE_LEN
+  cdef int TRAFFIC_CONVENTION_LEN
+  cdef int DRIVING_STYLE_LEN
+  cdef int NAV_FEATURE_LEN
+  cdef int NAV_INSTRUCTION_LEN
+  cdef int OUTPUT_SIZE
+  cdef int NET_OUTPUT_SIZE
+  cdef int MODEL_FREQ
+  cdef struct PublishState: pass
+
+  void fill_model_msg(MessageBuilder, float *, PublishState, uint32_t, uint32_t, uint32_t, float, uint64_t, uint64_t, float, bool, bool)
+  void fill_pose_msg(MessageBuilder, float *, uint32_t, uint32_t, uint64_t, bool)

selfdrive/modeld/models/driving_pyx.pyx

@@ -0,0 +1,52 @@
+# distutils: language = c++
+# cython: c_string_encoding=ascii
+
+import numpy as np
+cimport numpy as cnp
+from libcpp cimport bool
+from libc.string cimport memcpy
+from libc.stdint cimport uint32_t, uint64_t
+
+from .commonmodel cimport mat3
+from .driving cimport FEATURE_LEN as CPP_FEATURE_LEN, HISTORY_BUFFER_LEN as CPP_HISTORY_BUFFER_LEN, DESIRE_LEN as CPP_DESIRE_LEN, \
+                      TRAFFIC_CONVENTION_LEN as CPP_TRAFFIC_CONVENTION_LEN, DRIVING_STYLE_LEN as CPP_DRIVING_STYLE_LEN, \
+                      NAV_FEATURE_LEN as CPP_NAV_FEATURE_LEN, NAV_INSTRUCTION_LEN as CPP_NAV_INSTRUCTION_LEN, \
+                      OUTPUT_SIZE as CPP_OUTPUT_SIZE, NET_OUTPUT_SIZE as CPP_NET_OUTPUT_SIZE, MODEL_FREQ as CPP_MODEL_FREQ
+from .driving cimport MessageBuilder, PublishState as cppPublishState
+from .driving cimport fill_model_msg, fill_pose_msg
+
+FEATURE_LEN = CPP_FEATURE_LEN
+HISTORY_BUFFER_LEN = CPP_HISTORY_BUFFER_LEN
+DESIRE_LEN = CPP_DESIRE_LEN
+TRAFFIC_CONVENTION_LEN = CPP_TRAFFIC_CONVENTION_LEN
+DRIVING_STYLE_LEN = CPP_DRIVING_STYLE_LEN
+NAV_FEATURE_LEN = CPP_NAV_FEATURE_LEN
+NAV_INSTRUCTION_LEN = CPP_NAV_INSTRUCTION_LEN
+OUTPUT_SIZE = CPP_OUTPUT_SIZE
+NET_OUTPUT_SIZE = CPP_NET_OUTPUT_SIZE
+MODEL_FREQ = CPP_MODEL_FREQ
+
+cdef class PublishState:
+  cdef cppPublishState state
+
+def create_model_msg(float[:] model_outputs, PublishState ps, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop,
+                     uint64_t timestamp_eof, uint64_t timestamp_llk, float model_execution_time, bool nav_enabled, bool valid):
+  cdef MessageBuilder msg
+  fill_model_msg(msg, &model_outputs[0], ps.state, vipc_frame_id, vipc_frame_id_extra, frame_id, frame_drop,
+                 timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, valid)
+
+  output_size = msg.getSerializedSize()
+  output_data = bytearray(output_size)
+  cdef unsigned char * output_ptr = output_data
+  assert msg.serializeToBuffer(output_ptr, output_size) > 0, "output buffer is too small to serialize"
+  return bytes(output_data)
+
+def create_pose_msg(float[:] model_outputs, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames, uint64_t timestamp_eof, bool valid):
+  cdef MessageBuilder msg
+  fill_pose_msg(msg, &model_outputs[0], vipc_frame_id, vipc_dropped_frames, timestamp_eof, valid)
+
+  output_size = msg.getSerializedSize()
+  output_data = bytearray(output_size)
+  cdef unsigned char * output_ptr = output_data
+  assert msg.serializeToBuffer(output_ptr, output_size) > 0, "output buffer is too small to serialize"
+  return bytes(output_data)

selfdrive/modeld/models/nav.h

@@ -1,57 +0,0 @@
-#pragma once
-
-#include "cereal/messaging/messaging.h"
-#include "cereal/visionipc/visionipc_client.h"
-#include "common/util.h"
-#include "common/modeldata.h"
-#include "selfdrive/modeld/models/commonmodel.h"
-#include "selfdrive/modeld/runners/run.h"
-
-constexpr int NAV_INPUT_SIZE = 256*256;
-constexpr int NAV_FEATURE_LEN = 256;
-constexpr int NAV_INSTRUCTION_LEN = 150;
-constexpr int NAV_DESIRE_LEN = 32;
-
-struct NavModelOutputXY {
-  float x;
-  float y;
-};
-static_assert(sizeof(NavModelOutputXY) == sizeof(float)*2);
-
-struct NavModelOutputPlan {
-  std::array<NavModelOutputXY, TRAJECTORY_SIZE> mean;
-  std::array<NavModelOutputXY, TRAJECTORY_SIZE> std;
-};
-static_assert(sizeof(NavModelOutputPlan) == sizeof(NavModelOutputXY)*TRAJECTORY_SIZE*2);
-
-struct NavModelOutputDesirePrediction {
-  std::array<float, NAV_DESIRE_LEN> values;
-};
-static_assert(sizeof(NavModelOutputDesirePrediction) == sizeof(float)*NAV_DESIRE_LEN);
-
-struct NavModelOutputFeatures {
-  std::array<float, NAV_FEATURE_LEN> values;
-};
-static_assert(sizeof(NavModelOutputFeatures) == sizeof(float)*NAV_FEATURE_LEN);
-
-struct NavModelResult {
-  const NavModelOutputPlan plan;
-  const NavModelOutputDesirePrediction desire_pred;
-  const NavModelOutputFeatures features;
-  float dsp_execution_time;
-};
-static_assert(sizeof(NavModelResult) == sizeof(NavModelOutputPlan) + sizeof(NavModelOutputDesirePrediction) + sizeof(NavModelOutputFeatures) + sizeof(float));
-
-constexpr int NAV_OUTPUT_SIZE = sizeof(NavModelResult) / sizeof(float);
-constexpr int NAV_NET_OUTPUT_SIZE = NAV_OUTPUT_SIZE - 1;
-
-struct NavModelState {
-  RunModel *m;
-  uint8_t net_input_buf[NAV_INPUT_SIZE];
-  float output[NAV_OUTPUT_SIZE];
-};
-
-void navmodel_init(NavModelState* s);
-NavModelResult* navmodel_eval_frame(NavModelState* s, VisionBuf* buf);
-void navmodel_publish(PubMaster &pm, VisionIpcBufExtra &extra, const NavModelResult &model_res, float execution_time, bool route_valid);
-void navmodel_free(NavModelState* s);