Smart torque

cereal/log.capnp

@@ -745,6 +745,8 @@ struct PathPlan {
   desire @17 :Desire;
   laneChangeState @18 :LaneChangeState;
   laneChangeDirection @19 :LaneChangeDirection;
+  deltaDesired @20 :Float32;
+  angleOffsetLive @21 :Float32;
 
   enum Desire {
     none @0;

selfdrive/car/toyota/carcontroller.py

@@ -6,6 +6,10 @@
                                            create_acc_cancel_command, create_fcw_command
 from selfdrive.car.toyota.values import CAR, ECU, STATIC_MSGS, SteerLimitParams
 from opendbc.can.packer import CANPacker
+import math
+from selfdrive.smart_torque import st_wrapper
+import numpy as np
+import cereal.messaging as messaging
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 
@@ -107,13 +111,50 @@ def __init__(self, dbc_name, car_fingerprint, enable_camera, enable_dsu, enable_
 
     self.packer = CANPacker(dbc_name)
 
+    self.sm = messaging.SubMaster(['pathPlan'])
+    self.st_model = st_wrapper.get_wrapper()
+    self.st_model.init_model()
+    self.st_scales = {'angle_offset': [-1.5868093967437744, 2.339749574661255],
+                      'angle_steers': [-59.099998474121094, 75.9000015258789],
+                      'delta_desired': [-5.2593878142219195, 6.793199853675723],
+                      'driver_torque': [-365.0, 372.0],
+                      'v_ego': [6.437766075134277, 32.0827522277832]}
+    self.last_st_output = None
+    self.st_data = []
+    self.st_seq_len = 200  # seq length (2 seconds)
+
+  def handle_st(self, CS, path_plan):
+    v_ego = np.interp(CS.v_ego, self.st_scales['v_ego'], [0, 1])
+    angle_steers = np.interp(CS.angle_steers, self.st_scales['angle_steers'], [0, 1])
+    delta_desired = np.interp(math.degrees(path_plan.deltaDesired), self.st_scales['delta_desired'], [0, 1])
+    angle_offset = np.interp(path_plan.angleOffsetLive, self.st_scales['angle_offset'], [0, 1])
+    if self.last_st_output is None:
+      driver_torque = CS.steer_torque_driver
+    else:
+      driver_torque = self.last_st_output
+    driver_torque = np.interp(driver_torque, self.st_scales['driver_torque'], [0, 1])
+
+    self.st_data.append([v_ego, angle_steers, delta_desired, angle_offset, driver_torque])
+
+    with open('/data/delta_desired', 'a') as f:
+      f.write('{}\n'.format(math.degrees(path_plan.deltaDesired)))
+
+    while len(self.st_data) > self.st_seq_len:
+      del self.st_data[0]
+
+    if len(self.st_data) != self.st_seq_len:
+      return 0.0
+
+    self.last_st_output = self.st_model.run_model(np.array(self.st_data).flatten().tolist())
+    return np.interp(self.last_st_output, [0, 1], self.st_scales['driver_torque'])
+
   def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
              left_line, right_line, lead, left_lane_depart, right_lane_depart):
 
     # *** compute control surfaces ***
 
     # gas and brake
-
+    self.sm.update(0)
     apply_gas = clip(actuators.gas, 0., 1.)
 
     if CS.CP.enableGasInterceptor:
@@ -127,16 +168,16 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
     apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
 
     # steer torque
-    new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
-    apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.steer_torque_motor, SteerLimitParams)
-    self.steer_rate_limited = new_steer != apply_steer
+    # new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
+    # apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.steer_torque_motor, SteerLimitParams)
+    apply_steer = self.handle_st(CS, self.sm['pathPlan'])
 
     # only cut torque when steer state is a known fault
     if CS.steer_state in [9, 25]:
       self.last_fault_frame = frame
 
     # Cut steering for 2s after fault
-    if not enabled or (frame - self.last_fault_frame < 200):
+    if not enabled or (frame - self.last_fault_frame < 200) or CS.angle_steers_rate > 125:
       apply_steer = 0
       apply_steer_req = 0
     else:

selfdrive/controls/lib/pathplanner.py

@@ -209,6 +209,8 @@ def update(self, sm, pm, CP, VM):
     plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid)
     plan_send.pathPlan.sensorValid = bool(sm['liveParameters'].sensorValid)
     plan_send.pathPlan.posenetValid = bool(sm['liveParameters'].posenetValid)
