add webcam to cameras (#1201)

tools/sim/bridge.py

@@ -19,6 +19,7 @@ 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')
+parser.add_argument('--realmonitoring', action='store_true')
 args = parser.parse_args()
 
 pm = messaging.PubMaster(['frame', 'sensorEvents', 'can'])
@@ -68,6 +69,8 @@ def health_function():
     rk.keep_time()
 
 def fake_driver_monitoring():
+  if args.realmonitoring:
+    return
   pm = messaging.PubMaster(['driverState'])
   while 1:
     dat = messaging.new_message('driverState')
@@ -198,7 +201,7 @@ def go(q):
 
     vel = vehicle.get_velocity()
     speed = math.sqrt(vel.x**2 + vel.y**2 + vel.z**2) * 3.6
-    can_function(pm, speed, fake_wheel.angle, rk.frame, cruise_button=cruise_button)
+    can_function(pm, speed, fake_wheel.angle, rk.frame, cruise_button=cruise_button, is_engaged=is_openpilot_engaged)
 
     if rk.frame%1 == 0: # 20Hz?
       throttle_op, brake_op, steer_torque_op = sendcan_function(sendcan)

tools/sim/lib/can.py

@@ -20,7 +20,7 @@ SR = 7.5
 def angle_to_sangle(angle):
   return - math.degrees(angle) * SR
 
-def can_function(pm, speed, angle, idx, cruise_button=0):
+def can_function(pm, speed, angle, idx, cruise_button=0, is_engaged=False):
   msg = []
   msg.append(packer.make_can_msg("ENGINE_DATA", 0, {"XMISSION_SPEED": speed}, idx))
   msg.append(packer.make_can_msg("WHEEL_SPEEDS", 0,
@@ -50,6 +50,7 @@ def can_function(pm, speed, angle, idx, cruise_button=0):
   msg.append(packer.make_can_msg("CRUISE_PARAMS", 0, {}, idx))
   msg.append(packer.make_can_msg("CRUISE", 0, {}, idx))
   msg.append(packer.make_can_msg("SCM_FEEDBACK", 0, {"MAIN_ON": 1}, idx))
+  msg.append(packer.make_can_msg("POWERTRAIN_DATA", 0, {"ACC_STATUS": int(is_engaged)}, idx))
   #print(msg)
 
   # cam bus

tools/webcam/README.md

@@ -0,0 +1,43 @@
+Run openpilot with webcam on PC/laptop
+=====================
+What's needed:  
+- Ubuntu 16.04  
+- Python 3.7.3  
+- GPU (recommended)  
+- Two USB webcams, at least 720p and 78 degrees FOV (e.g. Logitech C920/C615)  
+- [Car harness](https://comma.ai/shop/products/comma-car-harness) w/ black panda (or the outdated grey panda/giraffe combo) to connect to your car  
+- [Panda paw](https://comma.ai/shop/products/panda-paw) (or USB-A to USB-A cable) to connect panda to your computer  
+- Tape, Charger, ...  
+That's it!  
+
+## Clone openpilot and install the requirements 
+```
+cd ~
+git clone https://github.com/commaai/openpilot.git 
+# Follow [this readme](https://github.com/commaai/openpilot/tree/master/tools) to install the requirements 
+# Add line "export PYTHONPATH=$HOME/openpilot" to your ~/.bashrc 
+# You may also need to install tensorflow-gpu 2.0.0 
+# Install [OpenCV4](https://www.pyimagesearch.com/2018/08/15/how-to-install-opencv-4-on-ubuntu/) 
+```
+## Build openpilot for webcam
+```
+cd ~/openpilot
+scons use_webcam=1
+touch prebuilt
+```
+## Connect the hardwares 
+```
+# Connect the road facing camera first, then the driver facing camera 
+# (default indexes are 1 and 2; can be modified in selfdrive/camerad/cameras/camera_webcam.cc)
+# Connect your computer to panda
+```
+## GO 
+```
+cd ~/openpilot/tools/webcam 
+./accept_terms.py # accept the user terms so that thermald can detect the car started 
+cd ~/openpilot/selfdrive 
+PASSIVE=0 NOSENSOR=1 ./manager.py 
+# Start the car, then the UI should show the road webcam's view 
+# Adjust and secure the webcams (you can run tools/webcam/front_mount_helper.py to help mount the driver camera)
+# Finish calibration and engage!
+```

tools/webcam/accept_terms.py

@@ -0,0 +1,9 @@
+#!/usr/bin/env python
+from common.params import Params
+from selfdrive.version import terms_version, training_version
+
+if __name__ == '__main__':
+  params = Params()
+  params.put("HasAcceptedTerms", str(terms_version, 'utf-8'))
+  params.put("CompletedTrainingVersion", str(training_version, 'utf-8'))
+  print("Terms Accepted!")

tools/webcam/front_mount_helper.py

@@ -0,0 +1,35 @@
+#!/usr/bin/env python
+import numpy as np
+
+# copied from common.transformations/camera.py
+eon_dcam_focal_length = 860.0 # pixels
+webcam_focal_length = 908.0 # pixels
+
+eon_dcam_intrinsics = np.array([
+  [eon_dcam_focal_length,   0,   1152/2.],
+  [  0,  eon_dcam_focal_length,  864/2.],
+  [  0,    0,     1]])
+
+webcam_intrinsics = np.array([
+  [webcam_focal_length,   0.,   1280/2.],
+  [  0.,  webcam_focal_length,  720/2.],
+  [  0.,    0.,     1.]])
+
+cam_id = 2
+
+if __name__ == "__main__":
+  import cv2
+
+  trans_webcam_to_eon_front = np.dot(eon_dcam_intrinsics,np.linalg.inv(webcam_intrinsics))
+
+  cap = cv2.VideoCapture(cam_id)
+  cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280)
+  cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720)
+
+  while (True):
+    ret, img = cap.read()
+    if ret:
+      img = cv2.warpPerspective(img, trans_webcam_to_eon_front, (1152,864), borderMode=cv2.BORDER_CONSTANT, borderValue=0)
+      img = img[:,-864//2:,:]
+      cv2.imshow('preview', img)
+      cv2.waitKey(10)

tools/webcam/warp_vis.py

@@ -0,0 +1,45 @@
+#!/usr/bin/env python
+import numpy as np
+
+# copied from common.transformations/camera.py
+eon_focal_length = 910.0 # pixels
+eon_dcam_focal_length = 860.0 # pixels
+
+webcam_focal_length = 908.0 # pixels
+
+eon_intrinsics = np.array([
+  [eon_focal_length,   0.,   1164/2.],
+  [  0.,  eon_focal_length,  874/2.],
+  [  0.,    0.,     1.]])
+
+eon_dcam_intrinsics = np.array([
+  [eon_dcam_focal_length,   0,   1152/2.],
+  [  0,  eon_dcam_focal_length,  864/2.],
+  [  0,    0,     1]])
+
+webcam_intrinsics = np.array([
+  [webcam_focal_length,   0.,   1280/2.],
+  [  0.,  webcam_focal_length,  720/2.],
+  [  0.,    0.,     1.]])
+
+if __name__ == "__main__":
+  import cv2
+  trans_webcam_to_eon_rear = np.dot(eon_intrinsics,np.linalg.inv(webcam_intrinsics))
+  trans_webcam_to_eon_front = np.dot(eon_dcam_intrinsics,np.linalg.inv(webcam_intrinsics))
+  print("trans_webcam_to_eon_rear:\n", trans_webcam_to_eon_rear)
+  print("trans_webcam_to_eon_front:\n", trans_webcam_to_eon_front)
+
+  cap = cv2.VideoCapture(1)
+  cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280)
+  cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720)
+
+  while (True):
+    ret, img = cap.read()
+    if ret:
+      # img = cv2.warpPerspective(img, trans_webcam_to_eon_rear, (1164,874), borderMode=cv2.BORDER_CONSTANT, borderValue=0)
+      img = cv2.warpPerspective(img, trans_webcam_to_eon_front, (1164,874), borderMode=cv2.BORDER_CONSTANT, borderValue=0)
+      print(img.shape, end='\r')
+      cv2.imshow('preview', img)
+      cv2.waitKey(10)
+
+