WIP: sim, need to unhack openpilot (#1174)

* comma hackathon

* update readme

* update readme

* Update README.md

* Update README.md

* Update README.md

* add more to readme

* Update README.md

* unhack

* Delete README.md

* Delete get_carla_095.sh

* Delete run_carla_095.sh

* remove NO_FLIP, and fix imu destroy

* remove pipfile

* make ui match

* actually match ui

Co-authored-by: George Hotz <geohot@gmail.com>

tools/sim/bridge.py

@@ -1,6 +1,7 @@
 #!/usr/bin/env python3
+import carla
 import os
-import time
+import time, termios, tty, sys
 import math
 import atexit
 import numpy as np
@@ -8,10 +9,14 @@ import threading
 import random
 import cereal.messaging as messaging
 import argparse
+import zmq
+import queue
 from common.params import Params
 from common.realtime import Ratekeeper
 from lib.can import can_function, sendcan_function
-import queue
+from lib.helpers import FakeSteeringWheel
+from lib.manual_ctrl import wheel_poll_thread
+from selfdrive.car.honda.values import CruiseButtons
 
 parser = argparse.ArgumentParser(description='Bridge between CARLA and openpilot.')
 parser.add_argument('--autopilot', action='store_true')
@@ -31,6 +36,7 @@ def cam_callback(image):
   dat.frame = {
     "frameId": image.frame,
     "image": img.tostring(),
+    "transform": [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
   }
   pm.send('frame', dat)
 
@@ -78,19 +84,18 @@ def go():
   import carla
   client = carla.Client("127.0.0.1", 2000)
   client.set_timeout(5.0)
-  world = client.load_world('Town03')
+  world = client.load_world('Town04')
-
   settings = world.get_settings()
   settings.fixed_delta_seconds = 0.05
   world.apply_settings(settings)
 
   weather = carla.WeatherParameters(
-      cloudyness=0.0,
+      cloudyness=0.1,
       precipitation=0.0,
       precipitation_deposits=0.0,
       wind_intensity=0.0,
-      sun_azimuth_angle=0.0,
+      sun_azimuth_angle=15.0,
-      sun_altitude_angle=0.0)
+      sun_altitude_angle=75.0)
   world.set_weather(weather)
 
   blueprint_library = world.get_blueprint_library()
@@ -101,12 +106,18 @@ def go():
   """
 
   world_map = world.get_map()
+  vehicle_bp = random.choice(blueprint_library.filter('vehicle.tesla.*'))
+  vehicle = world.spawn_actor(vehicle_bp, world_map.get_spawn_points()[16])
 
-  vehicle_bp = random.choice(blueprint_library.filter('vehicle.bmw.*'))
+  # make tires less slippery
-  vehicle = world.spawn_actor(vehicle_bp, random.choice(world_map.get_spawn_points()))
+  wheel_control = carla.WheelPhysicsControl(tire_friction=5)
+  physics_control = carla.VehiclePhysicsControl(mass=1326, wheels=[wheel_control]*4, \
+                                                torque_curve=[[20.0, 500.0], [5000.0, 500.0]], gear_switch_time=0)
+  vehicle.apply_physics_control(physics_control)
 
   if args.autopilot:
     vehicle.set_autopilot(True)
+  # print(vehicle.get_speed_limit())
 
   blueprint = blueprint_library.find('sensor.camera.rgb')
   blueprint.set_attribute('image_size_x', str(W))
@@ -117,7 +128,7 @@ def go():
   camera = world.spawn_actor(blueprint, transform, attach_to=vehicle)
   camera.listen(cam_callback)
 
-  # TODO: wait for carla 0.9.7
+  # reenable IMU
   imu_bp = blueprint_library.find('sensor.other.imu')
   imu = world.spawn_actor(imu_bp, transform, attach_to=vehicle)
   imu.listen(imu_callback)
@@ -133,18 +144,86 @@ def go():
   # can loop
   sendcan = messaging.sub_sock('sendcan')
   rk = Ratekeeper(100)
-  steer_angle = 0
+
+  # init
+  A_throttle = 2.
+  A_brake = 2.
+  A_steer_torque = 1.
+  fake_wheel = FakeSteeringWheel()
+  is_openpilot_engaged = False
+  in_reverse = False
+
+  # start input poll
+  from multiprocessing import Process
+  p = Process(target=wheel_poll_thread)
+  p.start()
+
+  # zmq receiver for input thread
+  context = zmq.Context()
+  socket = context.socket(zmq.REP)
+  socket.connect("tcp://127.0.0.1:4444")
+
+  throttle_out = 0
+  brake_out = 0
+  steer_angle_out = 0
+
   while 1:
     vel = vehicle.get_velocity()
-    speed = math.sqrt(vel.x**2 + vel.y**2 + vel.z**2)
+    speed = math.sqrt(vel.x**2 + vel.y**2 + vel.z**2) * 3.6
-
+
-    can_function(pm, speed, steer_angle, rk.frame, rk.frame%500 == 499)
+    try:
-    if rk.frame%5 == 0:
+      #check for a input message, this will not block
-      throttle, brake, steer = sendcan_function(sendcan)
+      message = socket.recv(flags=zmq.NOBLOCK)
-      steer_angle += steer/10000.0 # torque
+      socket.send(b"good")
-      vc = carla.VehicleControl(throttle=throttle, steer=steer_angle, brake=brake)
+      # print(message.decode('UTF-8'))
+
+      m = message.decode('UTF-8').split('_')
+      if m[0] == "steer":
+        steer_angle_out = float(m[1])
+        fake_wheel.set_angle(steer_angle_out) # touching the wheel overrides fake wheel angle
+        # print(" === steering overriden === ")
+      if m[0] == "throttle":
+        throttle_out = float(m[1]) / 100.
+        if throttle_out > 0.3:
+          can_function(pm, speed, fake_wheel.angle, rk.frame, cruise_button=CruiseButtons.CANCEL)
+          is_openpilot_engaged = False
+      if m[0] == "brake":
+        brake_out = float(m[1]) / 100.
+        if brake_out > 0.3:
+          can_function(pm, speed, fake_wheel.angle, rk.frame, cruise_button=CruiseButtons.CANCEL)
+          is_openpilot_engaged = False
+      if m[0] == "reverse":
+        in_reverse = not in_reverse
+        can_function(pm, speed, fake_wheel.angle, rk.frame, cruise_button=CruiseButtons.CANCEL)
+        is_openpilot_engaged = False
+      if m[0] == "cruise":
+        if m[1] == "down":
+          can_function(pm, speed, fake_wheel.angle, rk.frame, cruise_button=CruiseButtons.DECEL_SET)
+          is_openpilot_engaged = True
+        if m[1] == "up":
+          can_function(pm, speed, fake_wheel.angle, rk.frame, cruise_button=CruiseButtons.RES_ACCEL)
+          is_openpilot_engaged = True
+        if m[1] == "cancel":
+          can_function(pm, speed, fake_wheel.angle, rk.frame, cruise_button=CruiseButtons.CANCEL)
+          is_openpilot_engaged = False
+
+    except zmq.Again as e:
+      #skip if no message
+      pass
+
+    can_function(pm, speed, fake_wheel.angle, rk.frame)
+    if rk.frame%1 == 0: # 20Hz?
+      throttle_op, brake_op, steer_torque_op = sendcan_function(sendcan)
+      # print(" === torq, ",steer_torque_op, " ===")
+      if is_openpilot_engaged:
+        fake_wheel.response(steer_torque_op * A_steer_torque, speed)
+        throttle_out = throttle_op * A_throttle
+        brake_out = brake_op * A_brake
+        steer_angle_out = fake_wheel.angle
+        # print(steer_torque_op)
+      # print(steer_angle_out)
+      vc = carla.VehicleControl(throttle=throttle_out, steer=steer_angle_out / 3.14, brake=brake_out, reverse=in_reverse)
       vehicle.apply_control(vc)
-      print(speed, steer_angle, vc)
 
     rk.keep_time()
 

tools/sim/lib/can.py

@@ -6,6 +6,7 @@ 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
+import math
 
 from selfdrive.test.longitudinal_maneuvers.plant import get_car_can_parser
 cp = get_car_can_parser()
@@ -14,7 +15,12 @@ cp = get_car_can_parser()
 packer = CANPacker("honda_civic_touring_2016_can_generated")
 rpacker = CANPacker("acura_ilx_2016_nidec")
 
-def can_function(pm, speed, angle, idx, engage):
+SR = 7.5
+
+def angle_to_sangle(angle):
+  return - math.degrees(angle) * SR
+
+def can_function(pm, speed, angle, idx, cruise_button=0):
   msg = []
   msg.append(packer.make_can_msg("ENGINE_DATA", 0, {"XMISSION_SPEED": speed}, idx))
   msg.append(packer.make_can_msg("WHEEL_SPEEDS", 0,
@@ -23,10 +29,7 @@ def can_function(pm, speed, angle, idx, engage):
      "WHEEL_SPEED_RL": speed,
      "WHEEL_SPEED_RR": speed}, -1))
 
-  if engage:
+  msg.append(packer.make_can_msg("SCM_BUTTONS", 0, {"CRUISE_BUTTONS": cruise_button}, idx))
-    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])
@@ -37,7 +40,7 @@ def can_function(pm, speed, angle, idx, engage):
   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("STEERING_SENSORS", 0, {"STEER_ANGLE":angle_to_sangle(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))

tools/sim/lib/helpers.py

@@ -0,0 +1,36 @@
+import numpy as np
+
+class FakeSteeringWheel():
+  def __init__(self, dt=0.01):
+    # physical params
+    self.DAC = 4. / 0.625 # convert torque value from can to Nm
+    self.k = 0.035
+    self.centering_k = 4.1 * 0.9
+    self.b = 0.1 * 0.8
+    self.I = 1 * 1.36 * (0.175**2)
+    self.dt = dt
+    # ...
+
+    self.angle = 0. # start centered
+    # self.omega = 0.
+    
+  def response(self, torque, vego=0):
+    exerted_torque = torque * self.DAC
+    # centering_torque = np.clip(-(self.centering_k * self.angle), -1.1, 1.1)
+    # damping_torque = -(self.b * self.omega)
+    # self.omega += self.dt * (exerted_torque + centering_torque + damping_torque) / self.I
+    # self.omega = np.clip(self.omega, -1.1, 1.1)
+    # self.angle += self.dt * self.omega
+    self.angle += self.dt * self.k * exerted_torque # aristotle
+
+    # print(" ========== ")
+    # print("angle,", self.angle)
+    # print("omega,", self.omega)
+    print("torque,", exerted_torque)
+    # print("centering torque,", centering_torque)
+    # print("damping torque,", damping_torque)
+    # print(" ========== ")
+
+  def set_angle(self, target):
+    self.angle = target
+    # self.omega = 0.

tools/sim/lib/manual_ctrl.py

@@ -0,0 +1,200 @@
+#!/usr/bin/env python3
+# set up wheel
+import os, struct, array
+from fcntl import ioctl
+
+# Iterate over the joystick devices.
+print('Available devices:')
+for fn in os.listdir('/dev/input'):
+    if fn.startswith('js'):
+        print('  /dev/input/%s' % (fn))
+
+# We'll store the states here.
+axis_states = {}
+button_states = {}
+
+# These constants were borrowed from linux/input.h
+axis_names = {
+    0x00 : 'x',
+    0x01 : 'y',
+    0x02 : 'z',
+    0x03 : 'rx',
+    0x04 : 'ry',
+    0x05 : 'rz',
+    0x06 : 'trottle',
+    0x07 : 'rudder',
+    0x08 : 'wheel',
+    0x09 : 'gas',
+    0x0a : 'brake',
+    0x10 : 'hat0x',
+    0x11 : 'hat0y',
+    0x12 : 'hat1x',
+    0x13 : 'hat1y',
+    0x14 : 'hat2x',
+    0x15 : 'hat2y',
+    0x16 : 'hat3x',
+    0x17 : 'hat3y',
+    0x18 : 'pressure',
+    0x19 : 'distance',
+    0x1a : 'tilt_x',
+    0x1b : 'tilt_y',
+    0x1c : 'tool_width',
+    0x20 : 'volume',
+    0x28 : 'misc',
+}
+
+button_names = {
+    0x120 : 'trigger',
+    0x121 : 'thumb',
+    0x122 : 'thumb2',
+    0x123 : 'top',
+    0x124 : 'top2',
+    0x125 : 'pinkie',
+    0x126 : 'base',
+    0x127 : 'base2',
+    0x128 : 'base3',
+    0x129 : 'base4',
+    0x12a : 'base5',
+    0x12b : 'base6',
+    0x12f : 'dead',
+    0x130 : 'a',
+    0x131 : 'b',
+    0x132 : 'c',
+    0x133 : 'x',
+    0x134 : 'y',
+    0x135 : 'z',
+    0x136 : 'tl',
+    0x137 : 'tr',
+    0x138 : 'tl2',
+    0x139 : 'tr2',
+    0x13a : 'select',
+    0x13b : 'start',
+    0x13c : 'mode',
+    0x13d : 'thumbl',
+    0x13e : 'thumbr',
+
+    0x220 : 'dpad_up',
+    0x221 : 'dpad_down',
+    0x222 : 'dpad_left',
+    0x223 : 'dpad_right',
+
+    # XBox 360 controller uses these codes.
+    0x2c0 : 'dpad_left',
+    0x2c1 : 'dpad_right',
+    0x2c2 : 'dpad_up',
+    0x2c3 : 'dpad_down',
+}
+
+axis_map = []
+button_map = []
+
+def wheel_poll_thread():
+  # Open the joystick device.
+  fn = '/dev/input/js0'
+  print('Opening %s...' % fn)
+  jsdev = open(fn, 'rb')
+
+  # Get the device name.
+  #buf = bytearray(63)
+  buf = array.array('B', [0] * 64)
+  ioctl(jsdev, 0x80006a13 + (0x10000 * len(buf)), buf) # JSIOCGNAME(len)
+  js_name = buf.tobytes().rstrip(b'\x00').decode('utf-8')
+  print('Device name: %s' % js_name)
+
+  # Get number of axes and buttons.
+  buf = array.array('B', [0])
+  ioctl(jsdev, 0x80016a11, buf) # JSIOCGAXES
+  num_axes = buf[0]
+
+  buf = array.array('B', [0])
+  ioctl(jsdev, 0x80016a12, buf) # JSIOCGBUTTONS
+  num_buttons = buf[0]
+
+  # Get the axis map.
+  buf = array.array('B', [0] * 0x40)
+  ioctl(jsdev, 0x80406a32, buf) # JSIOCGAXMAP
+
+  for axis in buf[:num_axes]:
+      axis_name = axis_names.get(axis, 'unknown(0x%02x)' % axis)
+      axis_map.append(axis_name)
+      axis_states[axis_name] = 0.0
+
+  # Get the button map.
+  buf = array.array('H', [0] * 200)
+  ioctl(jsdev, 0x80406a34, buf) # JSIOCGBTNMAP
+
+  for btn in buf[:num_buttons]:
+      btn_name = button_names.get(btn, 'unknown(0x%03x)' % btn)
+      button_map.append(btn_name)
+      button_states[btn_name] = 0
+
+  print('%d axes found: %s' % (num_axes, ', '.join(axis_map)))
+  print('%d buttons found: %s' % (num_buttons, ', '.join(button_map)))
+
+  # Enable FF
+  import evdev
+  from evdev import ecodes, InputDevice, ff
+  device = evdev.list_devices()[0]
+  evtdev = InputDevice(device)
+  val = 24000
+  evtdev.write(ecodes.EV_FF, ecodes.FF_AUTOCENTER, val)
+
+  # Set up zmq sock
+  import zmq
+  context = zmq.Context()
+  socket = context.socket(zmq.REQ) # block on send
+  socket.bind('tcp://127.0.0.1:4444')
+
+  while True:
+    evbuf = jsdev.read(8)
+    time, value, mtype, number = struct.unpack('IhBB', evbuf)
+    # print(mtype, number, value)
+    if mtype & 0x02: # wheel & paddles
+      axis = axis_map[number]
+
+      if axis == "z": # gas
+        fvalue = value / 32767.0
+        axis_states[axis] = fvalue
+        normalized = (1 - fvalue) * 50
+        socket.send_string(str("throttle_%f" % normalized))
+        _ = socket.recv()
+
+      if axis == "rz": # brake
+        fvalue = value / 32767.0
+        axis_states[axis] = fvalue
+        normalized = (1 - fvalue) * 50
+        socket.send_string(str("brake_%f" % normalized))
+        _ = socket.recv()
+
+      if axis == "x": # steer angle
+        fvalue = value / 32767.0
+        axis_states[axis] = fvalue
+        normalized = fvalue
+        socket.send_string(str("steer_%f" % normalized))
+        _ = socket.recv()
+
+    if mtype & 0x01: # buttons
+      if number in [0,19]: # X
+        if value == 1: # press down
+          socket.send_string(str("cruise_down"))
+          _ = socket.recv()
+
+      if number in [3,18]: # triangle
+        if value == 1: # press down
+          socket.send_string(str("cruise_up"))
+          _ = socket.recv()
+
+      if number in [1,6]: # square
+        if value == 1: # press down
+          socket.send_string(str("cruise_cancel"))
+          _ = socket.recv()
+
+      if number in [10,21]: # R3
+        if value == 1: # press down
+          socket.send_string(str("reverse_switch"))
+          _ = socket.recv()
+
+if __name__ == '__main__':
+  from multiprocessing import Process
+  p = Process(target=wheel_poll_thread)
+  p.start()