diff --git a/Dockerfile.openpilot b/Dockerfile.openpilot index bdab4e041..62c655d7c 100644 --- a/Dockerfile.openpilot +++ b/Dockerfile.openpilot @@ -21,6 +21,7 @@ RUN apt-get update && apt-get install -y \ ocl-icd-opencl-dev \ opencl-headers +RUN pip install --upgrade pip==18.0 RUN pip install numpy==1.11.2 scipy==0.18.1 matplotlib==2.1.2 COPY requirements_openpilot.txt /tmp/ diff --git a/README.md b/README.md index 902781b89..3a69dc1c0 100644 --- a/README.md +++ b/README.md @@ -99,8 +99,8 @@ Supported Cars | Toyota | Highlander 2017-18 | All | Yes | Yes2| 0mph | 0mph | Toyota | | Toyota | Highlander Hybrid 2018 | All | Yes | Yes2| 0mph | 0mph | Toyota | | Toyota | Prius 2016 | TSS-P | Yes | Yes2| 0mph | 0mph | Toyota | -| Toyota | Prius 2017-18 | All | Yes | Yes2| 0mph | 0mph | Toyota | -| Toyota | Prius Prime 2017-18 | All | Yes | Yes2| 0mph | 0mph | Toyota | +| Toyota | Prius 2017-19 | All | Yes | Yes2| 0mph | 0mph | Toyota | +| Toyota | Prius Prime 2017-19 | All | Yes | Yes2| 0mph | 0mph | Toyota | | Toyota | Rav4 2016 | TSS-P | Yes | Yes2| 20mph1| 0mph | Toyota | | Toyota | Rav4 2017-18 | All | Yes | Yes2| 20mph1| 0mph | Toyota | | Toyota | Rav4 Hybrid 2017-18 | All | Yes | Yes2| 0mph | 0mph | Toyota | diff --git a/RELEASES.md b/RELEASES.md index 61d905ce2..c63b3ef79 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -9,6 +9,8 @@ Version 0.5.11 (2019-04-17) * Improve performance of visiond and ui * Honda Passport 2019 support * Lexus RX Hybrid 2019 support thanks to schomems! + * Improve road selection heuristic in mapd + * Add Lane Departure Warning to dashboard for Toyota thanks to arne182 Version 0.5.10 (2019-03-19) ======================== diff --git a/cereal/car.capnp b/cereal/car.capnp index 8fb312867..2915d6871 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -247,6 +247,8 @@ struct CarControl { audibleAlert @5: AudibleAlert; rightLaneVisible @6: Bool; leftLaneVisible @7: Bool; + rightLaneDepart @8: Bool; + leftLaneDepart @9: Bool; enum VisualAlert { # these are the choices from the Honda diff --git a/selfdrive/can/packer.py b/selfdrive/can/packer.py index 0fa0bb26c..7f3c8d285 100644 --- a/selfdrive/can/packer.py +++ b/selfdrive/can/packer.py @@ -1,3 +1,4 @@ +import six import struct from selfdrive.can.libdbc_py import libdbc, ffi @@ -20,7 +21,7 @@ class CANPacker(object): def pack(self, addr, values, counter): values_thing = [] - for name, value in values.items(): + for name, value in six.iteritems(values): if name not in self.sig_names: self.sig_names[name] = ffi.new("char[]", name) diff --git a/selfdrive/can/parser.py b/selfdrive/can/parser.py index 2e8384f63..83d0f6623 100644 --- a/selfdrive/can/parser.py +++ b/selfdrive/can/parser.py @@ -1,3 +1,4 @@ +import six import time from collections import defaultdict import numbers @@ -58,7 +59,7 @@ class CANParser(object): { 'address': msg_address, 'check_frequency': freq, - } for msg_address, freq in message_options.items()]) + } for msg_address, freq in six.iteritems(message_options)]) self.can = libdbc.can_init(bus, dbc_name, len(message_options_c), message_options_c, len(signal_options_c), signal_options_c, sendcan, tcp_addr) diff --git a/selfdrive/can/process_dbc.py b/selfdrive/can/process_dbc.py index 3f3f2ddca..31b131596 100755 --- a/selfdrive/can/process_dbc.py +++ b/selfdrive/can/process_dbc.py @@ -2,7 +2,7 @@ import os import glob import sys - +import six import jinja2 from collections import Counter @@ -39,10 +39,10 @@ def main(): continue #skip output is newer than template and dbc msgs = [(address, msg_name, msg_size, sorted(msg_sigs, key=lambda s: s.name not in ("COUNTER", "CHECKSUM"))) # process counter and checksums first - for address, ((msg_name, msg_size), msg_sigs) in sorted(can_dbc.msgs.items()) if msg_sigs] + for address, ((msg_name, msg_size), msg_sigs) in sorted(six.iteritems(can_dbc.msgs)) if msg_sigs] def_vals = {a: set(b) for a,b in can_dbc.def_vals.items()} #remove duplicates - def_vals = [(address, sig) for address, sig in sorted(def_vals.items())] + def_vals = [(address, sig) for address, sig in sorted(six.iteritems(def_vals))] if can_dbc.name.startswith("honda") or can_dbc.name.startswith("acura"): checksum_type = "honda" diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index a1999439f..d5afffbaa 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -218,7 +218,7 @@ class CarInterface(object): ret.gasPressed = self.CS.user_gas_pressed # brake pedal - ret.brake = self.CS.user_brake // 0xd0 + ret.brake = self.CS.user_brake / 0xd0 ret.brakePressed = self.CS.brake_pressed # steering wheel diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 4d173f4f5..716758313 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -12,9 +12,9 @@ def actuator_hystereses(brake, braking, brake_steady, v_ego, car_fingerprint): # hyst params brake_hyst_on = 0.02 # to activate brakes exceed this value brake_hyst_off = 0.005 # to deactivate brakes below this value - brake_hyst_gap = 0.01 # don't change brake command for small ocilalitons within this value + brake_hyst_gap = 0.01 # don't change brake command for small oscillations within this value - #*** histeresis logic to avoid brake blinking. go above 0.1 to trigger + #*** hysteresis logic to avoid brake blinking. go above 0.1 to trigger if (brake < brake_hyst_on and not braking) or brake < brake_hyst_off: brake = 0. braking = brake > 0. @@ -34,7 +34,7 @@ def actuator_hystereses(brake, braking, brake_steady, v_ego, car_fingerprint): return brake, braking, brake_steady -def brake_pump_hysteresys(apply_brake, apply_brake_last, last_pump_ts): +def brake_pump_hysteresis(apply_brake, apply_brake_last, last_pump_ts): ts = sec_since_boot() pump_on = False @@ -174,7 +174,7 @@ class CarController(object): # Send gas and brake commands. if (frame % 2) == 0: idx = frame // 2 - pump_on, self.last_pump_ts = brake_pump_hysteresys(apply_brake, self.apply_brake_last, self.last_pump_ts) + pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts) can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, hud.chime, hud.fcw, idx)) self.apply_brake_last = apply_brake diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 8197a7922..d69636d32 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -125,7 +125,8 @@ class CarController(object): self.packer = CANPacker(dbc_name) def update(self, sendcan, enabled, CS, frame, actuators, - pcm_cancel_cmd, hud_alert, audible_alert, forwarding_camera, left_line, right_line, lead): + pcm_cancel_cmd, hud_alert, audible_alert, forwarding_camera, + left_line, right_line, lead, left_lane_depart, right_lane_depart): # *** compute control surfaces *** @@ -246,7 +247,7 @@ class CarController(object): send_ui = False if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus: - can_sends.append(create_ui_command(self.packer, steer, sound1, sound2, left_line, right_line)) + can_sends.append(create_ui_command(self.packer, steer, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart)) if frame % 100 == 0 and ECU.DSU in self.fake_ecus: can_sends.append(create_fcw_command(self.packer, fcw)) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 20add65ba..01d9c7899 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -379,7 +379,8 @@ class CarInterface(object): self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert, c.hudControl.audibleAlert, self.forwarding_camera, - c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, c.hudControl.leadVisible) + c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, c.hudControl.leadVisible, + c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart) self.frame += 1 return False diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 720c0ef60..85b8dce8d 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -89,10 +89,11 @@ def create_fcw_command(packer, fcw): return packer.make_can_msg("ACC_HUD", 0, values) -def create_ui_command(packer, steer, sound1, sound2, left_line, right_line): +def create_ui_command(packer, steer, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart): values = { - "RIGHT_LINE": 1 if right_line else 2, - "LEFT_LINE": 1 if left_line else 2, + "RIGHT_LINE": 3 if right_lane_depart else 1 if right_line else 2, + "LEFT_LINE": 3 if left_lane_depart else 1 if left_line else 2, + "BARRIERS" : 3 if left_lane_depart or right_lane_depart else 0, "SET_ME_X0C": 0x0c, "SET_ME_X2C": 0x2c, "SET_ME_X38": 0x38, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 60d3536ff..31dcbef07 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -11,6 +11,7 @@ import selfdrive.messaging as messaging from selfdrive.config import Conversions as CV from selfdrive.services import service_list from selfdrive.car.car_helpers import get_car +from selfdrive.controls.lib.model_parser import CAMERA_OFFSET from selfdrive.controls.lib.drive_helpers import learn_angle_model_bias, \ get_events, \ create_event, \ @@ -305,8 +306,21 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis CC.hudControl.speedVisible = isEnabled(state) CC.hudControl.lanesVisible = isEnabled(state) CC.hudControl.leadVisible = plan.hasLead - CC.hudControl.rightLaneVisible = bool(path_plan.pathPlan.rProb > 0.5) - CC.hudControl.leftLaneVisible = bool(path_plan.pathPlan.lProb > 0.5) + + right_lane_visible = path_plan.pathPlan.rProb > 0.5 + left_lane_visible = path_plan.pathPlan.lProb > 0.5 + + CC.hudControl.rightLaneVisible = bool(right_lane_visible) + CC.hudControl.leftLaneVisible = bool(left_lane_visible) + + blinker = CS.leftBlinker or CS.rightBlinker + ldw_allowed = CS.vEgo > 12.5 and not blinker + + if len(list(path_plan.pathPlan.rPoly)) == 4: + CC.hudControl.rightLaneDepart = bool(ldw_allowed and path_plan.pathPlan.rPoly[3] > -(1 + CAMERA_OFFSET) and right_lane_visible) + if len(list(path_plan.pathPlan.lPoly)) == 4: + CC.hudControl.leftLaneDepart = bool(ldw_allowed and path_plan.pathPlan.lPoly[3] < (1 - CAMERA_OFFSET) and left_lane_visible) + CC.hudControl.visualAlert = AM.visual_alert CC.hudControl.audibleAlert = AM.audible_alert diff --git a/selfdrive/controls/lib/driver_monitor.py b/selfdrive/controls/lib/driver_monitor.py index 767b1c1a1..9af3cb1b1 100644 --- a/selfdrive/controls/lib/driver_monitor.py +++ b/selfdrive/controls/lib/driver_monitor.py @@ -51,7 +51,7 @@ class _DriverPose(): self.pitch_offset = 0. -def _monitor_hysteresys(variance_level, monitor_valid_prev): +def _monitor_hysteresis(variance_level, monitor_valid_prev): var_thr = 0.63 if monitor_valid_prev else 0.37 return variance_level < var_thr @@ -124,7 +124,7 @@ class DriverStatus(): self.monitor_param_on = params.get("IsDriverMonitoringEnabled") == "1" self.ts_last_check = ts - self.monitor_valid = _monitor_hysteresys(self.variance_filter.x, monitor_valid_prev) + self.monitor_valid = _monitor_hysteresis(self.variance_filter.x, monitor_valid_prev) self.monitor_on = self.monitor_valid and self.monitor_param_on if monitor_param_on_prev != self.monitor_param_on: self._reset_filters() diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 826b6dce1..40628ad09 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -20,7 +20,10 @@ def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ class PathPlanner(object): def __init__(self, CP): self.MP = ModelParser() - + + self.l_poly = [0., 0., 0., 0.] + self.r_poly = [0., 0., 0., 0.] + self.last_cloudlog_t = 0 context = zmq.Context() diff --git a/selfdrive/mapd/default_speeds_generator.py b/selfdrive/mapd/default_speeds_generator.py index ac2791c83..b03e66d7a 100755 --- a/selfdrive/mapd/default_speeds_generator.py +++ b/selfdrive/mapd/default_speeds_generator.py @@ -1,5 +1,6 @@ #!/usr/bin/env python import json +import six DEFAULT_OUTPUT_FILENAME = "default_speeds_by_region.json" @@ -145,7 +146,7 @@ def main(filename = DEFAULT_OUTPUT_FILENAME): DE.add_rule({"zone:maxspeed": "DE:rural"}, "100") DE.add_rule({"zone:maxspeed": "DE:motorway"}, "none") DE.add_rule({"bicycle_road": "yes"}, "30") - + """ --- DO NOT MODIFY CODE BELOW THIS LINE --- """ """ --- ADD YOUR COUNTRY OR STATE ABOVE --- """ @@ -205,7 +206,7 @@ class Country(Region): def jsonify(self): ret_dict = {} ret_dict[self.name] = {} - for r_name, region in self.regions.items(): + for r_name, region in six.iteritems(self.regions): ret_dict[self.name].update(region.jsonify()) ret_dict[self.name]['Default'] = self.rules return ret_dict diff --git a/selfdrive/mapd/mapd_helpers.py b/selfdrive/mapd/mapd_helpers.py index a6292bbea..50dde6ecb 100644 --- a/selfdrive/mapd/mapd_helpers.py +++ b/selfdrive/mapd/mapd_helpers.py @@ -1,3 +1,4 @@ +import six import math import json import numpy as np @@ -91,7 +92,7 @@ def geocode_maxspeed(tags, location_info): rule_valid = all( tag_name in tags and tags[tag_name] == value - for tag_name, value in rule['tags'].items() + for tag_name, value in six.iteritems(rule['tags']) ) if rule_valid: max_speed = rule['speed'] @@ -102,7 +103,7 @@ def geocode_maxspeed(tags, location_info): rule_valid = all( tag_name in tags and tags[tag_name] == value - for tag_name, value in rule['tags'].items() + for tag_name, value in six.iteritems(rule['tags']) ) if rule_valid: max_speed = rule['speed'] @@ -191,6 +192,10 @@ class Way: closest_way = way best_score = score + # Normal score is < 5 + if best_score > 50: + return None + return closest_way def __str__(self):