From b0bbca91a2709d41772117842890fcbd1fc5424e Mon Sep 17 00:00:00 2001 From: Andrew Frahn Date: Tue, 9 Jul 2019 13:37:43 +1000 Subject: [PATCH] 0.6.0.3 (#37) Version 0.6.0.3-ku7 (2019-07-09) ======================== * HKG - Send fingerprints to Sentry * HKG - WIP: bring back mapd from 0.5.12 * HKG - WIP: Auto Set Cruise to Speed Limit * HKG - Don't Learn Min Steer Speed when turning corners * HKG - Create MDPS12 Message (to reduce/stop Stock cam faults) * HKG - Implement Min Speed Speed Warning * Better Panda * 0.6.0.2 Untested * update releases * bug * Send fingerprints to Sentry * Refactor default Civic params (#720) * move civic params out * fix variable name * simplify ford scaling * cleanup * remove import dependency * requested changes * keep hyundai * 2019 Rav4 Limited AWD (#732) * Fingerprint * Merge Limited and XLE fingerprint because they're the same * 0.6.0.3 - Untested * no mapd just yet * Remote ASL * Done * panda too --- RELEASES.md | 14 +- panda/board/safety/safety_hyundai.h | 2 +- selfdrive/car/__init__.py | 28 ++ selfdrive/car/car_helpers.py | 7 + selfdrive/car/chrysler/interface.py | 23 +- selfdrive/car/ford/interface.py | 31 +- selfdrive/car/gm/interface.py | 25 +- selfdrive/car/honda/interface.py | 30 +- selfdrive/car/hyundai/carcontroller.py | 73 +++- selfdrive/car/hyundai/carstate.py | 7 +- selfdrive/car/hyundai/hyundaican.py | 10 +- selfdrive/car/hyundai/interface.py | 30 +- selfdrive/car/subaru/interface.py | 24 +- selfdrive/car/toyota/interface.py | 34 +- selfdrive/car/toyota/values.py | 4 +- selfdrive/common/version.h | 2 +- selfdrive/crash.py | 41 ++ selfdrive/manager.py | 2 + selfdrive/mapd/__init__.py | 0 selfdrive/mapd/default_speeds.json | 106 +++++ selfdrive/mapd/default_speeds_generator.py | 240 ++++++++++++ selfdrive/mapd/mapd.py | 341 ++++++++++++++++ selfdrive/mapd/mapd_helpers.py | 435 +++++++++++++++++++++ 23 files changed, 1331 insertions(+), 178 deletions(-) create mode 100644 selfdrive/mapd/__init__.py create mode 100644 selfdrive/mapd/default_speeds.json create mode 100755 selfdrive/mapd/default_speeds_generator.py create mode 100755 selfdrive/mapd/mapd.py create mode 100644 selfdrive/mapd/mapd_helpers.py diff --git a/RELEASES.md b/RELEASES.md index 32d7e142d0637b..49479b568a1082 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,16 +1,24 @@ +Version 0.6.0.3-ku7 (2019-07-09) +======================== +* HKG - Send fingerprints to Sentry +* HKG - WIP: bring back mapd from 0.5.12 +* HKG - WIP: Auto Set Cruise to Speed Limit +* HKG - Don't Learn Min Steer Speed when turning corners +* HKG - Create MDPS12 Message (to reduce/stop Stock cam faults) +* HKG - Implement Min Speed Speed Warning + Version 0.6.0.2-ku7 (2019-07-08) ======================== * HKG - Auto Discover Checksum if LKAS11 Message not present * HKG - Disable Torque on Lane Change -* HKG - Auto Learn Low Speed Cutoff -* HKG - Disable during Low Speed, where low speed is learnt +* HKG - Auto Learn Min Steer Speed Cutoff +* HKG - Disable during Min Steer Speed, where min speed is learnt * HKG - Improve Support, maybe even Genesis * HKG - Clean up redundant code, remove last "Fingerprint" stuff * HKG - Add Sentry Crash Reporting * HKG - PUF is back, Auto Forward CAM/CAR when EON is not active * HKG - Hopefully a better attempt at "Remove remnants of CP UI" - Version 0.6.0.1-ku7 (2019-07-05) ======================== * HKG - Auto Support Cars with all 3 Gear Selector Types diff --git a/panda/board/safety/safety_hyundai.h b/panda/board/safety/safety_hyundai.h index 55a9a421279128..c9e1f88c076276 100644 --- a/panda/board/safety/safety_hyundai.h +++ b/panda/board/safety/safety_hyundai.h @@ -135,7 +135,7 @@ static int hyundai_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { int addr = to_fwd->RIR>>21; if (addr == 832 && bus_num == hyundai_camera_bus) return -1; - // if (addr == 593 && bus_num == 0) return -1; + if (addr == 593 && bus_num == 0) return -1; if (bus_num == 0) return (uint8_t)(hyundai_camera_bus); if (bus_num == hyundai_camera_bus) return (uint8_t)(0); } diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index 078641969f2249..cfb1a853151b16 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -3,6 +3,34 @@ # kg of standard extra cargo to count for drive, gas, etc... STD_CARGO_KG = 136. + +# FIXME: hardcoding honda civic 2016 touring params so they can be used to +# scale unknown params for other cars +class CivicParams: + MASS = 1326. + STD_CARGO_KG + WHEELBASE = 2.70 + CENTER_TO_FRONT = WHEELBASE * 0.4 + CENTER_TO_REAR = WHEELBASE - CENTER_TO_FRONT + ROTATIONAL_INERTIA = 2500 + TIRE_STIFFNESS_FRONT = 192150 + TIRE_STIFFNESS_REAR = 202500 + +# TODO: get actual value, for now starting with reasonable value for +# civic and scaling by mass and wheelbase +def scale_rot_inertia(mass, wheelbase): + return CivicParams.ROTATIONAL_INERTIA * mass * wheelbase ** 2 / (CivicParams.MASS * CivicParams.WHEELBASE ** 2) + +# TODO: start from empirically derived lateral slip stiffness for the civic and scale by +# mass and CG position, so all cars will have approximately similar dyn behaviors +def scale_tire_stiffness(mass, wheelbase, center_to_front, tire_stiffness_factor=1.0): + center_to_rear = wheelbase - center_to_front + tire_stiffness_front = (CivicParams.TIRE_STIFFNESS_FRONT * tire_stiffness_factor) * mass / CivicParams.MASS * \ + (center_to_rear / wheelbase) / (CivicParams.CENTER_TO_REAR / CivicParams.WHEELBASE) + + tire_stiffness_rear = (CivicParams.TIRE_STIFFNESS_REAR * tire_stiffness_factor) * mass / CivicParams.MASS * \ + (center_to_front / wheelbase) / (CivicParams.CENTER_TO_FRONT / CivicParams.WHEELBASE) + + return tire_stiffness_front, tire_stiffness_rear def dbc_dict(pt_dbc, radar_dbc, chassis_dbc=None): return {'pt': pt_dbc, 'radar': radar_dbc, 'chassis': chassis_dbc} diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 1fd99f76e31454..c88abd3f2caef3 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -5,6 +5,7 @@ from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.swaglog import cloudlog import selfdrive.messaging as messaging +import selfdrive.crash as crash def get_startup_alert(car_recognized, controller_available): @@ -146,6 +147,12 @@ def get_car(logcan, sendcan): if candidate is None: cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints) candidate = "mock" + else: + cloudlog.warning("car does match fingerprint: %r", fingerprints) + try: + crash.capture_warning("fingerprinted %s" % candidate) + except: # fixes occasional travis errors + pass CarInterface, CarController = interfaces[candidate] params = CarInterface.get_params(candidate, fingerprints, vin) diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 6621c7e20ffcdf..793f7320844148 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -6,7 +6,7 @@ from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.chrysler.carstate import CarState, get_can_parser, get_camera_parser from selfdrive.car.chrysler.values import ECU, check_ecu_msgs, CAR -from selfdrive.car import STD_CARGO_KG +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness class CarInterface(object): @@ -51,16 +51,6 @@ def get_params(candidate, fingerprint, vin=""): # pedal ret.enableCruise = True - # FIXME: hardcoding honda civic 2016 touring params so they can be used to - # scale unknown params for other cars - mass_civic = 2923. * CV.LB_TO_KG + STD_CARGO_KG - wheelbase_civic = 2.70 - centerToFront_civic = wheelbase_civic * 0.4 - centerToRear_civic = wheelbase_civic - centerToFront_civic - rotationalInertia_civic = 2500 - tireStiffnessFront_civic = 85400 * 2.0 - tireStiffnessRear_civic = 90000 * 2.0 - # Speed conversion: 20, 45 mph ret.wheelbase = 3.089 # in meters for Pacifica Hybrid 2017 ret.steerRatio = 16.2 # Pacifica Hybrid 2017 @@ -85,20 +75,13 @@ def get_params(candidate, fingerprint, vin=""): ret.minSteerSpeed = 17.5 # m/s 17 on the way up, 13 on the way down once engaged. # TODO allow 2019 cars to steer down to 13 m/s if already engaged. - centerToRear = ret.wheelbase - ret.centerToFront # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase - ret.rotationalInertia = rotationalInertia_civic * \ - ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2) + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) # TODO: start from empirically derived lateral slip stiffness for the civic and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors - ret.tireStiffnessFront = tireStiffnessFront_civic * \ - ret.mass / mass_civic * \ - (centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic) - ret.tireStiffnessRear = tireStiffnessRear_civic * \ - ret.mass / mass_civic * \ - (ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic) + ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront) # no rear steering, at least on the listed cars above ret.steerRatioRear = 0. diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 7664d99640eaab..0f3bbc33cc2b84 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -7,7 +7,7 @@ from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.ford.carstate import CarState, get_can_parser from selfdrive.car.ford.values import MAX_ANGLE -from selfdrive.car import STD_CARGO_KG +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness class CarInterface(object): @@ -51,16 +51,6 @@ def get_params(candidate, fingerprint, vin=""): # pedal ret.enableCruise = True - # FIXME: hardcoding honda civic 2016 touring params so they can be used to - # scale unknown params for other cars - mass_civic = 2923. * CV.LB_TO_KG + STD_CARGO_KG - wheelbase_civic = 2.70 - centerToFront_civic = wheelbase_civic * 0.4 - centerToRear_civic = wheelbase_civic - centerToFront_civic - rotationalInertia_civic = 2500 - tireStiffnessFront_civic = 85400 - tireStiffnessRear_civic = 90000 - ret.wheelbase = 2.85 ret.steerRatio = 14.8 ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG @@ -69,32 +59,21 @@ def get_params(candidate, fingerprint, vin=""): ret.lateralTuning.pid.kf = 1. / MAX_ANGLE # MAX Steer angle to normalize FF ret.steerActuatorDelay = 0.1 # Default delay, not measured yet ret.steerRateCost = 1.0 - - f = 1.2 - tireStiffnessFront_civic *= f - tireStiffnessRear_civic *= f - ret.centerToFront = ret.wheelbase * 0.44 - + tire_stiffness_factor = 0.5328 # min speed to enable ACC. if car can do stop and go, then set enabling speed # to a negative value, so it won't matter. ret.minEnableSpeed = -1. - centerToRear = ret.wheelbase - ret.centerToFront # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase - ret.rotationalInertia = rotationalInertia_civic * \ - ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2) + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) # TODO: start from empirically derived lateral slip stiffness for the civic and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors - ret.tireStiffnessFront = tireStiffnessFront_civic * \ - ret.mass / mass_civic * \ - (centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic) - ret.tireStiffnessRear = tireStiffnessRear_civic * \ - ret.mass / mass_civic * \ - (ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic) + ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, + tire_stiffness_factor=tire_stiffness_factor) # no rear steering, at least on the listed cars above ret.steerRatioRear = 0. diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 4a909bc84403ff..c066a6523f2213 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -7,7 +7,7 @@ from selfdrive.car.gm.values import DBC, CAR, STOCK_CONTROL_MSGS, AUDIO_HUD, \ SUPERCRUISE_CARS, AccState from selfdrive.car.gm.carstate import CarState, CruiseButtons, get_powertrain_can_parser -from selfdrive.car import STD_CARGO_KG +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness class CanBus(object): @@ -60,6 +60,7 @@ def get_params(candidate, fingerprint, vin=""): # or camera is on powertrain bus (LKA cars without ACC). ret.enableCamera = not any(x for x in STOCK_CONTROL_MSGS[candidate] if x in fingerprint) ret.openpilotLongitudinalControl = ret.enableCamera + tire_stiffness_factor = 0.444 # not optimized yet if candidate == CAR.VOLT: # supports stop and go, but initial engage must be above 18mph (which include conservatism) @@ -129,30 +130,14 @@ def get_params(candidate, fingerprint, vin=""): ret.centerToFront = ret.wheelbase * 0.465 - # hardcoding honda civic 2016 touring params so they can be used to - # scale unknown params for other cars - mass_civic = 2923. * CV.LB_TO_KG + STD_CARGO_KG - wheelbase_civic = 2.70 - centerToFront_civic = wheelbase_civic * 0.4 - centerToRear_civic = wheelbase_civic - centerToFront_civic - rotationalInertia_civic = 2500 - tireStiffnessFront_civic = 85400 - tireStiffnessRear_civic = 90000 - - centerToRear = ret.wheelbase - ret.centerToFront # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase - ret.rotationalInertia = rotationalInertia_civic * \ - ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2) + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) # TODO: start from empirically derived lateral slip stiffness for the civic and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors - ret.tireStiffnessFront = tireStiffnessFront_civic * \ - ret.mass / mass_civic * \ - (centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic) - ret.tireStiffnessRear = tireStiffnessRear_civic * \ - ret.mass / mass_civic * \ - (ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic) + ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, + tire_stiffness_factor=tire_stiffness_factor) # same tuning for Volt and CT6 for now ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 72c85c862cea57..93aaf0182b7db6 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -10,7 +10,7 @@ from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.honda.carstate import CarState, get_can_parser, get_cam_can_parser from selfdrive.car.honda.values import CruiseButtons, CAR, HONDA_BOSCH, AUDIO_HUD, VISUAL_HUD, CAMERA_MSGS -from selfdrive.car import STD_CARGO_KG +from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness from selfdrive.controls.lib.planner import _A_CRUISE_MAX_V_FOLLOWING A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING) @@ -152,16 +152,6 @@ def get_params(candidate, fingerprint, vin=""): ret.enableCruise = not ret.enableGasInterceptor - # FIXME: hardcoding honda civic 2016 touring params so they can be used to - # scale unknown params for other cars - mass_civic = 2923. * CV.LB_TO_KG + STD_CARGO_KG - wheelbase_civic = 2.70 - centerToFront_civic = wheelbase_civic * 0.4 - centerToRear_civic = wheelbase_civic - centerToFront_civic - rotationalInertia_civic = 2500 - tireStiffnessFront_civic = 192150 - tireStiffnessRear_civic = 202500 - # Optimized car params: tire_stiffness_factor and steerRatio are a result of a vehicle # model optimization process. Certain Hondas have an extra steering sensor at the bottom # of the steering rack, which improves controls quality as it removes the steering column @@ -174,9 +164,9 @@ def get_params(candidate, fingerprint, vin=""): if candidate in [CAR.CIVIC, CAR.CIVIC_BOSCH]: stop_and_go = True - ret.mass = mass_civic - ret.wheelbase = wheelbase_civic - ret.centerToFront = centerToFront_civic + ret.mass = CivicParams.MASS + ret.wheelbase = CivicParams.WHEELBASE + ret.centerToFront = CivicParams.CENTER_TO_FRONT ret.steerRatio = 14.63 # 10.93 is end-to-end spec tire_stiffness_factor = 1. # Civic at comma has modified steering FW, so different tuning for the Neo in that car @@ -334,20 +324,14 @@ def get_params(candidate, fingerprint, vin=""): # conflict with PCM acc ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else 25.5 * CV.MPH_TO_MS - centerToRear = ret.wheelbase - ret.centerToFront # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase - ret.rotationalInertia = rotationalInertia_civic * \ - ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2) + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) # TODO: start from empirically derived lateral slip stiffness for the civic and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors - ret.tireStiffnessFront = (tireStiffnessFront_civic * tire_stiffness_factor) * \ - ret.mass / mass_civic * \ - (centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic) - ret.tireStiffnessRear = (tireStiffnessRear_civic * tire_stiffness_factor) * \ - ret.mass / mass_civic * \ - (ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic) + ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, + tire_stiffness_factor=tire_stiffness_factor) # no rear steering, at least on the listed cars above ret.steerRatioRear = 0. diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 1a7130d1bad2aa..b90be1525cfe49 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -1,9 +1,14 @@ from selfdrive.car import limit_steer_rate from selfdrive.car.hyundai.hyundaican import create_lkas11, create_lkas12, \ create_1191, create_1156, \ - create_clu11, learn_checksum + create_clu11, learn_checksum, create_mdps12 from selfdrive.car.hyundai.values import Buttons from selfdrive.can.packer import CANPacker +import zmq +from selfdrive.services import service_list +import selfdrive.messaging as messaging +from selfdrive.config import Conversions as CV +from common.params import Params # Steer torque limits @@ -24,9 +29,19 @@ def __init__(self, dbc_name, car_fingerprint): self.lkas11_cnt = 0 self.clu11_cnt = 0 + self.mdps12_cnt = 0 self.cnt = 0 self.last_resume_cnt = 0 + self.map_speed = 0 + #context = zmq.Context() + #self.map_data_sock = messaging.sub_sock(context, service_list['liveMapData'].port, conflate=True) + #self.params = Params() + self.speed_conv = 3.6 + self.speed_offset = 1.03 # Multiplier for cruise speed vs speed limit TODO: Add to UI + self.speed_enable = True # Enable Auto Speed Set TODO: Add to UI + self.speed_adjusted = False + self.checksum = "NONE" self.checksum_learn_cnt = 0 @@ -39,6 +54,7 @@ def __init__(self, dbc_name, car_fingerprint): def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): ### Error State Resets ### disable_steer = False + can_sends = [] ### Learn Checksum ### @@ -53,6 +69,7 @@ def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): # If MDPS is faulted from bad checksum, then cycle through all Checksums until 1 works if CS.steer_error == 1: self.camera_disconnected = True + print ("Camera Not Detected: Brute Forcing Checksums") if self.checksum_learn_cnt > 250: self.checksum_learn_cnt = 50 if self.checksum == "NONE": @@ -72,7 +89,7 @@ def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): ### Minimum Steer Speed ### # Learn Minimum Steer Speed - if CS.mdps12_flt != 0 and CS.v_ego_raw > 0.: + if CS.mdps12_flt != 0 and CS.v_ego_raw > 0. and abs(CS.angle_steers) < 10.0 : if CS.v_ego_raw > self.min_steer_speed: self.min_steer_speed = CS.v_ego_raw + 0.1 print ("Discovered new Min Speed as", self.min_steer_speed) @@ -81,7 +98,6 @@ def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): if CS.v_ego_raw < self.min_steer_speed: disable_steer = True - ### Turning Indicators ### if (CS.left_blinker_on == 1 or CS.right_blinker_on == 1): self.turning_signal_timer = 100 # Disable for 1.0 Seconds after blinker turned off @@ -101,12 +117,53 @@ def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): self.apply_steer_last = apply_steer + ''' + ### Auto Speed Limit ### + + # Read Speed Limit and define if adjustment needed + if (self.cnt % 50) == 0 and self.speed_enable: + if not (enabled and CS.acc_active): + self.speed_adjusted = False + map_data = messaging.recv_one_or_none(self.map_data_sock) + if map_data is not None: + if bool(self.params.get("IsMetric")): + self.speed_conv = CV.MS_TO_KPH + else: + self.speed_conv = CV.MS_TO_MPH + + if map_data.liveMapData.speedLimitValid: + last_speed = self.map_speed + v_speed = int(map_data.liveMapData.speedLimit * self.speed_offset) + self.map_speed = v_speed * self.speed_conv + if last_speed != self.map_speed: + self.speed_adjusted = False + else: + self.map_speed = 0 + self.speed_adjusted = True + else: + self.map_speed = 0 + self.speed_adjusted = True + + # Spam buttons for Speed Adjustment + if CS.acc_active and not self.speed_adjusted and self.map_speed > (8.5 * self.speed_conv) and (self.cnt % 9 == 0 or self.cnt % 9 == 1): + if (CS.cruise_set_speed * self.speed_conv) > (self.map_speed * 1.005): + can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.SET_DECEL, (1 if self.cnt % 9 == 1 else 0))) + elif (CS.cruise_set_speed * self.speed_conv) < (self.map_speed / 1.005): + can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL, (1 if self.cnt % 9 == 1 else 0))) + else: + self.speed_adjusted = True + + # Cancel Adjustment on Pedal + if CS.pedal_gas: + self.speed_adjusted = True + + ''' ### Generate CAN Messages ### - can_sends = [] self.lkas11_cnt = self.cnt % 0x10 self.clu11_cnt = self.cnt % 0x10 + self.mdps12_cnt = self.cnt % 0x100 if self.camera_disconnected: if (self.cnt % 10) == 0: @@ -119,11 +176,15 @@ def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): can_sends.append(create_lkas11(self.packer, self.car_fingerprint, apply_steer, steer_req, self.lkas11_cnt, enabled, CS.lkas11, hud_alert, (not self.camera_disconnected), self.checksum)) + if not self.camera_disconnected: + can_sends.append(create_mdps12(self.packer, self.car_fingerprint, self.mdps12_cnt, CS.mdps12, CS.lkas11, \ + self.checksum)) + if pcm_cancel_cmd: - can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.CANCEL)) + can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.CANCEL, 0)) elif CS.stopped and (self.cnt - self.last_resume_cnt) > 5: self.last_resume_cnt = self.cnt - can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL)) + can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL, 0)) self.cnt += 1 diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index e982d343c80f54..684ef84ec116b7 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -159,6 +159,10 @@ def __init__(self, CP): self.right_blinker_on = 0 self.right_blinker_flash = 0 self.has_scc = False + self.min_steer_speed = 0 + + def update_min_speed(speed): + self.min_steer_speed = speed def update(self, cp, cp_cam): if (cp.vl["SCC11"]['TauGapSet'] > 0): @@ -270,6 +274,7 @@ def update(self, cp, cp_cam): else: self.gear_tcu = "unknown" - # save the entire LKAS11 and CLU11 + # save the entire LKAS11, CLU11 and MDPS12 messages self.lkas11 = cp_cam.vl["LKAS11"] self.clu11 = cp.vl["CLU11"] + self.mdps12 = cp.vl["MDPS12"] diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index f1999bdece50a6..7590e5601a452b 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -58,7 +58,7 @@ def create_1191(): def create_1156(): return make_can_msg(1156, "\x08\x20\xfe\x3f\x00\xe0\xfd\x3f", 0) -def create_clu11(packer, clu11, button): +def create_clu11(packer, clu11, button, cnt): values = { "CF_Clu_CruiseSwState": button, "CF_Clu_CruiseSwMain": clu11["CF_Clu_CruiseSwMain"], @@ -71,12 +71,12 @@ def create_clu11(packer, clu11, button): "CF_Clu_RheostatLevel": clu11["CF_Clu_RheostatLevel"], "CF_Clu_CluInfo": clu11["CF_Clu_CluInfo"], "CF_Clu_AmpInfo": clu11["CF_Clu_AmpInfo"], - "CF_Clu_AliveCnt1": 0, + "CF_Clu_AliveCnt1": cnt, } return packer.make_can_msg("CLU11", 0, values) -def create_mdps12(packer, car_fingerprint, cnt, mdps12, lkas11, camcan, checksum): +def create_mdps12(packer, car_fingerprint, cnt, mdps12, lkas11, checksum): values = { "CR_Mdps_StrColTq": mdps12["CR_Mdps_StrColTq"], "CF_Mdps_Def": mdps12["CF_Mdps_Def"], @@ -92,12 +92,12 @@ def create_mdps12(packer, car_fingerprint, cnt, mdps12, lkas11, camcan, checksum } if not (checksum == "crc8"): - dat = packer.make_can_msg("MDPS12", camcan, values)[2] + dat = packer.make_can_msg("MDPS12", 2, values)[2] dat = [ord(i) for i in dat] checksum = (dat[0] + dat[1] + dat[2] + dat[4] + dat[5] + dat[6] + dat[7]) % 256 values["CF_Mdps_Chksum2"] = checksum - return packer.make_can_msg("MDPS12", camcan, values) + return packer.make_can_msg("MDPS12", 2, values) def learn_checksum(packer, lkas11): values = { diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 6b7619656cfba7..05da0bf6aa9230 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -6,8 +6,7 @@ from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.hyundai.carstate import CarState, get_can_parser, get_camera_parser from selfdrive.car.hyundai.values import CAR, get_hud_alerts -from selfdrive.car import STD_CARGO_KG - +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness class CarInterface(object): def __init__(self, CP, CarController): @@ -51,18 +50,8 @@ def get_params(candidate, fingerprint, vin=""): ret.safetyModel = car.CarParams.SafetyModel.hyundai ret.enableCruise = True # stock acc - # FIXME: hardcoding honda civic 2016 touring params so they can be used to - # scale unknown params for other cars - mass_civic = 2923. * CV.LB_TO_KG + STD_CARGO_KG - wheelbase_civic = 2.70 - centerToFront_civic = wheelbase_civic * 0.4 - centerToRear_civic = wheelbase_civic - centerToFront_civic - rotationalInertia_civic = 2500 - tireStiffnessFront_civic = 192150 - tireStiffnessRear_civic = 202500 tire_stiffness_factor = 1. - # Hyundai Kia and Genesis Starting Values # Auto Tune corrects anything from here on out. @@ -87,21 +76,14 @@ def get_params(candidate, fingerprint, vin=""): ret.centerToFront = ret.wheelbase * 0.4 - centerToRear = ret.wheelbase - ret.centerToFront - # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase - ret.rotationalInertia = rotationalInertia_civic * \ - ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2) + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) # TODO: start from empirically derived lateral slip stiffness for the civic and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors - ret.tireStiffnessFront = (tireStiffnessFront_civic * tire_stiffness_factor) * \ - ret.mass / mass_civic * \ - (centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic) - ret.tireStiffnessRear = (tireStiffnessRear_civic * tire_stiffness_factor) * \ - ret.mass / mass_civic * \ - (ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic) + ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, + tire_stiffness_factor=tire_stiffness_factor) # no rear steering, at least on the listed cars above @@ -196,9 +178,9 @@ def update(self, c): ret.seatbeltUnlatched = not self.CS.seatbelt # low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s) - if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.: + if ret.vEgo < (self.CS.min_steer_speed + 2.) and self.CS.min_steer_speed > 10.: self.low_speed_alert = True - if ret.vEgo > (self.CP.minSteerSpeed + 4.): + if ret.vEgo > (self.CS.min_steer_speed + 4.): self.low_speed_alert = False events = [] diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index a609ee9092c115..e9d9c117fc714c 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -6,7 +6,7 @@ from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.subaru.values import CAR from selfdrive.car.subaru.carstate import CarState, get_powertrain_can_parser, get_camera_can_parser -from selfdrive.car import STD_CARGO_KG +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness class CarInterface(object): @@ -58,7 +58,6 @@ def get_params(candidate, fingerprint, vin=""): ret.wheelbase = 2.67 ret.centerToFront = ret.wheelbase * 0.5 ret.steerRatio = 15 - tire_stiffness_factor = 1.0 ret.steerActuatorDelay = 0.4 # end-to-end angle controller ret.lateralTuning.pid.kf = 0.00005 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 20.], [0., 20.]] @@ -84,30 +83,13 @@ def get_params(candidate, fingerprint, vin=""): # end from gm - # hardcoding honda civic 2016 touring params so they can be used to - # scale unknown params for other cars - mass_civic = 2923. * CV.LB_TO_KG + STD_CARGO_KG - wheelbase_civic = 2.70 - centerToFront_civic = wheelbase_civic * 0.4 - centerToRear_civic = wheelbase_civic - centerToFront_civic - rotationalInertia_civic = 2500 - tireStiffnessFront_civic = 192150 - tireStiffnessRear_civic = 202500 - centerToRear = ret.wheelbase - ret.centerToFront - # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase - ret.rotationalInertia = rotationalInertia_civic * \ - ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2) + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) # TODO: start from empirically derived lateral slip stiffness for the civic and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors - ret.tireStiffnessFront = (tireStiffnessFront_civic * tire_stiffness_factor) * \ - ret.mass / mass_civic * \ - (centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic) - ret.tireStiffnessRear = (tireStiffnessRear_civic * tire_stiffness_factor) * \ - ret.mass / mass_civic * \ - (ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic) + ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront) return ret diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 83bf921d95d6dd..a1cffae9594942 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -6,7 +6,7 @@ from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.toyota.carstate import CarState, get_can_parser, get_cam_can_parser from selfdrive.car.toyota.values import ECU, check_ecu_msgs, CAR, NO_STOP_TIMER_CAR -from selfdrive.car import STD_CARGO_KG +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness from selfdrive.swaglog import cloudlog @@ -54,16 +54,6 @@ def get_params(candidate, fingerprint, vin=""): # pedal ret.enableCruise = not ret.enableGasInterceptor - # FIXME: hardcoding honda civic 2016 touring params so they can be used to - # scale unknown params for other cars - mass_civic = 2923. * CV.LB_TO_KG + STD_CARGO_KG - wheelbase_civic = 2.70 - centerToFront_civic = wheelbase_civic * 0.4 - centerToRear_civic = wheelbase_civic - centerToFront_civic - rotationalInertia_civic = 2500 - tireStiffnessFront_civic = 192150 - tireStiffnessRear_civic = 202500 - ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay if candidate != CAR.PRIUS: ret.lateralTuning.init('pid') @@ -100,7 +90,7 @@ def get_params(candidate, fingerprint, vin=""): ret.safetyParam = 100 ret.wheelbase = 2.70 ret.steerRatio = 17.8 - tire_stiffness_factor = 0.444 + tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]] ret.lateralTuning.pid.kf = 0.00003 # full torque for 20 deg at 80mph means 0.00007818594 @@ -140,7 +130,7 @@ def get_params(candidate, fingerprint, vin=""): ret.safetyParam = 73 ret.wheelbase = 2.78 ret.steerRatio = 16.0 - tire_stiffness_factor = 0.444 # not optimized yet + tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 4607. * CV.LB_TO_KG + STD_CARGO_KG #mean between normal and hybrid limited ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.05]] ret.lateralTuning.pid.kf = 0.00006 @@ -170,7 +160,7 @@ def get_params(candidate, fingerprint, vin=""): ret.safetyParam = 73 ret.wheelbase = 2.63906 ret.steerRatio = 13.9 - tire_stiffness_factor = 0.444 + tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] ret.lateralTuning.pid.kf = 0.00007818594 @@ -179,8 +169,8 @@ def get_params(candidate, fingerprint, vin=""): stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.8702 - ret.steerRatio = 16.0 #not optimized - tire_stiffness_factor = 0.444 + ret.steerRatio = 16.0 # not optimized + tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 3704. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] ret.lateralTuning.pid.kf = 0.00007818594 @@ -195,20 +185,14 @@ def get_params(candidate, fingerprint, vin=""): # to a negative value, so it won't matter. ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else 19. * CV.MPH_TO_MS - centerToRear = ret.wheelbase - ret.centerToFront # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase - ret.rotationalInertia = rotationalInertia_civic * \ - ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2) + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) # TODO: start from empirically derived lateral slip stiffness for the civic and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors - ret.tireStiffnessFront = (tireStiffnessFront_civic * tire_stiffness_factor) * \ - ret.mass / mass_civic * \ - (centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic) - ret.tireStiffnessRear = (tireStiffnessRear_civic * tire_stiffness_factor) * \ - ret.mass / mass_civic * \ - (ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic) + ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, + tire_stiffness_factor=tire_stiffness_factor) # no rear steering, at least on the listed cars above ret.steerRatioRear = 0. diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index f487888424942d..da8b02dcd7212c 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -165,9 +165,9 @@ def check_ecu_msgs(fingerprint, ecu): { 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 355: 5, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 565: 8, 608: 8, 610: 8, 643: 7, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1076: 8, 1077: 8,1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1235: 8, 1279: 8, 1541: 8, 1552: 8, 1553:8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 }, - # XLE and AWD + # XLE, Limited, and AWD { - 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 565: 8, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 889: 8, 891: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 987: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 + 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 565: 8, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 889: 8, 891: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 987: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8 }], CAR.COROLLA_TSS2: [ # hatch 2019+ and sedan 2020+ diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index 3710c7d39598f6..219f916614ba41 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.6.0.2-ku7" +#define COMMA_VERSION "0.6.0.3-ku7" \ No newline at end of file diff --git a/selfdrive/crash.py b/selfdrive/crash.py index fe7e328e679f14..89cd420570755a 100644 --- a/selfdrive/crash.py +++ b/selfdrive/crash.py @@ -19,6 +19,44 @@ def install(): else: from raven import Client from raven.transport.http import HTTPTransport + import json + + error_tags = {'dirty': dirty, 'username': 'char_error'} + + try: + with open("/data/data/ai.comma.plus.offroad/files/persistStore/persist-auth", "r") as f: + auth = json.loads(f.read()) + auth = json.loads(auth['commaUser']) + tags = ['username', 'email'] + for tag in tags: + try: + error_tags[tag] = ''.join(char for char in auth[tag].decode('utf-8', 'ignore') if char.isalnum()) + except: + pass + except: + pass + + try: + with open("/data/params/d/CommunityPilotUser", "r") as f: + auth = json.loads(f.read()) + tags = ['username', 'email'] + for tag in tags: + try: + error_tags[tag] = ''.join(char for char in auth[tag].decode('utf-8', 'ignore') if char.isalnum()) + except: + pass + except: + pass + + logging_data = {"branch": "/data/params/d/GitBranch", "commit": "/data/params/d/GitCommit", "remote": "/data/params/d/GitRemote"} + + for key in logging_data: + try: + with open(logging_data[key], "r") as f: + error_tags[key] = str(f.read()) + except: + error_tags[key] = "unknown" + client = Client('https://84d713b5bd674bcbb7030d1b86115dcb:80109516f2dd4ee0b9dbb72331930189@sentry.io/1405628', install_sys_hook=False, transport=HTTPTransport, release=version, tags={'dirty': dirty}) @@ -28,6 +66,9 @@ def capture_exception(*args, **kwargs): client.captureException(*args, **kwargs) cloudlog.error("crash", exc_info=kwargs.get('exc_info', 1)) + def capture_warning(warning_string): + client.captureMessage(warning_string, level='warning') + def bind_user(**kwargs): client.user_context(kwargs) diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 8c28a9e2e97f99..bfa38dc72b491b 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -100,6 +100,7 @@ def unblock_stdout(): "plannerd": "selfdrive.controls.plannerd", "radard": "selfdrive.controls.radard", "ubloxd": ("selfdrive/locationd", ["./ubloxd"]), + # "mapd": "selfdrive.mapd.mapd", "loggerd": ("selfdrive/loggerd", ["./loggerd"]), "logmessaged": "selfdrive.logmessaged", "tombstoned": "selfdrive.tombstoned", @@ -151,6 +152,7 @@ def get_running(): 'proclogd', 'ubloxd', 'gpsd', + # 'mapd', 'deleter', ] diff --git a/selfdrive/mapd/__init__.py b/selfdrive/mapd/__init__.py new file mode 100644 index 00000000000000..e69de29bb2d1d6 diff --git a/selfdrive/mapd/default_speeds.json b/selfdrive/mapd/default_speeds.json new file mode 100644 index 00000000000000..7126c27fcdef40 --- /dev/null +++ b/selfdrive/mapd/default_speeds.json @@ -0,0 +1,106 @@ +{ + "_comment": "These speeds are from https://wiki.openstreetmap.org/wiki/Speed_limits Special cases have been stripped", + "AR:urban": "40", + "AR:urban:primary": "60", + "AR:urban:secondary": "60", + "AR:rural": "110", + "AT:urban": "50", + "AT:rural": "100", + "AT:trunk": "100", + "AT:motorway": "130", + "BE:urban": "50", + "BE-VLG:rural": "70", + "BE-WAL:rural": "90", + "BE:trunk": "120", + "BE:motorway": "120", + "CH:urban[1]": "50", + "CH:rural": "80", + "CH:trunk": "100", + "CH:motorway": "120", + "CZ:pedestrian_zone": "20", + "CZ:living_street": "20", + "CZ:urban": "50", + "CZ:urban_trunk": "80", + "CZ:urban_motorway": "80", + "CZ:rural": "90", + "CZ:trunk": "110", + "CZ:motorway": "130", + "DK:urban": "50", + "DK:rural": "80", + "DK:motorway": "130", + "DE:living_street": "7", + "DE:residential": "30", + "DE:urban": "50", + "DE:rural": "100", + "DE:trunk": "none", + "DE:motorway": "none", + "FI:urban": "50", + "FI:rural": "80", + "FI:trunk": "100", + "FI:motorway": "120", + "FR:urban": "50", + "FR:rural": "80", + "FR:trunk": "110", + "FR:motorway": "130", + "GR:urban": "50", + "GR:rural": "90", + "GR:trunk": "110", + "GR:motorway": "130", + "HU:urban": "50", + "HU:rural": "90", + "HU:trunk": "110", + "HU:motorway": "130", + "IT:urban": "50", + "IT:rural": "90", + "IT:trunk": "110", + "IT:motorway": "130", + "JP:national": "60", + "JP:motorway": "100", + "LT:living_street": "20", + "LT:urban": "50", + "LT:rural": "90", + "LT:trunk": "120", + "LT:motorway": "130", + "PL:living_street": "20", + "PL:urban": "50", + "PL:rural": "90", + "PL:trunk": "100", + "PL:motorway": "140", + "RO:urban": "50", + "RO:rural": "90", + "RO:trunk": "100", + "RO:motorway": "130", + "RU:living_street": "20", + "RU:urban": "60", + "RU:rural": "90", + "RU:motorway": "110", + "SK:urban": "50", + "SK:rural": "90", + "SK:trunk": "90", + "SK:motorway": "90", + "SI:urban": "50", + "SI:rural": "90", + "SI:trunk": "110", + "SI:motorway": "130", + "ES:living_street": "20", + "ES:urban": "50", + "ES:rural": "50", + "ES:trunk": "90", + "ES:motorway": "120", + "SE:urban": "50", + "SE:rural": "70", + "SE:trunk": "90", + "SE:motorway": "110", + "GB:nsl_restricted": "30 mph", + "GB:nsl_single": "60 mph", + "GB:nsl_dual": "70 mph", + "GB:motorway": "70 mph", + "UA:urban": "50", + "UA:rural": "90", + "UA:trunk": "110", + "UA:motorway": "130", + "UZ:living_street": "30", + "UZ:urban": "70", + "UZ:rural": "100", + "UZ:motorway": "110" +} diff --git a/selfdrive/mapd/default_speeds_generator.py b/selfdrive/mapd/default_speeds_generator.py new file mode 100755 index 00000000000000..888c9e1df9e933 --- /dev/null +++ b/selfdrive/mapd/default_speeds_generator.py @@ -0,0 +1,240 @@ +#!/usr/bin/env python +import json + +DEFAULT_OUTPUT_FILENAME = "default_speeds_by_region.json" + +def main(filename = DEFAULT_OUTPUT_FILENAME): + countries = [] + + """ + -------------------------------------------------- + US - United State of America + -------------------------------------------------- + """ + US = Country("US") # First step, create the country using the ISO 3166 two letter code + countries.append(US) # Second step, add the country to countries list + + """ Default rules """ + # Third step, add some default rules for the country + # Speed limit rules are based on OpenStreetMaps (OSM) tags. + # The dictionary {...} defines the tag_name: value + # if a road in OSM has a tag with the name tag_name and this value, the speed limit listed below will be applied. + # The text at the end is the speed limit (use no unit for km/h) + # Rules apply in the order in which they are written for each country + # Rules for specific regions (states) take priority over country rules + # If you modify existing country rules, you must update all existing states without that rule to use the old rule + US.add_rule({"highway": "motorway"}, "65 mph") # On US roads with the tag highway and value motorway, the speed limit will default to 65 mph + US.add_rule({"highway": "trunk"}, "55 mph") + US.add_rule({"highway": "primary"}, "55 mph") + US.add_rule({"highway": "secondary"}, "45 mph") + US.add_rule({"highway": "tertiary"}, "35 mph") + US.add_rule({"highway": "unclassified"}, "55 mph") + US.add_rule({"highway": "residential"}, "25 mph") + US.add_rule({"highway": "service"}, "25 mph") + US.add_rule({"highway": "motorway_link"}, "55 mph") + US.add_rule({"highway": "trunk_link"}, "55 mph") + US.add_rule({"highway": "primary_link"}, "55 mph") + US.add_rule({"highway": "secondary_link"}, "45 mph") + US.add_rule({"highway": "tertiary_link"}, "35 mph") + US.add_rule({"highway": "living_street"}, "15 mph") + + """ States """ + new_york = US.add_region("New York") # Fourth step, add a state/region to country + new_york.add_rule({"highway": "primary"}, "45 mph") # Fifth step , add rules to the state. See the text above for how to write rules + new_york.add_rule({"highway": "secondary"}, "55 mph") + new_york.add_rule({"highway": "tertiary"}, "55 mph") + new_york.add_rule({"highway": "residential"}, "30 mph") + new_york.add_rule({"highway": "primary_link"}, "45 mph") + new_york.add_rule({"highway": "secondary_link"}, "55 mph") + new_york.add_rule({"highway": "tertiary_link"}, "55 mph") + # All if not written by the state, the rules will default to the country rules + + #california = US.add_region("California") + # California uses only the default US rules + + michigan = US.add_region("Michigan") + michigan.add_rule({"highway": "motorway"}, "70 mph") + + oregon = US.add_region("Oregon") + oregon.add_rule({"highway": "motorway"}, "55 mph") + oregon.add_rule({"highway": "secondary"}, "35 mph") + oregon.add_rule({"highway": "tertiary"}, "30 mph") + oregon.add_rule({"highway": "service"}, "15 mph") + oregon.add_rule({"highway": "secondary_link"}, "35 mph") + oregon.add_rule({"highway": "tertiary_link"}, "30 mph") + + south_dakota = US.add_region("South Dakota") + south_dakota.add_rule({"highway": "motorway"}, "80 mph") + south_dakota.add_rule({"highway": "trunk"}, "70 mph") + south_dakota.add_rule({"highway": "primary"}, "65 mph") + south_dakota.add_rule({"highway": "trunk_link"}, "70 mph") + south_dakota.add_rule({"highway": "primary_link"}, "65 mph") + + wisconsin = US.add_region("Wisconsin") + wisconsin.add_rule({"highway": "trunk"}, "65 mph") + wisconsin.add_rule({"highway": "tertiary"}, "45 mph") + wisconsin.add_rule({"highway": "unclassified"}, "35 mph") + wisconsin.add_rule({"highway": "trunk_link"}, "65 mph") + wisconsin.add_rule({"highway": "tertiary_link"}, "45 mph") + + """ + -------------------------------------------------- + AU - Australia + -------------------------------------------------- + """ + AU = Country("AU") + countries.append(AU) + + """ Default rules """ + AU.add_rule({"highway": "motorway"}, "100") + AU.add_rule({"highway": "trunk"}, "80") + AU.add_rule({"highway": "primary"}, "80") + AU.add_rule({"highway": "secondary"}, "50") + AU.add_rule({"highway": "tertiary"}, "50") + AU.add_rule({"highway": "unclassified"}, "80") + AU.add_rule({"highway": "residential"}, "50") + AU.add_rule({"highway": "service"}, "40") + AU.add_rule({"highway": "motorway_link"}, "90") + AU.add_rule({"highway": "trunk_link"}, "80") + AU.add_rule({"highway": "primary_link"}, "80") + AU.add_rule({"highway": "secondary_link"}, "50") + AU.add_rule({"highway": "tertiary_link"}, "50") + AU.add_rule({"highway": "living_street"}, "30") + + """ + -------------------------------------------------- + CA - Canada + -------------------------------------------------- + """ + CA = Country("CA") + countries.append(CA) + + """ Default rules """ + CA.add_rule({"highway": "motorway"}, "100") + CA.add_rule({"highway": "trunk"}, "80") + CA.add_rule({"highway": "primary"}, "80") + CA.add_rule({"highway": "secondary"}, "50") + CA.add_rule({"highway": "tertiary"}, "50") + CA.add_rule({"highway": "unclassified"}, "80") + CA.add_rule({"highway": "residential"}, "40") + CA.add_rule({"highway": "service"}, "40") + CA.add_rule({"highway": "motorway_link"}, "90") + CA.add_rule({"highway": "trunk_link"}, "80") + CA.add_rule({"highway": "primary_link"}, "80") + CA.add_rule({"highway": "secondary_link"}, "50") + CA.add_rule({"highway": "tertiary_link"}, "50") + CA.add_rule({"highway": "living_street"}, "20") + + + """ + -------------------------------------------------- + DE - Germany + -------------------------------------------------- + """ + DE = Country("DE") + countries.append(DE) + + """ Default rules """ + DE.add_rule({"highway": "motorway"}, "none") + DE.add_rule({"highway": "living_street"}, "10") + DE.add_rule({"highway": "residential"}, "30") + DE.add_rule({"zone:traffic": "DE:rural"}, "100") + DE.add_rule({"zone:traffic": "DE:urban"}, "50") + DE.add_rule({"zone:maxspeed": "DE:30"}, "30") + DE.add_rule({"zone:maxspeed": "DE:urban"}, "50") + DE.add_rule({"zone:maxspeed": "DE:rural"}, "100") + DE.add_rule({"zone:maxspeed": "DE:motorway"}, "none") + DE.add_rule({"bicycle_road": "yes"}, "30") + + + """ + -------------------------------------------------- + EE - Estonia + -------------------------------------------------- + """ + EE = Country("EE") + countries.append(EE) + + """ Default rules """ + EE.add_rule({"highway": "motorway"}, "90") + EE.add_rule({"highway": "trunk"}, "90") + EE.add_rule({"highway": "primary"}, "90") + EE.add_rule({"highway": "secondary"}, "50") + EE.add_rule({"highway": "tertiary"}, "50") + EE.add_rule({"highway": "unclassified"}, "90") + EE.add_rule({"highway": "residential"}, "40") + EE.add_rule({"highway": "service"}, "40") + EE.add_rule({"highway": "motorway_link"}, "90") + EE.add_rule({"highway": "trunk_link"}, "70") + EE.add_rule({"highway": "primary_link"}, "70") + EE.add_rule({"highway": "secondary_link"}, "50") + EE.add_rule({"highway": "tertiary_link"}, "50") + EE.add_rule({"highway": "living_street"}, "20") + + + """ --- DO NOT MODIFY CODE BELOW THIS LINE --- """ + """ --- ADD YOUR COUNTRY OR STATE ABOVE --- """ + + # Final step + write_json(countries, filename) + +def write_json(countries, filename = DEFAULT_OUTPUT_FILENAME): + out_dict = {} + for country in countries: + out_dict.update(country.jsonify()) + json_string = json.dumps(out_dict, indent=2) + with open(filename, "wb") as f: + f.write(json_string) + + +class Region(object): + ALLOWABLE_TAG_KEYS = ["highway", "zone:traffic", "bicycle_road", "zone:maxspeed"] + ALLOWABLE_HIGHWAY_TYPES = ["motorway", "trunk", "primary", "secondary", "tertiary", "unclassified", "residential", "service", "motorway_link", "trunk_link", "primary_link", "secondary_link", "tertiary_link", "living_street"] + def __init__(self, name): + self.name = name + self.rules = [] + + def add_rule(self, tag_conditions, speed): + new_rule = {} + if not isinstance(tag_conditions, dict): + raise TypeError("Rule tag conditions must be dictionary") + if not all(tag_key in self.ALLOWABLE_TAG_KEYS for tag_key in tag_conditions): + raise ValueError("Rule tag keys must be in allowable tag kesy") # If this is by mistake, please update ALLOWABLE_TAG_KEYS + if 'highway' in tag_conditions: + if not tag_conditions['highway'] in self.ALLOWABLE_HIGHWAY_TYPES: + raise ValueError("Invalid Highway type {}".format(tag_conditions["highway"])) + new_rule['tags'] = tag_conditions + try: + new_rule['speed'] = str(speed) + except ValueError: + raise ValueError("Rule speed must be string") + self.rules.append(new_rule) + + def jsonify(self): + ret_dict = {} + ret_dict[self.name] = self.rules + return ret_dict + +class Country(Region): + ALLOWABLE_COUNTRY_CODES = ["AF","AX","AL","DZ","AS","AD","AO","AI","AQ","AG","AR","AM","AW","AU","AT","AZ","BS","BH","BD","BB","BY","BE","BZ","BJ","BM","BT","BO","BQ","BA","BW","BV","BR","IO","BN","BG","BF","BI","KH","CM","CA","CV","KY","CF","TD","CL","CN","CX","CC","CO","KM","CG","CD","CK","CR","CI","HR","CU","CW","CY","CZ","DK","DJ","DM","DO","EC","EG","SV","GQ","ER","EE","ET","FK","FO","FJ","FI","FR","GF","PF","TF","GA","GM","GE","DE","GH","GI","GR","GL","GD","GP","GU","GT","GG","GN","GW","GY","HT","HM","VA","HN","HK","HU","IS","IN","ID","IR","IQ","IE","IM","IL","IT","JM","JP","JE","JO","KZ","KE","KI","KP","KR","KW","KG","LA","LV","LB","LS","LR","LY","LI","LT","LU","MO","MK","MG","MW","MY","MV","ML","MT","MH","MQ","MR","MU","YT","MX","FM","MD","MC","MN","ME","MS","MA","MZ","MM","NA","NR","NP","NL","NC","NZ","NI","NE","NG","NU","NF","MP","NO","OM","PK","PW","PS","PA","PG","PY","PE","PH","PN","PL","PT","PR","QA","RE","RO","RU","RW","BL","SH","KN","LC","MF","PM","VC","WS","SM","ST","SA","SN","RS","SC","SL","SG","SX","SK","SI","SB","SO","ZA","GS","SS","ES","LK","SD","SR","SJ","SZ","SE","CH","SY","TW","TJ","TZ","TH","TL","TG","TK","TO","TT","TN","TR","TM","TC","TV","UG","UA","AE","GB","US","UM","UY","UZ","VU","VE","VN","VG","VI","WF","EH","YE","ZM","ZW"] + def __init__(self, ISO_3166_alpha_2): + Region.__init__(self, ISO_3166_alpha_2) + if ISO_3166_alpha_2 not in self.ALLOWABLE_COUNTRY_CODES: + raise ValueError("Not valid IOS 3166 country code") + self.regions = {} + + def add_region(self, name): + self.regions[name] = Region(name) + return self.regions[name] + + def jsonify(self): + ret_dict = {} + ret_dict[self.name] = {} + for r_name, region in self.regions.items(): + ret_dict[self.name].update(region.jsonify()) + ret_dict[self.name]['Default'] = self.rules + return ret_dict + + +if __name__ == '__main__': + main() diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py new file mode 100755 index 00000000000000..f20c8ba3da12bf --- /dev/null +++ b/selfdrive/mapd/mapd.py @@ -0,0 +1,341 @@ +#!/usr/bin/env python + +# Add phonelibs openblas to LD_LIBRARY_PATH if import fails +from common.basedir import BASEDIR +try: + from scipy import spatial +except ImportError as e: + import os + import sys + + + openblas_path = os.path.join(BASEDIR, "phonelibs/openblas/") + os.environ['LD_LIBRARY_PATH'] += ':' + openblas_path + + args = [sys.executable] + args.extend(sys.argv) + os.execv(sys.executable, args) + +DEFAULT_SPEEDS_BY_REGION_JSON_FILE = BASEDIR + "/selfdrive/mapd/default_speeds_by_region.json" +from selfdrive.mapd import default_speeds_generator +default_speeds_generator.main(DEFAULT_SPEEDS_BY_REGION_JSON_FILE) + +import os +import sys +import time +import zmq +import threading +import numpy as np +import overpy +from collections import defaultdict +from math import sin,cos +from common.params import Params +from common.transformations.coordinates import geodetic2ecef +from selfdrive.services import service_list +import selfdrive.messaging as messaging +from selfdrive.mapd.mapd_helpers import MAPS_LOOKAHEAD_DISTANCE, Way, circle_through_points +import selfdrive.crash as crash +from selfdrive.version import version, dirty + + +OVERPASS_API_URL = "https://overpass.kumi.systems/api/interpreter" +OVERPASS_HEADERS = { + 'User-Agent': 'NEOS (comma.ai)', + 'Accept-Encoding': 'gzip' +} + +last_gps = None +query_lock = threading.Lock() +last_query_result = None +last_query_pos = None +cache_valid = False + +def build_way_query(lat, lon, radius=50): + """Builds a query to find all highways within a given radius around a point""" + pos = " (around:%f,%f,%f)" % (radius, lat, lon) + lat_lon = "(%f,%f)" % (lat, lon) + q = """( + way + """ + pos + """ + [highway][highway!~"^(footway|path|bridleway|steps|cycleway|construction|bus_guideway|escape)$"]; + >;);out;""" + """is_in""" + lat_lon + """;area._[admin_level~"[24]"]; + convert area ::id = id(), admin_level = t['admin_level'], + name = t['name'], "ISO3166-1:alpha2" = t['ISO3166-1:alpha2'];out; + """ + return q + + +def query_thread(): + global last_query_result, last_query_pos, cache_valid + api = overpy.Overpass(url=OVERPASS_API_URL, headers=OVERPASS_HEADERS, timeout=10.) + + while True: + time.sleep(1) + if last_gps is not None: + fix_ok = last_gps.flags & 1 + if not fix_ok: + continue + + if last_query_pos is not None: + cur_ecef = geodetic2ecef((last_gps.latitude, last_gps.longitude, last_gps.altitude)) + prev_ecef = geodetic2ecef((last_query_pos.latitude, last_query_pos.longitude, last_query_pos.altitude)) + dist = np.linalg.norm(cur_ecef - prev_ecef) + if dist < 3000: #updated when we are 1km from the edge of the downloaded circle + continue + + if dist > 4000: + cache_valid = False + + q = build_way_query(last_gps.latitude, last_gps.longitude, radius=4000) + try: + new_result = api.query(q) + + # Build kd-tree + nodes = [] + real_nodes = [] + node_to_way = defaultdict(list) + location_info = {} + + for n in new_result.nodes: + nodes.append((float(n.lat), float(n.lon), 0)) + real_nodes.append(n) + + for way in new_result.ways: + for n in way.nodes: + node_to_way[n.id].append(way) + + for area in new_result.areas: + if area.tags.get('admin_level', '') == "2": + location_info['country'] = area.tags.get('ISO3166-1:alpha2', '') + if area.tags.get('admin_level', '') == "4": + location_info['region'] = area.tags.get('name', '') + + nodes = np.asarray(nodes) + nodes = geodetic2ecef(nodes) + tree = spatial.cKDTree(nodes) + + query_lock.acquire() + last_query_result = new_result, tree, real_nodes, node_to_way, location_info + last_query_pos = last_gps + cache_valid = True + query_lock.release() + + except Exception as e: + print(e) + query_lock.acquire() + last_query_result = None + query_lock.release() + +def save_gps_data(gps): + try: + location = [gps.speed, gps.bearing, gps.latitude, gps.longitude, gps.altitude, gps.accuracy, time.time()] + with open("/data/openpilot/selfdrive/data_collection/gps-data", "a") as f: + f.write("{}\n".format(location)) + except: + pass + +def mapsd_thread(): + global last_gps + + context = zmq.Context() + poller = zmq.Poller() + gps_sock = messaging.sub_sock(context, service_list['gpsLocation'].port, conflate=True) + gps_external_sock = messaging.sub_sock(context, service_list['gpsLocationExternal'].port, conflate=True, poller=poller) + map_data_sock = messaging.pub_sock(context, service_list['liveMapData'].port) + + cur_way = None + curvature_valid = False + curvature = None + upcoming_curvature = 0. + dist_to_turn = 0. + road_points = None + max_speed = None + max_speed_ahead = None + max_speed_ahead_dist = None + + max_speed_prev = 0 + + while True: + gps = messaging.recv_one(gps_sock) + gps_ext = None + for socket, event in poller.poll(0): + if socket is gps_external_sock: + gps_ext = messaging.recv_one(socket) + if gps_ext is not None: + gps = gps_ext.gpsLocationExternal + else: + gps = gps.gpsLocation + + save_gps_data(gps) + + last_gps = gps + + fix_ok = gps.flags & 1 + if not fix_ok or last_query_result is None or not cache_valid: + cur_way = None + curvature = None + max_speed_ahead = None + max_speed_ahead_dist = None + curvature_valid = False + upcoming_curvature = 0. + dist_to_turn = 0. + road_points = None + map_valid = False + else: + map_valid = True + lat = gps.latitude + lon = gps.longitude + heading = gps.bearing + speed = gps.speed + + query_lock.acquire() + cur_way = Way.closest(last_query_result, lat, lon, heading, cur_way) + if cur_way is not None: + pnts, curvature_valid = cur_way.get_lookahead(lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE) + + xs = pnts[:, 0] + ys = pnts[:, 1] + road_points = [float(x) for x in xs], [float(y) for y in ys] + + if speed < 10: + curvature_valid = False + if curvature_valid and pnts.shape[0] <= 3: + curvature_valid = False + + # The curvature is valid when at least MAPS_LOOKAHEAD_DISTANCE of road is found + if curvature_valid: + # Compute the curvature for each point + with np.errstate(divide='ignore'): + circles = [circle_through_points(*p) for p in zip(pnts, pnts[1:], pnts[2:])] + circles = np.asarray(circles) + radii = np.nan_to_num(circles[:, 2]) + radii[radii < 10] = np.inf + curvature = 1. / radii + + # Index of closest point + closest = np.argmin(np.linalg.norm(pnts, axis=1)) + dist_to_closest = pnts[closest, 0] # We can use x distance here since it should be close + + # Compute distance along path + dists = list() + dists.append(0) + for p, p_prev in zip(pnts, pnts[1:, :]): + dists.append(dists[-1] + np.linalg.norm(p - p_prev)) + dists = np.asarray(dists) + dists = dists - dists[closest] + dist_to_closest + dists = dists[1:-1] + + close_idx = np.logical_and(dists > 0, dists < 500) + dists = dists[close_idx] + curvature = curvature[close_idx] + + if len(curvature): + # TODO: Determine left or right turn + curvature = np.nan_to_num(curvature) + + # Outlier rejection + new_curvature = np.percentile(curvature, 90, interpolation='lower') + + k = 0.6 + upcoming_curvature = k * upcoming_curvature + (1 - k) * new_curvature + in_turn_indices = curvature > 0.8 * new_curvature + + if np.any(in_turn_indices): + dist_to_turn = np.min(dists[in_turn_indices]) + else: + dist_to_turn = 999 + else: + upcoming_curvature = 0. + dist_to_turn = 999 + + query_lock.release() + + dat = messaging.new_message() + dat.init('liveMapData') + + if last_gps is not None: + dat.liveMapData.lastGps = last_gps + + if cur_way is not None: + dat.liveMapData.wayId = cur_way.id + + # Speed limit + max_speed = cur_way.max_speed() + if max_speed is not None: + #new_latitude = gps.latitude + (MAPS_LOOKAHEAD_DISTANCE * cos(heading/180*3.14159265358979) / (6371010 + gps.altitude)) * (180 / 3.14159265358979) + #new_longitude = gps.longitude + (MAPS_LOOKAHEAD_DISTANCE * sin(heading/180*3.14159265358979) / (6371010 + gps.altitude)) * (180 / 3.14159265358979) / cos(gps.latitude * 3.14159265358979/180) + ahead_speed = None + max_speed_ahead = None + max_speed_ahead_dist = None + #ahead_speed = Way.closest(last_query_result, new_latitude, new_longitude, heading, ahead_speed) + #if ahead_speed is not None and ahead_speed < max_speed: + # max_speed_ahead = ahead_speed.max_speed() + # print "speed ahead found" + # print max_speed_ahead + # max_speed_ahead_dist = cur_way.distance_to_closest_node(lat, lon, heading, pnts) + # print "distance" + # print max_speed_ahead_dist + + if abs(max_speed - max_speed_prev) > 0.1: + max_speed_prev = max_speed + + + + # TODO: use the function below to anticipate upcoming speed limits + max_speed_ahead, max_speed_ahead_dist = cur_way.max_speed_ahead(max_speed, lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE) + if max_speed_ahead is not None and max_speed_ahead_dist is not None: + dat.liveMapData.speedLimitAheadValid = True + dat.liveMapData.speedLimitAhead = float(max_speed_ahead) + dat.liveMapData.speedLimitAheadDistance = float(max_speed_ahead_dist) + + + advisory_max_speed = cur_way.advisory_max_speed() + if advisory_max_speed is not None: + dat.liveMapData.speedAdvisoryValid = True + dat.liveMapData.speedAdvisory = advisory_max_speed + + # Curvature + dat.liveMapData.curvatureValid = curvature_valid + dat.liveMapData.curvature = float(upcoming_curvature) + dat.liveMapData.distToTurn = float(dist_to_turn) + if road_points is not None: + dat.liveMapData.roadX, dat.liveMapData.roadY = road_points + if curvature is not None: + dat.liveMapData.roadCurvatureX = [float(x) for x in dists] + dat.liveMapData.roadCurvature = [float(x) for x in curvature] + + + if max_speed is not None: + dat.liveMapData.speedLimitValid = True + dat.liveMapData.speedLimit = max_speed + + #print "max_speed_prev" + #print max_speed_prev + #print "max_speed" + #print max_speed + dat.liveMapData.mapValid = map_valid + + map_data_sock.send(dat.to_bytes()) + + +def main(gctx=None): + params = Params() + dongle_id = params.get("DongleId") + crash.bind_user(id=dongle_id) + crash.bind_extra(version=version, dirty=dirty, is_eon=True) + crash.install() + + main_thread = threading.Thread(target=mapsd_thread) + main_thread.daemon = True + main_thread.start() + + q_thread = threading.Thread(target=query_thread) + q_thread.daemon = True + q_thread.start() + + while True: + time.sleep(0.1) + + +if __name__ == "__main__": + main() diff --git a/selfdrive/mapd/mapd_helpers.py b/selfdrive/mapd/mapd_helpers.py new file mode 100644 index 00000000000000..de24ea58882c03 --- /dev/null +++ b/selfdrive/mapd/mapd_helpers.py @@ -0,0 +1,435 @@ +import math +import json +import numpy as np +from datetime import datetime +from common.basedir import BASEDIR +from selfdrive.config import Conversions as CV +from common.transformations.coordinates import LocalCoord, geodetic2ecef + +LOOKAHEAD_TIME = 10. +MAPS_LOOKAHEAD_DISTANCE = 50 * LOOKAHEAD_TIME + +DEFAULT_SPEEDS_JSON_FILE = BASEDIR + "/selfdrive/mapd/default_speeds.json" +DEFAULT_SPEEDS = {} +with open(DEFAULT_SPEEDS_JSON_FILE, "rb") as f: + DEFAULT_SPEEDS = json.loads(f.read()) + +DEFAULT_SPEEDS_BY_REGION_JSON_FILE = BASEDIR + "/selfdrive/mapd/default_speeds_by_region.json" +DEFAULT_SPEEDS_BY_REGION = {} +with open(DEFAULT_SPEEDS_BY_REGION_JSON_FILE, "rb") as f: + DEFAULT_SPEEDS_BY_REGION = json.loads(f.read()) + +def circle_through_points(p1, p2, p3): + """Fits a circle through three points + Formulas from: http://www.ambrsoft.com/trigocalc/circle3d.htm""" + x1, y1, _ = p1 + x2, y2, _ = p2 + x3, y3, _ = p3 + + A = x1 * (y2 - y3) - y1 * (x2 - x3) + x2 * y3 - x3 * y2 + B = (x1**2 + y1**2) * (y3 - y2) + (x2**2 + y2**2) * (y1 - y3) + (x3**2 + y3**2) * (y2 - y1) + C = (x1**2 + y1**2) * (x2 - x3) + (x2**2 + y2**2) * (x3 - x1) + (x3**2 + y3**2) * (x1 - x2) + D = (x1**2 + y1**2) * (x3 * y2 - x2 * y3) + (x2**2 + y2**2) * (x1 * y3 - x3 * y1) + (x3**2 + y3**2) * (x2 * y1 - x1 * y2) + + return (-B / (2 * A), - C / (2 * A), np.sqrt((B**2 + C**2 - 4 * A * D) / (4 * A**2))) + +def parse_speed_unit(max_speed): + """Converts a maxspeed string to m/s based on the unit present in the input. + OpenStreetMap defaults to kph if no unit is present. """ + + if not max_speed: + return None + + conversion = CV.KPH_TO_MS + if 'mph' in max_speed: + max_speed = max_speed.replace(' mph', '') + conversion = CV.MPH_TO_MS + try: + return float(max_speed) * conversion + except ValueError: + return None + +def parse_speed_tags(tags): + """Parses tags on a way to find the maxspeed string""" + max_speed = None + + if 'maxspeed' in tags: + max_speed = tags['maxspeed'] + + if 'maxspeed:conditional' in tags: + try: + max_speed_cond, cond = tags['maxspeed:conditional'].split(' @ ') + cond = cond[1:-1] + + start, end = cond.split('-') + now = datetime.now() # TODO: Get time and timezone from gps fix so this will work correctly on replays + start = datetime.strptime(start, "%H:%M").replace(year=now.year, month=now.month, day=now.day) + end = datetime.strptime(end, "%H:%M").replace(year=now.year, month=now.month, day=now.day) + + if start <= now <= end: + max_speed = max_speed_cond + except ValueError: + pass + + if not max_speed and 'source:maxspeed' in tags: + max_speed = DEFAULT_SPEEDS.get(tags['source:maxspeed'], None) + if not max_speed and 'maxspeed:type' in tags: + max_speed = DEFAULT_SPEEDS.get(tags['maxspeed:type'], None) + + max_speed = parse_speed_unit(max_speed) + return max_speed + +def geocode_maxspeed(tags, location_info): + max_speed = None + try: + geocode_country = location_info.get('country', '') + geocode_region = location_info.get('region', '') + + country_rules = DEFAULT_SPEEDS_BY_REGION.get(geocode_country, {}) + country_defaults = country_rules.get('Default', []) + for rule in country_defaults: + rule_valid = all( + tag_name in tags + and tags[tag_name] == value + for tag_name, value in rule['tags'].items() + ) + if rule_valid: + max_speed = rule['speed'] + break #stop searching country + + region_rules = country_rules.get(geocode_region, []) + for rule in region_rules: + rule_valid = all( + tag_name in tags + and tags[tag_name] == value + for tag_name, value in rule['tags'].items() + ) + if rule_valid: + max_speed = rule['speed'] + break #stop searching region + except KeyError: + pass + max_speed = parse_speed_unit(max_speed) + return max_speed + +class Way: + def __init__(self, way, query_results): + self.id = way.id + self.way = way + self.query_results = query_results + + points = list() + + for node in self.way.get_nodes(resolve_missing=False): + points.append((float(node.lat), float(node.lon), 0.)) + + self.points = np.asarray(points) + + @classmethod + def closest(cls, query_results, lat, lon, heading, prev_way=None): + results, tree, real_nodes, node_to_way, location_info = query_results + + cur_pos = geodetic2ecef((lat, lon, 0)) + nodes = tree.query_ball_point(cur_pos, 500) + + # If no nodes within 500m, choose closest one + if not nodes: + nodes = [tree.query(cur_pos)[1]] + + ways = [] + for n in nodes: + real_node = real_nodes[n] + ways += node_to_way[real_node.id] + ways = set(ways) + + closest_way = None + best_score = None + for way in ways: + way = Way(way, query_results) + points = way.points_in_car_frame(lat, lon, heading) + + on_way = way.on_way(lat, lon, heading, points) + if not on_way: + continue + + # Create mask of points in front and behind + x = points[:, 0] + y = points[:, 1] + angles = np.arctan2(y, x) + front = np.logical_and((-np.pi / 2) < angles, + angles < (np.pi / 2)) + behind = np.logical_not(front) + + dists = np.linalg.norm(points, axis=1) + + # Get closest point behind the car + dists_behind = np.copy(dists) + dists_behind[front] = np.NaN + closest_behind = points[np.nanargmin(dists_behind)] + + # Get closest point in front of the car + dists_front = np.copy(dists) + dists_front[behind] = np.NaN + closest_front = points[np.nanargmin(dists_front)] + + # fit line: y = a*x + b + x1, y1, _ = closest_behind + x2, y2, _ = closest_front + a = (y2 - y1) / max((x2 - x1), 1e-5) + b = y1 - a * x1 + + # With a factor of 60 a 20m offset causes the same error as a 20 degree heading error + # (A 20 degree heading offset results in an a of about 1/3) + score = abs(a) * 60. + abs(b) + + # Prefer same type of road + if prev_way is not None: + if way.way.tags.get('highway', '') == prev_way.way.tags.get('highway', ''): + score *= 0.5 + + if closest_way is None or score < best_score: + closest_way = way + best_score = score + + # Normal score is < 5 + if best_score > 50: + return None + + return closest_way + + def __str__(self): + return "%s %s" % (self.id, self.way.tags) + + def max_speed(self): + """Extracts the (conditional) speed limit from a way""" + if not self.way: + return None + + max_speed = parse_speed_tags(self.way.tags) + if not max_speed: + location_info = self.query_results[4] + max_speed = geocode_maxspeed(self.way.tags, location_info) + + return max_speed + + def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead): + """Look ahead for a max speed""" + if not self.way: + return None + + speed_ahead = None + speed_ahead_dist = None + lookahead_ways = 3 + way = self + for i in range(lookahead_ways): + way_pts = way.points_in_car_frame(lat, lon, heading) + #print way_pts + # Check current lookahead distance + if way_pts[0,0] < 0: + max_dist = np.linalg.norm(way_pts[-1, :]) + elif way_pts[-1,0] < 0: + max_dist = np.linalg.norm(way_pts[0, :]) + else: + max_dist = min(np.linalg.norm(way_pts[1, :]),np.linalg.norm(way_pts[0, :]),np.linalg.norm(way_pts[-1, :])) + + + if max_dist > 2 * lookahead: + #print "max_dist break" + break + try: + if way.way.tags['junction']=='roundabout': + latmin = 181 + lonmin = 181 + latmax = -181 + lonmax = -181 + for n in way.way.nodes: + lonmax = max(n.lon,lonmax) + lonmin = min(n.lon,lonmin) + latmax = max(n.lat,latmax) + latmin = min(n.lat,latmin) + a = 111132.954*math.cos(float(latmax+latmin)/360*3.141592)*float(lonmax-lonmin) + speed_ahead = np.sqrt(1.6075*a) + min_dist = 999.9 + for w in way_pts: + min_dist = min(min_dist, float(np.linalg.norm(w))) + speed_ahead_dist = min_dist + break + except KeyError: + pass + if 'maxspeed' in way.way.tags: + spd = parse_speed_tags(way.way.tags) + #print "spd found" + #print spd + if not spd: + location_info = self.query_results[4] + spd = geocode_maxspeed(way.way.tags, location_info) + #print "spd is actually" + #print spd + if spd < current_speed_limit: + speed_ahead = spd + min_dist = min(np.linalg.norm(way_pts[1, :]),np.linalg.norm(way_pts[0, :]),np.linalg.norm(way_pts[-1, :])) + speed_ahead_dist = min_dist + #print "slower speed found" + #print min_dist + + break + # Find next way + way = way.next_way(heading) + if not way: + #print "no way break" + break + + return speed_ahead, speed_ahead_dist + + def advisory_max_speed(self): + if not self.way: + return None + + tags = self.way.tags + adv_speed = None + + if 'maxspeed:advisory' in tags: + adv_speed = tags['maxspeed:advisory'] + adv_speed = parse_speed_unit(adv_speed) + return adv_speed + + def on_way(self, lat, lon, heading, points=None): + if points is None: + points = self.points_in_car_frame(lat, lon, heading) + x = points[:, 0] + return np.min(x) < 0. and np.max(x) > 0. + + def closest_point(self, lat, lon, heading, points=None): + if points is None: + points = self.points_in_car_frame(lat, lon, heading) + i = np.argmin(np.linalg.norm(points, axis=1)) + return points[i] + + def distance_to_closest_node(self, lat, lon, heading, points=None): + if points is None: + points = self.points_in_car_frame(lat, lon, heading) + return np.min(np.linalg.norm(points, axis=1)) + + def points_in_car_frame(self, lat, lon, heading): + lc = LocalCoord.from_geodetic([lat, lon, 0.]) + + # Build rotation matrix + heading = math.radians(-heading + 90) + c, s = np.cos(heading), np.sin(heading) + rot = np.array([[c, s, 0.], [-s, c, 0.], [0., 0., 1.]]) + + # Convert to local coordinates + points_carframe = lc.geodetic2ned(self.points).T + + # Rotate with heading of car + points_carframe = np.dot(rot, points_carframe[(1, 0, 2), :]).T + + return points_carframe + + def next_way(self, heading): + results, tree, real_nodes, node_to_way, location_info = self.query_results + #print "way.id" + #print self.id + #print "node0.id" + #print self.way.nodes[0].id + #print "node-1.id" + #print self.way.nodes[-1].id + #print "heading" + #print heading + angle=heading - math.atan2(self.way.nodes[0].lon-self.way.nodes[-1].lon,self.way.nodes[0].lat-self.way.nodes[-1].lat)*180/3.14159265358979 - 180 + #print "angle before" + #print angle + if angle < -180: + angle = angle + 360 + if angle > 180: + angle = angle - 360 + #print "angle" + #print angle + backwards = abs(angle) > 90 + #print "backwards" + #print backwards + if backwards: + node = self.way.nodes[0] + else: + node = self.way.nodes[-1] + + ways = node_to_way[node.id] + + way = None + try: + # Simple heuristic to find next way + ways = [w for w in ways if w.id != self.id] + if len(ways) == 1: + way = Way(ways[0], self.query_results) + #print "only one way found" + return way + ways = [w for w in ways if (w.nodes[0] == node or w.nodes[-1] == node)] + if len(ways) == 1: + way = Way(ways[0], self.query_results) + #print "only one way found" + return way + if len(ways) == 2: + try: + if ways[0].tags['junction']=='roundabout': + #print ("roundabout found") + way = Way(ways[0], self.query_results) + return way + except KeyError: + pass + # Filter on highway tag + acceptable_tags = list() + cur_tag = self.way.tags['highway'] + acceptable_tags.append(cur_tag) + if cur_tag == 'motorway_link': + acceptable_tags.append('motorway') + acceptable_tags.append('trunk') + acceptable_tags.append('primary') + ways = [w for w in ways if w.tags['highway'] in acceptable_tags] + if len(ways) == 1: + way = Way(ways[0], self.query_results) + #print "only one way found" + return way + # Filter on number of lanes + cur_num_lanes = int(self.way.tags['lanes']) + if len(ways) > 1: + ways_same_lanes = [w for w in ways if int(w.tags['lanes']) == cur_num_lanes] + if len(ways_same_lanes) == 1: + ways = ways_same_lanes + if len(ways) > 1: + ways = [w for w in ways if int(w.tags['lanes']) > cur_num_lanes] + if len(ways) == 1: + way = Way(ways[0], self.query_results) + + except (KeyError, ValueError): + pass + + return way + + def get_lookahead(self, lat, lon, heading, lookahead): + pnts = None + way = self + valid = False + + for i in range(5): + # Get new points and append to list + new_pnts = way.points_in_car_frame(lat, lon, heading) + + if pnts is None: + pnts = new_pnts + else: + pnts = np.vstack([pnts, new_pnts]) + + # Check current lookahead distance + max_dist = np.linalg.norm(pnts[-1, :]) + if max_dist > lookahead: + valid = True + + if max_dist > 2 * lookahead: + break + + # Find next way + way = way.next_way(heading) + if not way: + break + + return pnts, valid