+    plan_send.pathPlan.deltaDesired = float(delta_desired)
+    plan_send.pathPlan.angleOffsetLive = float(angle_offset)
 
     plan_send.pathPlan.desire = desire
     plan_send.pathPlan.laneChangeState = self.lane_change_state

selfdrive/smart_torque/Makefile

@@ -0,0 +1,42 @@
+CXX = clang++
+
+PHONELIBS = ../../phonelibs
+
+WARN_FLAGS = -Werror=implicit-function-declaration \
+             -Werror=incompatible-pointer-types \
+             -Werror=int-conversion \
+             -Werror=return-type \
+             -Werror=format-extra-args \
+             -Wno-deprecated-declarations
+
+CFLAGS = -std=gnu11 -fPIC -O2 $(WARN_FLAGS)
+CXXFLAGS = -std=c++14 -fPIC -O2 $(WARN_FLAGS)
+
+SNPE_FLAGS = -I$(PHONELIBS)/snpe/include/
+SNPE_LIBS = -lSNPE -lsymphony-cpu -lsymphonypower
+
+OBJS = smart_torque.o
+OUTPUT = smart_torque.so
+
+.PHONY: all
+all: $(OUTPUT)
+
+DEPS := $(OBJS:.o=.d)
+
+$(OUTPUT): $(OBJS)
+	@echo "[ LINK ] $@"
+	$(CXX) -fPIC -o '$@' $^ \
+        $(LDFLAGS) \
+        $(SNPE_LIBS) \
+        -shared
+
+%.o: %.cc
+	@echo "[ CXX ] $@"
+	$(CXX) $(CXXFLAGS) -MMD \
+           -Iinclude -I.. -I../.. \
+           $(SNPE_FLAGS) \
+           -c -o '$@' '$<'
+
+.PHONY: clean
+clean:
+	rm -f $(OBJS) $(DEPS)

selfdrive/smart_torque/smart_torque.cc

