Simulator running with keyboard (#1176)

* sim with keyboard

* write calibration

* need recv

* use readchar

* doesn't accelerate

* queue, not zmq

* fix line wrap

* fix physics

* add BLOCK to manager

selfdrive/manager.py

@@ -407,6 +407,10 @@ def manager_thread():
   if os.getenv("NOBOARD") is None:
     start_managed_process("pandad")
 
+  if os.getenv("BLOCK") is not None:
+    for k in os.getenv("BLOCK").split(","):
+      del managed_processes[k]
+
   logger_dead = False
 
   while 1:

tools/sim/bridge.py

@@ -9,17 +9,16 @@ 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
 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')
+parser.add_argument('--joystick', action='store_true')
 args = parser.parse_args()
 
 pm = messaging.PubMaster(['frame', 'sensorEvents', 'can'])
@@ -80,7 +79,10 @@ def fake_driver_monitoring():
     pm.send('driverState', dat)
     time.sleep(0.1)
 
-def go():
+def go(q):
+  threading.Thread(target=health_function).start()
+  threading.Thread(target=fake_driver_monitoring).start()
+
   import carla
   client = carla.Client("127.0.0.1", 2000)
   client.set_timeout(5.0)
@@ -111,8 +113,11 @@ def go():
 
   # make tires less slippery
   wheel_control = carla.WheelPhysicsControl(tire_friction=5)
-  physics_control = carla.VehiclePhysicsControl(mass=1326, wheels=[wheel_control]*4, \
+  physics_control = vehicle.get_physics_control()
-                                                torque_curve=[[20.0, 500.0], [5000.0, 500.0]], gear_switch_time=0)
+  physics_control.mass = 1326
+  physics_control.wheels = [wheel_control]*4
+  physics_control.torque_curve = [[20.0, 500.0], [5000.0, 500.0]]
+  physics_control.gear_switch_time = 0.0
   vehicle.apply_physics_control(physics_control)
 
   if args.autopilot:
@@ -143,7 +148,7 @@ def go():
 
   # can loop
   sendcan = messaging.sub_sock('sendcan')
-  rk = Ratekeeper(100)
+  rk = Ratekeeper(100, print_delay_threshold=0.05)
 
   # init
   A_throttle = 2.
@@ -153,31 +158,19 @@ def go():
   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()
+    cruise_button = 0
-    speed = math.sqrt(vel.x**2 + vel.y**2 + vel.z**2) * 3.6
 
-    try:
+    # check for a input message, this will not block
-      #check for a input message, this will not block
+    if not q.empty():
-      message = socket.recv(flags=zmq.NOBLOCK)
+      print("here")
-      socket.send(b"good")
+      message = q.get()
-      # print(message.decode('UTF-8'))
 
-      m = message.decode('UTF-8').split('_')
+      m = message.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
@@ -185,33 +178,32 @@ def go():
       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)
+          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)
+          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)
+        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)
+          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)
+          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)
+          cruise_button = CruiseButtons.CANCEL
           is_openpilot_engaged = False
 
-    except zmq.Again as e:
+    vel = vehicle.get_velocity()
-      #skip if no message
+    speed = math.sqrt(vel.x**2 + vel.y**2 + vel.z**2) * 3.6
-      pass
+    can_function(pm, speed, fake_wheel.angle, rk.frame, cruise_button=cruise_button)
 
-    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, " ===")
@@ -234,9 +226,7 @@ if __name__ == "__main__":
   params.put("HasAcceptedTerms", terms_version)
   params.put("CompletedTrainingVersion", training_version)
   params.put("CommunityFeaturesToggle", "1")
-
+  params.put("CalibrationParams", '{"vanishing_point": [582.06, 442.78], "valid_blocks": 20}')
-  threading.Thread(target=health_function).start()
-  threading.Thread(target=fake_driver_monitoring).start()
 
   # no carla, still run
   try:
@@ -246,5 +236,18 @@ if __name__ == "__main__":
     while 1:
       time.sleep(1)
     
