Merge pull request commaai#65 from commaai/devel

058

CONTRIBUTING.md

@@ -2,11 +2,11 @@
 
 Our software is open source so you can solve your own problems without needing help from others. And if you solve a problem and are so kind, you can upstream it for the rest of the world to use.
 
-Most open source development activity is coordinated through our [slack](https://slack.comma.ai). A lot of documentation is available on our [medium](https://medium.com/@comma_ai/)
+Most open source development activity is coordinated through our [Discord](https://discord.comma.ai). A lot of documentation is available on our [medium](https://medium.com/@comma_ai/)
 
 ## Getting Started
 
- * Join our slack [slack.comma.ai](https://slack.comma.ai)
+ * Join our [Discord](https://discord.comma.ai)
  * Make sure you have a [GitHub account](https://github.com/signup/free)
  * Fork [our repositories](https://github.com/commaai) on GitHub
 

Dockerfile.openpilot

@@ -1,7 +1,26 @@
 FROM ubuntu:16.04
 ENV PYTHONUNBUFFERED 1
 
-RUN apt-get update && apt-get install -y build-essential clang vim screen wget bzip2 git libglib2.0-0 python-pip capnproto libcapnp-dev libzmq5-dev libffi-dev libusb-1.0-0
+RUN apt-get update && apt-get install -y \
+    build-essential \
+    clang \
+    vim \
+    screen \
+    wget \
+    bzip2 \
+    git \
+    libglib2.0-0 \
+    python-pip \
+    capnproto \
+    libcapnp-dev \
+    libzmq5-dev \
+    libffi-dev \
+    libusb-1.0-0 \
+    libssl-dev \
+    ocl-icd-libopencl1 \
+    ocl-icd-opencl-dev \
+    opencl-headers
+
 RUN pip install numpy==1.11.2 scipy==0.18.1 matplotlib==2.1.2
 
 COPY requirements_openpilot.txt /tmp/

README.md

@@ -31,7 +31,7 @@ openpilot is developed by [comma.ai](https://comma.ai/) and users like you.
 
 We have a [Twitter you should follow](https://twitter.com/comma_ai).
 
-Also, we have a several thousand people community on [slack](https://slack.comma.ai).
+Also, we have a several thousand people community on [Discord](https://discord.comma.ai).
 
 <table>
   <tr>
@@ -65,11 +65,13 @@ Supported Cars
 | Chevrolet<sup>3</sup>| Malibu 2017              | Adaptive Cruise      | Yes     | Yes            | 0mph             | 7mph           | Custom<sup>7</sup>|
 | Chevrolet<sup>3</sup>| Volt 2017-18             | Adaptive Cruise      | Yes     | Yes            | 0mph             | 7mph           | Custom<sup>7</sup>|
 | Cadillac<sup>3</sup> | ATS 2018                 | Adaptive Cruise      | Yes     | Yes            | 0mph             | 7mph           | Custom<sup>7</sup>|
+| Chrysler             | Pacifica Hybrid 2017     | Adaptive Cruise      | Yes     | Stock          | 0mph             | 9mph           | Custom<sup>8</sup>|
 | GMC<sup>3</sup>      | Acadia Denali 2018       | Adaptive Cruise      | Yes     | Yes            | 0mph             | 7mph           | Custom<sup>7</sup>|
 | Holden<sup>3</sup>   | Astra 2017               | Adaptive Cruise      | Yes     | Yes            | 0mph             | 7mph           | Custom<sup>7</sup>|
 | Honda                | Accord 2018              | All                  | Yes     | Stock          | 0mph             | 3mph           | Bosch             |
 | Honda                | Civic Sedan/Coupe 2016-18| Honda Sensing        | Yes     | Yes            | 0mph             | 12mph          | Nidec             |
-| Honda                | Civic Hatchback 2017-18  | Honda Sensing        | Yes     | Stock          | 0mph             | 12mph          | Bosch             |
+| Honda                | Civic Sedan/Coupe 2019   | Honda Sensing        | Yes     | Stock          | 0mph             | 2mph           | Bosch             |
+| Honda                | Civic Hatchback 2017-19  | Honda Sensing        | Yes     | Stock          | 0mph             | 12mph          | Bosch             |
 | Honda                | CR-V 2015-16             | Touring              | Yes     | Yes            | 25mph<sup>1</sup>| 12mph          | Nidec             |
 | Honda                | CR-V 2017-18             | Honda Sensing        | Yes     | Stock          | 0mph             | 12mph          | Bosch             |
 | Honda                | Odyssey 2017-19          | Honda Sensing        | Yes     | Yes            | 25mph<sup>1</sup>| 0mph           | Inverted Nidec    |
@@ -99,35 +101,32 @@ Supported Cars
 <sup>3</sup>[GM installation guide](https://zoneos.com/volt/).  
 <sup>4</sup>It needs an extra 120Ohm resistor ([pic1](https://i.imgur.com/CmdKtTP.jpg), [pic2](https://i.imgur.com/s2etUo6.jpg)) on bus 3 and giraffe switches set to 01X1 (11X1 for stock LKAS), where X depends on if you have the [comma power](https://comma.ai/shop/products/power/).  
 <sup>5</sup>28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.  
-<sup>6</sup>Open sourced [Hyundai Giraffe](https://github.com/commaai/neo/tree/master/giraffe/hyundai) is designed ofor the 2019 Sante Fe; pinout may differ for other Hyundais.  
-<sup>7</sup>Community built Giraffe, find more information here, [GM Giraffe](https://zoneos.com/shop/).  
+<sup>6</sup>Open sourced [Hyundai Giraffe](https://github.com/commaai/neo/tree/master/giraffe/hyundai) is designed for the 2019 Sante Fe; pinout may differ for other Hyundais.  
+<sup>7</sup>Community built Giraffe, find more information [here](https://zoneos.com/shop/).  
+<sup>8</sup>Community built Giraffe, find more information [here](https://github.com/adhintz/openpilot/wiki). Comma official Giraffe coming soon.  
 
 Community Maintained Cars
 ------
 
-| Make          | Model                     | Supported Package    | Lateral | Longitudinal   | No Accel Below   | No Steer Below | Giraffe           |
-| -------       | ----------------------    | -------------------- | ------- | ------------   | --------------   | -------------- | ------------------|
-| Honda         | Fit 2018                  | Honda Sensing        | Yes     | Yes            | 25mph<sup>1</sup>| 12mph          | Inverted Nidec    |
-| Tesla         | Model S 2012              | All                  | Yes     | Not yet        | Not applicable   | 0mph           | Custom<sup>8</sup>|
-| Tesla         | Model S 2013              | All                  | Yes     | Not yet        | Not applicable   | 0mph           | Custom<sup>8</sup>|
+| Make                 | Model                    | Supported Package    | Lateral | Longitudinal   | No Accel Below   | No Steer Below | Giraffe           |
+| ---------------------| -------------------------| ---------------------| --------| ---------------| -----------------| ---------------|-------------------|
+| Honda                | Fit 2018                 | Honda Sensing        | Yes     | Yes            | 25mph<sup>1</sup>| 12mph          | Inverted Nidec    |
+| Tesla                | Model S 2012             | All                  | Yes     | Not yet        | Not applicable   | 0mph           | Custom<sup>9</sup>|
+| Tesla                | Model S 2013             | All                  | Yes     | Not yet        | Not applicable   | 0mph           | Custom<sup>9</sup>|
 
 [[Honda Fit Pull Request]](https://github.com/commaai/openpilot/pull/266). <br />
 [[Tesla Model S Pull Request]](https://github.com/commaai/openpilot/pull/246) <br />
-<sup>8</sup>Community built Giraffe, find more information here [Community Tesla Giraffe](https://github.com/jeankalud/neo/tree/tesla_giraffe/giraffe/tesla) <br />
+<sup>9</sup>Community built Giraffe, find more information here [Community Tesla Giraffe](https://github.com/jeankalud/neo/tree/tesla_giraffe/giraffe/tesla) <br />
 
 Community Maintained Cars are not confirmed by comma.ai to meet our [safety model](https://github.com/commaai/openpilot/blob/devel/SAFETY.md). Be extra cautious using them.
 
 In Progress Cars
 ------
-- All TSS-P Toyota with Steering Assist.
-  - 'Full Speed Range Dynamic Radar Cruise Control' is required to enable stop-and-go. Only the Prius, Camry and C-HR have this option.
-  - Even though the Tundra, Sequoia and the Land Cruiser have TSS-P, they don't have Steering Assist and are not supported.
+- All TSS-P Toyota with Steering Assist and LSS-P Lexus with Steering Assist or Lane Keep Assist.
   - Only remaining Toyota cars with no port yet are the Avalon and the Sienna.
-- All LSS-P Lexus with Steering Assist or Lane Keep Assist.
-  - 'All-Speed Range Dynamic Radar Cruise Control' is required to enable stop-and-go. Only the GS, GSH, F, RX, RXH, LX, NX, NXH, LC, LCH, LS, LSH have this option.
-  - Even though the LX have TSS-P, it does not have Steering Assist and is not supported.
 - All Hyundai with SmartSense.
 - All Kia with SCC and LKAS.
+- All Chrysler, Jeep, Fiat with Adaptive Cruise Control and LaneSense.
 
 How can I add support for my car?
 ------
@@ -136,7 +135,7 @@ If your car has adaptive cruise control and lane keep assist, you are in luck. U
 
 We've written guides for [Brand](https://medium.com/@comma_ai/how-to-write-a-car-port-for-openpilot-7ce0785eda84) and [Model](https://medium.com/@comma_ai/openpilot-port-guide-for-toyota-models-e5467f4b5fe6) ports. These guides might help you after you have the basics figured out.
 
-- BMW, Audi, Volvo, and Mercedes all use [FlexRay](https://en.wikipedia.org/wiki/FlexRay) and are unlikely to be supported any time soon.
+- BMW, Audi, Volvo, and Mercedes all use [FlexRay](https://en.wikipedia.org/wiki/FlexRay) and can be supported after [FlexRay support](https://github.com/commaai/openpilot/pull/463) is merged.
 - We put time into a Ford port, but the steering has a 10 second cutout limitation that makes it unusable.
 - The 2016-2017 Honda Accord uses a custom signaling protocol for steering that's unlikely to ever be upstreamed.
 
@@ -162,6 +161,8 @@ Directory structure
         ├── locationd       # Soon to be home of precise location
         ├── logcatd         # Android logcat as a service
         ├── loggerd         # Logger and uploader of car data
+        ├── mapd            # Fetches map data and computes next global path
+        ├── orbd            # Computes ORB features from frames
         ├── proclogd        # Logs information from proc
         ├── sensord         # IMU / GPS interface code
         ├── test            # Car simulator running code through virtual maneuvers
@@ -186,23 +187,21 @@ By using it, you agree to [our privacy policy](https://community.comma.ai/privac
 Testing on PC
 ------
 
-There is rudimentary infrastructure to run a basic simulation and generate a report of openpilot's behavior in different scenarios.
+Check out [openpilot-tools](https://github.com/commaai/openpilot-tools): lots of tools you can use to replay driving data, test and develop openpilot from your pc.
+
+Also, within openpilot there is a rudimentary infrastructure to run a basic simulation and generate a report of openpilot's behavior in different longitudinal control scenarios.
 
 ```bash
 # Requires working docker
 ./run_docker_tests.sh
 ```
 
-The resulting plots are displayed in `selfdrive/test/tests/plant/out/longitudinal/index.html`
-
-More extensive testing infrastructure and simulation environments are coming soon.
-
-
 Contributing
 ------
 
-We welcome both pull requests and issues on
-[github](http://github.com/commaai/openpilot). Bug fixes and new car ports encouraged.
+We welcome both pull requests and issues on [github](http://github.com/commaai/openpilot). Bug fixes and new car ports encouraged.
+
+We also have a [bounty program](https://comma.ai/bounties.html).
 
 Want to get paid to work on openpilot? [comma.ai is hiring](https://comma.ai/jobs/)
 

RELEASES.md

@@ -1,3 +1,13 @@
+Version 0.5.8 (2019-01-17)
+========================
+ * Open sourced visiond
+ * Auto-slowdown for upcoming turns
+ * Chrysler/Jeep/Fiat support thanks to adhintz!
+ * Honda Civic 2019 support thanks to csouers!
+ * Improved use of car display in Toyota thanks to arne182!
+ * No data upload when connected to Android or iOS hotspots and "Enable Upload Over Cellular" setting is off
+ * EON stops charging when 12V battery drops below 11.8V
+
 Version 0.5.7 (2018-12-06)
 ========================
  * Speed limit from OpenStreetMap added to UI

cereal/car.capnp

@@ -244,6 +244,8 @@ struct CarControl {
     leadVisible @3: Bool;
     visualAlert @4: VisualAlert;
     audibleAlert @5: AudibleAlert;
+    rightLaneVisible @6: Bool;
+    leftLaneVisible @7: Bool;
 
     enum VisualAlert {
       # these are the choices from the Honda

cereal/log.capnp

@@ -187,10 +187,10 @@ struct SensorEventData {
     iOS @1;
     fiber @2;
     velodyne @3;  # Velodyne IMU
-    # c3 sensors below
-    bno055 @4;
-    lsm6ds3 @5;
-    bmp280 @6;
+    bno055 @4;    # Bosch accelerometer
+    lsm6ds3 @5;   # accelerometer (c2)
+    bmp280 @6;    # barometer (c2)
+    mmc3416x @7;  # magnetometer (c2)
   }
 }
 
@@ -399,6 +399,8 @@ struct Live100Data {
   curvature @37 :Float32;       # path curvature from vehicle model
   hudLeadDEPRECATED @14 :Int32;
   cumLagMs @15 :Float32;
+  startMonoTime @48 :UInt64;
+  mapValid @49 :Bool;
 
   enabled @19 :Bool;
   active @36 :Bool;
@@ -420,6 +422,10 @@ struct Live100Data {
   engageable @41 :Bool;  # can OP be engaged?
   driverMonitoringOn @43 :Bool;
 
+  # maps
+  vCurvature @46 :Float32;
+  decelForTurn @47 :Bool;
+
   enum ControlState {
     disabled @0;
     preEnabled @1;
@@ -556,6 +562,8 @@ struct Plan {
   aTarget @18 :Float32;
   jerkFactor @6 :Float32;
   hasLead @7 :Bool;
+  hasLeftLane @23 :Bool;
+  hasRightLane @24 :Bool;
   fcw @8 :Bool;
   longitudinalPlanSource @15 :LongitudinalPlanSource;
 
@@ -567,6 +575,7 @@ struct Plan {
   # maps
   vCurvature @21 :Float32;
   decelForTurn @22 :Bool;
+  mapValid @25 :Bool;
 
   struct GpsTrajectory {
     x @0 :List(Float32);
@@ -1584,8 +1593,15 @@ struct LiveMapData {
   roadCurvatureX @8 :List(Float32);
   roadCurvature @9 :List(Float32);
   distToTurn @10 :Float32;
+  mapValid @11 :Bool;
 }
 
+struct CameraOdometry {
+  trans @0 :List(Float32); # m/s in device frame
+  rot @1 :List(Float32); # rad/s in device frame
+  transStd @2 :List(Float32); # std m/s in device frame
+  rotStd @3 :List(Float32); # std rad/s in device frame
+}
 
 struct Event {
   # in nanoseconds?
@@ -1654,5 +1670,6 @@ struct Event {
     boot @60 :Boot;
     liveParameters @61 :LiveParametersData;
     liveMapData @62 :LiveMapData;
+    cameraOdometry @63 :CameraOdometry;
   }
 }

common/transformations/camera.py

@@ -1,5 +1,6 @@
 import numpy as np
 import common.transformations.orientation as orient
+import cv2
 
 FULL_FRAME_SIZE = (1164, 874)
 W, H = FULL_FRAME_SIZE[0], FULL_FRAME_SIZE[1]
@@ -62,31 +63,35 @@ def roll_from_ke(m):
   return np.arctan2(-(m[1, 0] - m[1, 1] * m[2, 0] / m[2, 1]),
                     -(m[0, 0] - m[0, 1] * m[2, 0] / m[2, 1]))
 
-def normalize(img_pts):
+
+def normalize(img_pts, intrinsics=eon_intrinsics):
   # normalizes image coordinates
   # accepts single pt or array of pts
+  intrinsics_inv = np.linalg.inv(intrinsics)
   img_pts = np.array(img_pts)
   input_shape = img_pts.shape
   img_pts = np.atleast_2d(img_pts)
   img_pts = np.hstack((img_pts, np.ones((img_pts.shape[0],1))))
-  img_pts_normalized = eon_intrinsics_inv.dot(img_pts.T).T
+  img_pts_normalized = intrinsics_inv.dot(img_pts.T).T
   img_pts_normalized[(img_pts < 0).any(axis=1)] = np.nan
   return img_pts_normalized[:,:2].reshape(input_shape)
 
-def denormalize(img_pts):
+
+def denormalize(img_pts, intrinsics=eon_intrinsics):
   # denormalizes image coordinates
   # accepts single pt or array of pts
   img_pts = np.array(img_pts)
   input_shape = img_pts.shape
   img_pts = np.atleast_2d(img_pts)
   img_pts = np.hstack((img_pts, np.ones((img_pts.shape[0],1))))
-  img_pts_denormalized = eon_intrinsics.dot(img_pts.T).T
+  img_pts_denormalized = intrinsics.dot(img_pts.T).T
   img_pts_denormalized[img_pts_denormalized[:,0] > W] = np.nan
   img_pts_denormalized[img_pts_denormalized[:,0] < 0] = np.nan
   img_pts_denormalized[img_pts_denormalized[:,1] > H] = np.nan
   img_pts_denormalized[img_pts_denormalized[:,1] < 0] = np.nan
   return img_pts_denormalized[:,:2].reshape(input_shape)
 
+
 def device_from_ecef(pos_ecef, orientation_ecef, pt_ecef):
   # device from ecef frame
   # device frame is x -> forward, y-> right, z -> down
@@ -99,6 +104,7 @@ def device_from_ecef(pos_ecef, orientation_ecef, pt_ecef):
   pt_device = np.einsum('jk,ik->ij', device_from_ecef_rot, pt_ecef_rel)
   return pt_device.reshape(input_shape)
 
+
 def img_from_device(pt_device):
   # img coordinates from pts in device frame
   # first transforms to view frame, then to img coords
@@ -113,3 +119,30 @@ def img_from_device(pt_device):
   pt_img = pt_view/pt_view[:,2:3]
   return pt_img.reshape(input_shape)[:,:2]
 
+
+def rotate_img(img, eulers, crop=None, intrinsics=eon_intrinsics):
+  size = img.shape[:2]
+  rot = orient.rot_from_euler(eulers)
+  quadrangle = np.array([[0, 0],
+                         [size[1]-1, 0],
+                         [0, size[0]-1],
+                         [size[1]-1, size[0]-1]], dtype=np.float32)
+  quadrangle_norm = np.hstack((normalize(quadrangle, intrinsics=intrinsics), np.ones((4,1))))
+  warped_quadrangle_full = np.einsum('ij, kj->ki', intrinsics.dot(rot), quadrangle_norm)
+  warped_quadrangle = np.column_stack((warped_quadrangle_full[:,0]/warped_quadrangle_full[:,2],
+                                       warped_quadrangle_full[:,1]/warped_quadrangle_full[:,2])).astype(np.float32)
+  if crop:
+    W_border = (size[1] - crop[0])/2
+    H_border = (size[0] - crop[1])/2
+    outside_crop = (((warped_quadrangle[:,0] < W_border) |
+                     (warped_quadrangle[:,0] >= size[1] - W_border)) &
+                    ((warped_quadrangle[:,1] < H_border) |
+                     (warped_quadrangle[:,1] >= size[0] - H_border)))
+    if not outside_crop.all():
+      raise ValueError("warped image not contained inside crop")
+  else:
+    H_border, W_border = 0, 0
+  M = cv2.getPerspectiveTransform(quadrangle, warped_quadrangle)
+  img_warped = cv2.warpPerspective(img, M, size[::-1])
+  return img_warped[H_border: size[0] - H_border,
+                    W_border: size[1] - W_border]

common/transformations/orientation.py

@@ -221,6 +221,8 @@ def ned_euler_from_ecef(ned_ecef_init, ecef_poses):
   ecef_poses = array(ecef_poses)
   output_shape = ecef_poses.shape
   ned_ecef_init = np.atleast_2d(ned_ecef_init)
+  if ned_ecef_init.shape[0] == 1:
+    ned_ecef_init = np.tile(ned_ecef_init[0], (output_shape[0], 1))
   ecef_poses = np.atleast_2d(ecef_poses)
 
   ned_poses = np.zeros(ecef_poses.shape)