@@ -0,0 +1,89 @@
+#include "smart_torque.h"
+using namespace std;
+
+std::unique_ptr<zdl::SNPE::SNPE> snpe;
+
+float *output;
+
+zdl::DlSystem::Runtime_t checkRuntime()
+{
+    static zdl::DlSystem::Version_t Version = zdl::SNPE::SNPEFactory::getLibraryVersion();
+    static zdl::DlSystem::Runtime_t Runtime;
+    std::cout << "SNPE Version: " << Version.asString().c_str() << std::endl; //Print Version number
+    if (zdl::SNPE::SNPEFactory::isRuntimeAvailable(zdl::DlSystem::Runtime_t::GPU)) {
+        Runtime = zdl::DlSystem::Runtime_t::GPU;
+    } else {
+        Runtime = zdl::DlSystem::Runtime_t::CPU;
+    }
+    return Runtime;
+}
+
+void initializeSNPE(zdl::DlSystem::Runtime_t runtime) {
+  std::unique_ptr<zdl::DlContainer::IDlContainer> container;
+  container = zdl::DlContainer::IDlContainer::open("/data/openpilot/selfdrive/smart_torque/st_modelv1.dlc");
+  //printf("loaded model\n");
+  int counter = 0;
+  zdl::SNPE::SNPEBuilder snpeBuilder(container.get());
+  snpe = snpeBuilder.setOutputLayers({})
+                      .setRuntimeProcessor(runtime)
+                      .setUseUserSuppliedBuffers(false)
+                      .setPerformanceProfile(zdl::DlSystem::PerformanceProfile_t::HIGH_PERFORMANCE)
+                      .build();
+}
+
+
+std::unique_ptr<zdl::DlSystem::ITensor> loadInputTensor(std::unique_ptr<zdl::SNPE::SNPE> &snpe, std::vector<float> inputVec) {
+  std::unique_ptr<zdl::DlSystem::ITensor> input;
+  const auto &strList_opt = snpe->getInputTensorNames();
+  if (!strList_opt) throw std::runtime_error("Error obtaining Input tensor names");
+  const auto &strList = *strList_opt;
+
+  const auto &inputDims_opt = snpe->getInputDimensions(strList.at(0));
+  const auto &inputShape = *inputDims_opt;
+
+  input = zdl::SNPE::SNPEFactory::getTensorFactory().createTensor(inputShape);
+  std::copy(inputVec.begin(), inputVec.end(), input->begin());
+
+  return input;
+}
+
+float returnOutput(const zdl::DlSystem::ITensor* tensor) {
+  float op = *tensor->cbegin();
+  return op;
+}
+
+zdl::DlSystem::ITensor* executeNetwork(std::unique_ptr<zdl::SNPE::SNPE>& snpe,
+                    std::unique_ptr<zdl::DlSystem::ITensor>& input) {
+  static zdl::DlSystem::TensorMap outputTensorMap;
+  snpe->execute(input.get(), outputTensorMap);
+  zdl::DlSystem::StringList tensorNames = outputTensorMap.getTensorNames();
+
+  const char* name = tensorNames.at(0);  // only should the first
+  auto tensorPtr = outputTensorMap.getTensor(name);
+  return tensorPtr;
+}
+
+extern "C" {
+  void init_model(){
+      zdl::DlSystem::Runtime_t runt=checkRuntime();
+      initializeSNPE(runt);
+  }
+
+  float run_model(float inputData[1000]){
+      int size = 1000;
+      std::vector<float> inputVec;
+      for (int i = 0; i < size; i++ ) {
+        inputVec.push_back(inputData[i]);
+      }
+
+      std::unique_ptr<zdl::DlSystem::ITensor> inputTensor = loadInputTensor(snpe, inputVec);
+      zdl::DlSystem::ITensor* oTensor = executeNetwork(snpe, inputTensor);
+      return returnOutput(oTensor);
+  }
+
+int main(){
+  std::cout << "hello";
+  return 0;
+}
+
+}

selfdrive/smart_torque/smart_torque.h

@@ -0,0 +1,19 @@
+#include <SNPE/SNPE.hpp>
+#include <SNPE/SNPEBuilder.hpp>
+#include <SNPE/SNPEFactory.hpp>
+#include <DlContainer/IDlContainer.hpp>
+#include <DlSystem/DlError.hpp>
+#include <DlSystem/ITensor.hpp>
+#include <DlSystem/ITensorFactory.hpp>
+#include <DlSystem/IUserBuffer.hpp>
+#include <DlSystem/IUserBufferFactory.hpp>
+#include <string>
+#include <sstream>
+#include <iostream>
+#include <fstream>
+#include <array>
+
+extern "C"{
+  void init_model();
+  float run_model(float inputData[1000]);
+}

selfdrive/smart_torque/st_wrapper.py

@@ -0,0 +1,18 @@
+from cffi import FFI
+import subprocess
+try:
+    subprocess.check_call(["make", "-j4"], cwd="/data/openpilot/selfdrive/smart_torque")
+except:
+    pass
+
+
+def get_wrapper():  # initialize st model and process long predictions
+    libmpc_fn = "/data/openpilot/selfdrive/smart_torque/smart_torque.so"
+
+    ffi = FFI()
+    ffi.cdef("""    
+    void init_model();
+    float run_model(float inputData[1000]);
+    """)
+
+    return ffi.dlopen(libmpc_fn)