-  go()
+  from multiprocessing import Process, Queue
+  q = Queue()
+  p = Process(target=go, args=(q,))
+  p.daemon = True
+  p.start()
+
+  if args.joystick:
+    # start input poll for joystick
+    from lib.manual_ctrl import wheel_poll_thread
+    wheel_poll_thread(q)
+  else:
+    # start input poll for keyboard
+    from lib.keyboard_ctrl import keyboard_poll_thread
+    keyboard_poll_thread(q)
 

tools/sim/lib/helpers.py

@@ -26,11 +26,11 @@ class FakeSteeringWheel():
     # print(" ========== ")
     # print("angle,", self.angle)
     # print("omega,", self.omega)
-    print("torque,", exerted_torque)
+    # 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.
+    # self.omega = 0.

tools/sim/lib/keyboard_ctrl.py

@@ -0,0 +1,61 @@
+import time
+import sys
+import termios
+from termios import *
+
+# Indexes for termios list.
+IFLAG = 0
+OFLAG = 1
+CFLAG = 2
+LFLAG = 3
+ISPEED = 4
+OSPEED = 5
+CC = 6
+
+def getch():
+  fd = sys.stdin.fileno()
+  old_settings = termios.tcgetattr(fd)
+  try:
+    # set
+    mode = termios.tcgetattr(fd)
+    mode[IFLAG] = mode[IFLAG] & ~(BRKINT | ICRNL | INPCK | ISTRIP | IXON)
+    #mode[OFLAG] = mode[OFLAG] & ~(OPOST)
+    mode[CFLAG] = mode[CFLAG] & ~(CSIZE | PARENB)
+    mode[CFLAG] = mode[CFLAG] | CS8
+    mode[LFLAG] = mode[LFLAG] & ~(ECHO | ICANON | IEXTEN | ISIG)
+    mode[CC][VMIN] = 1
+    mode[CC][VTIME] = 0
+    termios.tcsetattr(fd, termios.TCSAFLUSH, mode)
+
+    ch = sys.stdin.read(1)
+  finally:
+    termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
+  return ch
+
+def keyboard_poll_thread(q):
+  while True:
+    c = getch()
+    print("got %s" % c)
+    if c == '1':
+      q.put(str("cruise_up"))
+    if c == '2':
+      q.put(str("cruise_down"))
+    if c == '3':
+      q.put(str("cruise_cancel"))
+    if c == 'q':
+      exit(0)
+
+def test(q):
+  while 1:
+    print("hello")
+    time.sleep(1.0)
+
+if __name__ == '__main__':
+  from multiprocessing import Process, Queue
+  q = Queue()
+  p = Process(target=test, args=(q,))
+  p.daemon = True
+  p.start()
+
+  keyboard_poll_thread(q)
+

tools/sim/lib/manual_ctrl.py

@@ -88,7 +88,7 @@ button_names = {
 axis_map = []
 button_map = []
 
-def wheel_poll_thread():
+def wheel_poll_thread(q):
   # Open the joystick device.
   fn = '/dev/input/js0'
   print('Opening %s...' % fn)
@@ -139,12 +139,6 @@ def wheel_poll_thread():
   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)
@@ -156,43 +150,36 @@ def wheel_poll_thread():
         fvalue = value / 32767.0
         axis_states[axis] = fvalue
         normalized = (1 - fvalue) * 50
-        socket.send_string(str("throttle_%f" % normalized))
+        q.put(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))
+        q.put(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))
+        q.put(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"))
+          q.put(str("cruise_down"))
-          _ = socket.recv()
 
       if number in [3,18]: # triangle
         if value == 1: # press down
-          socket.send_string(str("cruise_up"))
+          q.put(str("cruise_up"))
-          _ = socket.recv()
 
       if number in [1,6]: # square
         if value == 1: # press down
-          socket.send_string(str("cruise_cancel"))
+          q.put(str("cruise_cancel"))
-          _ = socket.recv()
 
       if number in [10,21]: # R3
         if value == 1: # press down
-          socket.send_string(str("reverse_switch"))
+          q.put(str("reverse_switch"))
-          _ = socket.recv()
 
 if __name__ == '__main__':
   from multiprocessing import Process