From 81c3c8c84f54fb58f6668221fbfad239ff396e99 Mon Sep 17 00:00:00 2001 From: Kumar <36933347+rav4kumar@users.noreply.github.com> Date: Tue, 26 Nov 2019 23:56:43 -0700 Subject: [PATCH 01/31] Reduce the ui lag --- selfdrive/ui/ui.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c index 183a2f3d8a1968..0ba09e79f60b48 100644 --- a/selfdrive/ui/ui.c +++ b/selfdrive/ui/ui.c @@ -72,7 +72,7 @@ const int header_h = 420; const int footer_h = 280; const int footer_y = vwp_h-bdr_s-footer_h; -const int UI_FREQ = 30; // Hz +const int UI_FREQ = 24; // Hz const int MODEL_PATH_MAX_VERTICES_CNT = 98; const int MODEL_LANE_PATH_CNT = 3; From 2f720fc05ea39bc3c37502d52a128079ad5273d5 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Wed, 27 Nov 2019 09:22:26 +0100 Subject: [PATCH 02/31] No TR mods for speeds below 15 mph --- selfdrive/controls/lib/long_mpc.py | 1 + 1 file changed, 1 insertion(+) diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index 871b8778b5b076..69864a24e7eca3 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -117,6 +117,7 @@ def smooth_follow(self): # in m/s x = [4.4704, 6.7056] # smoothly ramp TR between 10 and 15 mph from 1.8s to defined TR above at 15mph y = [1.8, interp(x[1], x_vel, y_mod)] TR = interp(self.v_ego, x, y) + return round(TR, 3) if self.lead_data['v_lead'] is not None: # since the new mpc now handles braking nicely, simplify mods x = [-2.68, -2.1, -1.26, -0.61, 0, 0.61, 1.26, 2.1, 2.68] # relative velocity values From 6db5256bcfd585ee0c7104e610f4bdae07cf0802 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Wed, 27 Nov 2019 09:27:42 +0100 Subject: [PATCH 03/31] always brake if lead car closer than 4m --- selfdrive/controls/lib/planner.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 865a3a81e49a3d..38ee4ca92313e2 100644 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -117,10 +117,10 @@ def __init__(self, CP): self.params = Params() - def choose_solution(self, v_cruise_setpoint, enabled, yRel, dRel, steeringAngle): + def choose_solution(self, v_cruise_setpoint, enabled, lead_1, lead_2, steeringAngle): if enabled: solutions = {'model': self.v_model, 'cruise': self.v_cruise} - if self.mpc1.prev_lead_status and (abs(math.atan2(yRel,dRel)*180/math.pi - steeringAngle/10) < yRel + max(math.atan2(1,dRel)*180/math.pi, 5.0)): + if self.mpc1.prev_lead_status and (abs(math.atan2(lead_1.yRel, lead_1.dRel)*180/math.pi - steeringAngle/10) < lead_1.yRel + max(math.atan2(1, lead_1.dRel)*180/math.pi, 5.0)): solutions['mpc1'] = self.mpc1.v_mpc if self.mpc2.prev_lead_status: solutions['mpc2'] = self.mpc2.v_mpc @@ -129,7 +129,10 @@ def choose_solution(self, v_cruise_setpoint, enabled, yRel, dRel, steeringAngle) self.longitudinalPlanSource = slowest # Choose lowest of MPC and cruise - if slowest == 'mpc1': + if (lead_1.status and lead_1.dRel < 4.0) or (lead_2.status and lead_2.dRel) < 4.0: + self.v_acc = 0.0 + self.a_acc = -4.0 + elif slowest == 'mpc1': self.v_acc = self.mpc1.v_mpc self.a_acc = self.mpc1.a_mpc elif slowest == 'mpc2': @@ -302,7 +305,7 @@ def update(self, sm, pm, CP, VM, PP): self.mpc1.update(pm, sm['carState'], lead_1, v_cruise_setpoint) self.mpc2.update(pm, sm['carState'], lead_2, v_cruise_setpoint) - self.choose_solution(v_cruise_setpoint, enabled, lead_1.yRel, lead_1.dRel, sm['carState'].steeringAngle) + self.choose_solution(v_cruise_setpoint, enabled, lead_1, lead_2, sm['carState'].steeringAngle) # determine fcw if self.mpc1.new_lead: @@ -340,10 +343,7 @@ def update(self, sm, pm, CP, VM, PP): plan_send.plan.aStart = float(self.a_acc_start) plan_send.plan.vTarget = float(self.v_acc) plan_send.plan.aTarget = float(self.a_acc) - if (lead_1.status and lead_1.dRel < 4.0) or (lead_2.status and lead_2.dRel) < 4.0: - plan_send.plan.vTargetFuture = float(0.0) - else: - plan_send.plan.vTargetFuture = float(self.v_acc_future) + plan_send.plan.vTargetFuture = float(self.v_acc_future) plan_send.plan.hasLead = self.mpc1.prev_lead_status plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource From 49159dda6c380a827e2032b2308a10f797070ed3 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Wed, 27 Nov 2019 12:08:36 +0100 Subject: [PATCH 04/31] Lead1 status does not work --- selfdrive/controls/lib/planner.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 38ee4ca92313e2..ffd180ba18e887 100644 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -129,10 +129,10 @@ def choose_solution(self, v_cruise_setpoint, enabled, lead_1, lead_2, steeringAn self.longitudinalPlanSource = slowest # Choose lowest of MPC and cruise - if (lead_1.status and lead_1.dRel < 4.0) or (lead_2.status and lead_2.dRel) < 4.0: - self.v_acc = 0.0 - self.a_acc = -4.0 - elif slowest == 'mpc1': + #if (lead_1.status and lead_1.dRel < 4.0) or (lead_2.status and lead_2.dRel) < 4.0: + # self.v_acc = 0.0 + # self.a_acc = -4.0 + if slowest == 'mpc1': self.v_acc = self.mpc1.v_mpc self.a_acc = self.mpc1.a_mpc elif slowest == 'mpc2': @@ -180,7 +180,7 @@ def update(self, sm, pm, CP, VM, PP): lead_2 = sm['radarState'].leadTwo enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping) - following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0 + following = self.mpc1.prev_lead_status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0 v_speedlimit = NO_CURVATURE_SPEED v_curvature_map = NO_CURVATURE_SPEED From 3fbaf3aa1821569844865467b1926bf4b704a264 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Wed, 27 Nov 2019 12:13:38 +0100 Subject: [PATCH 05/31] Special case when at standstill and brake pressed --- selfdrive/car/toyota/interface.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 62bc47cb37a7bf..624a2fadeabcf3 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -423,10 +423,11 @@ def update(self, c, can_strings): events.append(create_event('pcmDisable', [ET.USER_DISABLE])) # disable on pedals rising edge or when brake is pressed and speed isn't zero - if ((ret.gasPressed and not self.gas_pressed_prev) or \ - (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001))) and disengage_event: + if (((ret.gasPressed and not self.gas_pressed_prev) or \ + (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001))) and disengage_event) or (ret.brakePressed and not self.brake_pressed_prev and ret.vEgo < 0.1): events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) - + + if ret.gasPressed and disengage_event: events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) From defcebafe7b44b2ed4a42fbc024c8cb92f05269b Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Wed, 27 Nov 2019 13:57:27 +0100 Subject: [PATCH 06/31] remove yRel from check --- selfdrive/controls/lib/planner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index ffd180ba18e887..2d80610fd69594 100644 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -120,7 +120,7 @@ def __init__(self, CP): def choose_solution(self, v_cruise_setpoint, enabled, lead_1, lead_2, steeringAngle): if enabled: solutions = {'model': self.v_model, 'cruise': self.v_cruise} - if self.mpc1.prev_lead_status and (abs(math.atan2(lead_1.yRel, lead_1.dRel)*180/math.pi - steeringAngle/10) < lead_1.yRel + max(math.atan2(1, lead_1.dRel)*180/math.pi, 5.0)): + if self.mpc1.prev_lead_status and (abs(math.atan2(lead_1.yRel, lead_1.dRel)*180./math.pi - steeringAngle/10.) < max(math.atan2(1, lead_1.dRel)*180./math.pi, 5.0)): solutions['mpc1'] = self.mpc1.v_mpc if self.mpc2.prev_lead_status: solutions['mpc2'] = self.mpc2.v_mpc From 25149209bdfa5efc2e7709344b695f1caa35d6b7 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Wed, 27 Nov 2019 14:18:36 +0100 Subject: [PATCH 07/31] Apply same check for mpc2 --- selfdrive/controls/lib/planner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 2d80610fd69594..ece1c89babf51a 100644 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -122,7 +122,7 @@ def choose_solution(self, v_cruise_setpoint, enabled, lead_1, lead_2, steeringAn solutions = {'model': self.v_model, 'cruise': self.v_cruise} if self.mpc1.prev_lead_status and (abs(math.atan2(lead_1.yRel, lead_1.dRel)*180./math.pi - steeringAngle/10.) < max(math.atan2(1, lead_1.dRel)*180./math.pi, 5.0)): solutions['mpc1'] = self.mpc1.v_mpc - if self.mpc2.prev_lead_status: + if self.mpc2.prev_lead_status and (abs(math.atan2(lead_2.yRel, lead_2.dRel)*180./math.pi - steeringAngle/10.) < max(math.atan2(1, lead_2.dRel)*180./math.pi, 5.0)): solutions['mpc2'] = self.mpc2.v_mpc slowest = min(solutions, key=solutions.get) From 96b66a8382759d1cc3dbdaea7fdcbe034b178756 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Wed, 27 Nov 2019 16:20:17 +0100 Subject: [PATCH 08/31] Use circular path instead of projected angle Consider that the edge of the car is 1m left and right of the camera. Work out where the turn radius is based on steering angle and check if the lead will colide or not. Based off of https://www.quora.com/What-is-the-method-to-calculate-turning-radius-if-only-the-steering-ratio-and-vehicles-basic-dimensions-are-known https://math.stackexchange.com/questions/198764/how-to-know-if-a-point-is-inside-a-circle --- selfdrive/controls/lib/planner.py | 26 +++++++++++++++++++++----- 1 file changed, 21 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index ece1c89babf51a..5150df6f620321 100644 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -119,12 +119,28 @@ def __init__(self, CP): def choose_solution(self, v_cruise_setpoint, enabled, lead_1, lead_2, steeringAngle): if enabled: + solutions = {'model': self.v_model, 'cruise': self.v_cruise} - if self.mpc1.prev_lead_status and (abs(math.atan2(lead_1.yRel, lead_1.dRel)*180./math.pi - steeringAngle/10.) < max(math.atan2(1, lead_1.dRel)*180./math.pi, 5.0)): - solutions['mpc1'] = self.mpc1.v_mpc - if self.mpc2.prev_lead_status and (abs(math.atan2(lead_2.yRel, lead_2.dRel)*180./math.pi - steeringAngle/10.) < max(math.atan2(1, lead_2.dRel)*180./math.pi, 5.0)): - solutions['mpc2'] = self.mpc2.v_mpc - + + center_x = -2.5 # Wheel base 2.5m + if steeringAngle > 100: # only at high angles + center_y = -1+2.5/math.tan(steeringAngle/1800.*math.pi) # Car Width 2m. Left side considered in left hand turn + if self.mpc1.prev_lead_status and (math.sqrt((lead_1.dRel-center_x)**2+(lead_1.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi))): + solutions['mpc1'] = self.mpc1.v_mpc + if self.mpc2.prev_lead_status and (math.sqrt((lead_2.dRel-center_x)**2+(lead_2.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi))): + solutions['mpc2'] = self.mpc2.v_mpc + elif steeringAngle < 100: # only at high angles + center_y = +1-2.5/math.tan(steeringAngle/1800.*math.pi) # Car Width 2m. Right side considered in right hand turn + if self.mpc1.prev_lead_status and (math.sqrt((lead_1.dRel-center_x)**2+(lead_1.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi))): + solutions['mpc1'] = self.mpc1.v_mpc + if self.mpc2.prev_lead_status and (math.sqrt((lead_2.dRel-center_x)**2+(lead_2.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi))): + solutions['mpc2'] = self.mpc2.v_mpc + else: + if self.mpc1.prev_lead_status: + solutions['mpc1'] = self.mpc1.v_mpc + if self.mpc2.prev_lead_status: + solutions['mpc2'] = self.mpc2.v_mpc + slowest = min(solutions, key=solutions.get) self.longitudinalPlanSource = slowest From cf59ef59f3569ea1e50ad8e2b6e4d54cc64edd80 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Wed, 27 Nov 2019 16:38:55 +0100 Subject: [PATCH 09/31] Add missing negative --- selfdrive/controls/lib/planner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 5150df6f620321..5f826dd4df2384 100644 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -129,7 +129,7 @@ def choose_solution(self, v_cruise_setpoint, enabled, lead_1, lead_2, steeringAn solutions['mpc1'] = self.mpc1.v_mpc if self.mpc2.prev_lead_status and (math.sqrt((lead_2.dRel-center_x)**2+(lead_2.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi))): solutions['mpc2'] = self.mpc2.v_mpc - elif steeringAngle < 100: # only at high angles + elif steeringAngle < -100: # only at high angles center_y = +1-2.5/math.tan(steeringAngle/1800.*math.pi) # Car Width 2m. Right side considered in right hand turn if self.mpc1.prev_lead_status and (math.sqrt((lead_1.dRel-center_x)**2+(lead_1.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi))): solutions['mpc1'] = self.mpc1.v_mpc From 603d40240a1c6e946fb58e33608b348493bad4b8 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Thu, 28 Nov 2019 15:19:45 +0100 Subject: [PATCH 10/31] lead check also for self.v_acc_future --- selfdrive/controls/lib/planner.py | 47 ++++++++++++++----------------- 1 file changed, 21 insertions(+), 26 deletions(-) diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 5f826dd4df2384..d1a61d71840372 100644 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -118,36 +118,27 @@ def __init__(self, CP): self.params = Params() def choose_solution(self, v_cruise_setpoint, enabled, lead_1, lead_2, steeringAngle): + center_x = -2.5 # Wheel base 2.5m + lead1_check = True + lead2_check = True + if steeringAngle > 100: # only at high angles + center_y = -1+2.5/math.tan(steeringAngle/1800.*math.pi) # Car Width 2m. Left side considered in left hand turn + lead1_check = math.sqrt((lead_1.dRel-center_x)**2+(lead_1.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi)) + lead2_check = math.sqrt((lead_2.dRel-center_x)**2+(lead_2.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi)) + elif steeringAngle < -100: # only at high angles + center_y = +1-2.5/math.tan(steeringAngle/1800.*math.pi) # Car Width 2m. Right side considered in right hand turn + lead1_check = math.sqrt((lead_1.dRel-center_x)**2+(lead_1.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi)) + lead2_check = math.sqrt((lead_2.dRel-center_x)**2+(lead_2.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi)) if enabled: - solutions = {'model': self.v_model, 'cruise': self.v_cruise} - - center_x = -2.5 # Wheel base 2.5m - if steeringAngle > 100: # only at high angles - center_y = -1+2.5/math.tan(steeringAngle/1800.*math.pi) # Car Width 2m. Left side considered in left hand turn - if self.mpc1.prev_lead_status and (math.sqrt((lead_1.dRel-center_x)**2+(lead_1.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi))): - solutions['mpc1'] = self.mpc1.v_mpc - if self.mpc2.prev_lead_status and (math.sqrt((lead_2.dRel-center_x)**2+(lead_2.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi))): - solutions['mpc2'] = self.mpc2.v_mpc - elif steeringAngle < -100: # only at high angles - center_y = +1-2.5/math.tan(steeringAngle/1800.*math.pi) # Car Width 2m. Right side considered in right hand turn - if self.mpc1.prev_lead_status and (math.sqrt((lead_1.dRel-center_x)**2+(lead_1.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi))): - solutions['mpc1'] = self.mpc1.v_mpc - if self.mpc2.prev_lead_status and (math.sqrt((lead_2.dRel-center_x)**2+(lead_2.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi))): - solutions['mpc2'] = self.mpc2.v_mpc - else: - if self.mpc1.prev_lead_status: - solutions['mpc1'] = self.mpc1.v_mpc - if self.mpc2.prev_lead_status: - solutions['mpc2'] = self.mpc2.v_mpc - + if self.mpc1.prev_lead_status and lead1_check: + solutions['mpc1'] = self.mpc1.v_mpc + if self.mpc2.prev_lead_status and lead2_check: + solutions['mpc2'] = self.mpc2.v_mpc + slowest = min(solutions, key=solutions.get) self.longitudinalPlanSource = slowest - # Choose lowest of MPC and cruise - #if (lead_1.status and lead_1.dRel < 4.0) or (lead_2.status and lead_2.dRel) < 4.0: - # self.v_acc = 0.0 - # self.a_acc = -4.0 if slowest == 'mpc1': self.v_acc = self.mpc1.v_mpc self.a_acc = self.mpc1.a_mpc @@ -161,7 +152,11 @@ def choose_solution(self, v_cruise_setpoint, enabled, lead_1, lead_2, steeringAn self.v_acc = self.v_model self.a_acc = self.a_model - self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint]) + self.v_acc_future = v_cruise_setpoint + if lead1_check: + self.v_acc_future = min([self.mpc1.v_mpc_future, self.v_acc_future]) + if lead2_check: + self.v_acc_future = min([self.mpc2.v_mpc_future, self.v_acc_future]) def update(self, sm, pm, CP, VM, PP): self.arne_sm.update(0) From a0e1ad1dbcee6fde5363cdb0c8d8ecb9f598833c Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Thu, 28 Nov 2019 16:29:22 +0100 Subject: [PATCH 11/31] Leave 1m safety distance to car while turning --- selfdrive/controls/lib/planner.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index d1a61d71840372..eaed5f8cc7cdee 100644 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -122,11 +122,11 @@ def choose_solution(self, v_cruise_setpoint, enabled, lead_1, lead_2, steeringAn lead1_check = True lead2_check = True if steeringAngle > 100: # only at high angles - center_y = -1+2.5/math.tan(steeringAngle/1800.*math.pi) # Car Width 2m. Left side considered in left hand turn + center_y = -2+2.5/math.tan(steeringAngle/1800.*math.pi) # Car Width 2m. Left side considered in left hand turn lead1_check = math.sqrt((lead_1.dRel-center_x)**2+(lead_1.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi)) lead2_check = math.sqrt((lead_2.dRel-center_x)**2+(lead_2.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi)) elif steeringAngle < -100: # only at high angles - center_y = +1-2.5/math.tan(steeringAngle/1800.*math.pi) # Car Width 2m. Right side considered in right hand turn + center_y = +2-2.5/math.tan(steeringAngle/1800.*math.pi) # Car Width 2m. Right side considered in right hand turn lead1_check = math.sqrt((lead_1.dRel-center_x)**2+(lead_1.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi)) lead2_check = math.sqrt((lead_2.dRel-center_x)**2+(lead_2.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi)) if enabled: From b3af02b043d089623ea16a2055b7d71023b17a5a Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Fri, 29 Nov 2019 07:09:01 +0100 Subject: [PATCH 12/31] Circle centre stays the same but radius increases --- selfdrive/controls/lib/planner.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index eaed5f8cc7cdee..9477bbfa04b2dc 100644 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -122,13 +122,13 @@ def choose_solution(self, v_cruise_setpoint, enabled, lead_1, lead_2, steeringAn lead1_check = True lead2_check = True if steeringAngle > 100: # only at high angles - center_y = -2+2.5/math.tan(steeringAngle/1800.*math.pi) # Car Width 2m. Left side considered in left hand turn - lead1_check = math.sqrt((lead_1.dRel-center_x)**2+(lead_1.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi)) - lead2_check = math.sqrt((lead_2.dRel-center_x)**2+(lead_2.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi)) + center_y = -1+2.5/math.tan(steeringAngle/1800.*math.pi) # Car Width 2m. Left side considered in left hand turn + lead1_check = math.sqrt((lead_1.dRel-center_x)**2+(lead_1.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi))+1. + lead2_check = math.sqrt((lead_2.dRel-center_x)**2+(lead_2.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi))+1. elif steeringAngle < -100: # only at high angles - center_y = +2-2.5/math.tan(steeringAngle/1800.*math.pi) # Car Width 2m. Right side considered in right hand turn - lead1_check = math.sqrt((lead_1.dRel-center_x)**2+(lead_1.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi)) - lead2_check = math.sqrt((lead_2.dRel-center_x)**2+(lead_2.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi)) + center_y = +1-2.5/math.tan(steeringAngle/1800.*math.pi) # Car Width 2m. Right side considered in right hand turn + lead1_check = math.sqrt((lead_1.dRel-center_x)**2+(lead_1.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi))+1. + lead2_check = math.sqrt((lead_2.dRel-center_x)**2+(lead_2.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi))+1. if enabled: solutions = {'model': self.v_model, 'cruise': self.v_cruise} if self.mpc1.prev_lead_status and lead1_check: From 0d111336d8ca34e3b2e6972c9a5d59bba3459c53 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Fri, 29 Nov 2019 15:29:56 +0100 Subject: [PATCH 13/31] revert --- selfdrive/ui/ui.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c index 0ba09e79f60b48..183a2f3d8a1968 100644 --- a/selfdrive/ui/ui.c +++ b/selfdrive/ui/ui.c @@ -72,7 +72,7 @@ const int header_h = 420; const int footer_h = 280; const int footer_y = vwp_h-bdr_s-footer_h; -const int UI_FREQ = 24; // Hz +const int UI_FREQ = 30; // Hz const int MODEL_PATH_MAX_VERTICES_CNT = 98; const int MODEL_LANE_PATH_CNT = 3; From 14678a5ec942f5b0d28be49b77e15c6ae9174dc4 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Fri, 29 Nov 2019 15:31:21 +0100 Subject: [PATCH 14/31] Check if this fixes the UI lag. --- selfdrive/boardd/boardd.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index c4763012cbf7ea..1cd48e6360df7c 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -353,7 +353,7 @@ void can_health(PubSocket *publisher) { } #ifndef __x86_64__ - if ((no_ignition_cnt > NO_IGNITION_CNT_MAX) && (health.usb_power_mode == (uint8_t)(cereal::HealthData::UsbPowerMode::CDP)) || ((health.voltage < 10500) && !ignition)){ + if ((no_ignition_cnt > NO_IGNITION_CNT_MAX) && (health.usb_power_mode == (uint8_t)(cereal::HealthData::UsbPowerMode::CDP))){ printf("TURN OFF CHARGING!\n"); pthread_mutex_lock(&usb_lock); libusb_control_transfer(dev_handle, 0xc0, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CLIENT), 0, NULL, 0, TIMEOUT); From c130aaa946fbdbadadb3c240b1a1a74790de1b62 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Fri, 29 Nov 2019 21:14:22 +0100 Subject: [PATCH 15/31] Update to have RAV4H_TSS2 working --- selfdrive/car/toyota/values.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 0b8445bb81497e..128cedc7289ef0 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -198,5 +198,5 @@ class ECU: ''' # todo: I'm using stock 066 below, above this is from 065-clean. Arne, check this out and delete the above if it looks good NO_DSU_CAR = [CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2] -TSS2_CAR = [CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2] +TSS2_CAR = [CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2] NO_STOP_TIMER_CAR = [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.SIENNA] # no resume button press required From 8a153abaa43c790e1c7cc4ad3cd4b553c90cd7c9 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Fri, 29 Nov 2019 21:53:13 +0100 Subject: [PATCH 16/31] add more , CAR.RAV4H_TSS2 --- selfdrive/car/toyota/values.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 128cedc7289ef0..326f1d3205abce 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -197,6 +197,6 @@ class ECU: NO_STOP_TIMER_CAR = [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.LEXUS_ESH_TSS2, CAR.SIENNA, CAR.RAV4H_TSS2] # no resume button press required ''' # todo: I'm using stock 066 below, above this is from 065-clean. Arne, check this out and delete the above if it looks good -NO_DSU_CAR = [CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2] +NO_DSU_CAR = [CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2] TSS2_CAR = [CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2] -NO_STOP_TIMER_CAR = [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.SIENNA] # no resume button press required +NO_STOP_TIMER_CAR = [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.SIENNA, CAR.RAV4H_TSS2] # no resume button press required From ad59afaa21c23bf176ebae146cf5c7299ff3bfc9 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Sat, 30 Nov 2019 18:18:03 +0100 Subject: [PATCH 17/31] only do the alert check every minute --- selfdrive/thermald.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/selfdrive/thermald.py b/selfdrive/thermald.py index 7c42004d3267e4..4fa856c8161f9a 100755 --- a/selfdrive/thermald.py +++ b/selfdrive/thermald.py @@ -327,13 +327,6 @@ def thermald_thread(): elif usb_power and not usb_power_prev: params.delete("Offroad_ChargeDisabled") - if msg.thermal.batteryVoltage < 11500: - alert_connectivity_prompt = copy.copy(OFFROAD_ALERTS["Offroad_VoltageLow"]) - alert_connectivity_prompt["text"] += str(msg.thermal.batteryVoltage) + " Volts." - params.delete("Offroad_VoltageLow") - params.put("Offroad_VoltageLow", json.dumps(alert_connectivity_prompt)) - else: - params.delete("Offroad_VoltageLow") thermal_status_prev = thermal_status usb_power_prev = usb_power @@ -343,6 +336,13 @@ def thermald_thread(): # report to server once per minute if (count % int(60. / DT_TRML)) == 0: + if msg.thermal.batteryVoltage < 11500: + alert_connectivity_prompt = copy.copy(OFFROAD_ALERTS["Offroad_VoltageLow"]) + alert_connectivity_prompt["text"] += str(msg.thermal.batteryVoltage) + " Volts." + params.delete("Offroad_VoltageLow") + params.put("Offroad_VoltageLow", json.dumps(alert_connectivity_prompt)) + else: + params.delete("Offroad_VoltageLow") cloudlog.event("STATUS_PACKET", count=count, health=(health.to_dict() if health else None), From b699fb132896e52c27b6ecde09d62fb2511336bc Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Sat, 30 Nov 2019 18:25:25 +0100 Subject: [PATCH 18/31] add pull 888 into fork https://github.com/commaai/openpilot/pull/888/files --- selfdrive/boardd/boardd.cc | 21 ++++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 1cd48e6360df7c..21ca224d2c514a 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -57,6 +57,8 @@ bool loopback_can = false; cereal::HealthData::HwType hw_type = cereal::HealthData::HwType::UNKNOWN; bool is_pigeon = false; const uint32_t NO_IGNITION_CNT_MAX = 2 * 60 * 60 * 24 * 1; // turn off charge after 1 days +const uint32_t VBATT_START_CHARGING = 12000; +const uint32_t VBATT_PAUSE_CHARGING = 10500; uint32_t no_ignition_cnt = 0; bool connected_once = false; bool ignition_last = false; @@ -353,11 +355,20 @@ void can_health(PubSocket *publisher) { } #ifndef __x86_64__ - if ((no_ignition_cnt > NO_IGNITION_CNT_MAX) && (health.usb_power_mode == (uint8_t)(cereal::HealthData::UsbPowerMode::CDP))){ - printf("TURN OFF CHARGING!\n"); - pthread_mutex_lock(&usb_lock); - libusb_control_transfer(dev_handle, 0xc0, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CLIENT), 0, NULL, 0, TIMEOUT); - pthread_mutex_unlock(&usb_lock); + bool cdp_mode = health.usb_power_mode == (uint8_t)(cereal::HealthData::UsbPowerMode::CDP); + bool no_ignition_exp = no_ignition_cnt > NO_IGNITION_CNT_MAX; + if ((no_ignition_exp || (health.voltage < VBATT_PAUSE_CHARGING)) && cdp_mode && !ignition) { + printf("TURN OFF CHARGING!\n"); + pthread_mutex_lock(&usb_lock); + libusb_control_transfer(dev_handle, 0xc0, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CLIENT), 0, NULL, 0, TIMEOUT); + pthread_mutex_unlock(&usb_lock); + } + if (!no_ignition_exp && (health.voltage > VBATT_START_CHARGING) && !cdp_mode) { + + printf("TURN ON CHARGING!\n"); + pthread_mutex_lock(&usb_lock); + libusb_control_transfer(dev_handle, 0xc0, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CDP), 0, NULL, 0, TIMEOUT); + pthread_mutex_unlock(&usb_lock); } #endif From 03278f7cab855b73c08a44626a9b8c17ab0a7889 Mon Sep 17 00:00:00 2001 From: littlemountainman Date: Sat, 30 Nov 2019 20:20:09 +0100 Subject: [PATCH 19/31] create traffic init --- selfdrive/traffic.py | 119 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 119 insertions(+) create mode 100644 selfdrive/traffic.py diff --git a/selfdrive/traffic.py b/selfdrive/traffic.py new file mode 100644 index 00000000000000..da6aa12f437208 --- /dev/null +++ b/selfdrive/traffic.py @@ -0,0 +1,119 @@ +import os +import cv2 +import numpy as np +import zmq + +def detect(source): + + font = cv2.FONT_HERSHEY_SIMPLEX + img = cv2.imread(source) + + cimg = img + hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) + + + lower_red1 = np.array([0,100,100]) + upper_red1 = np.array([10,255,255]) + lower_red2 = np.array([160,100,100]) + upper_red2 = np.array([180,255,255]) + lower_green = np.array([40,50,50]) + upper_green = np.array([90,255,255]) + lower_yellow = np.array([15,150,150]) + upper_yellow = np.array([35,255,255]) + mask1 = cv2.inRange(hsv, lower_red1, upper_red1) + mask2 = cv2.inRange(hsv, lower_red2, upper_red2) + maskg = cv2.inRange(hsv, lower_green, upper_green) + masky = cv2.inRange(hsv, lower_yellow, upper_yellow) + maskr = cv2.add(mask1, mask2) + + size = img.shape + + r_circles = cv2.HoughCircles(maskr, cv2.HOUGH_GRADIENT, 1, 80, + param1=50, param2=10, minRadius=0, maxRadius=30) + + g_circles = cv2.HoughCircles(maskg, cv2.HOUGH_GRADIENT, 1, 60, + param1=50, param2=10, minRadius=0, maxRadius=30) + + y_circles = cv2.HoughCircles(masky, cv2.HOUGH_GRADIENT, 1, 30, + param1=50, param2=5, minRadius=0, maxRadius=30) + + # traffic light detect + r = 5 + bound = 4.0 / 10 + if r_circles is not None: + r_circles = np.uint16(np.around(r_circles)) + + for i in r_circles[0, :]: + if i[0] > size[1] or i[1] > size[0]or i[1] > size[0]*bound: + continue + + h, s = 0.0, 0.0 + for m in range(-r, r): + for n in range(-r, r): + + if (i[1]+m) >= size[0] or (i[0]+n) >= size[1]: + continue + h += maskr[i[1]+m, i[0]+n] + s += 1 + if h / s > 50: + cv2.circle(cimg, (i[0], i[1]), i[2]+10, (0, 255, 0), 2) + cv2.circle(maskr, (i[0], i[1]), i[2]+30, (255, 255, 255), 2) + #cv2.putText(cimg,'RED',(i[0], i[1]), font, 1,(255,0,0),2,cv2.LINE_AA) + print("RED") + if g_circles is not None: + g_circles = np.uint16(np.around(g_circles)) + + for i in g_circles[0, :]: + if i[0] > size[1] or i[1] > size[0] or i[1] > size[0]*bound: + continue + + h, s = 0.0, 0.0 + for m in range(-r, r): + for n in range(-r, r): + + if (i[1]+m) >= size[0] or (i[0]+n) >= size[1]: + continue + h += maskg[i[1]+m, i[0]+n] + s += 1 + if h / s > 100: + cv2.circle(cimg, (i[0], i[1]), i[2]+10, (0, 255, 0), 2) + cv2.circle(maskg, (i[0], i[1]), i[2]+30, (255, 255, 255), 2) + #cv2.putText(cimg,'GREEN',(i[0], i[1]), font, 1,(255,0,0),2,cv2.LINE_AA) + print("GREEN") + if y_circles is not None: + y_circles = np.uint16(np.around(y_circles)) + + for i in y_circles[0, :]: + if i[0] > size[1] or i[1] > size[0] or i[1] > size[0]*bound: + continue + + h, s = 0.0, 0.0 + for m in range(-r, r): + for n in range(-r, r): + + if (i[1]+m) >= size[0] or (i[0]+n) >= size[1]: + continue + h += masky[i[1]+m, i[0]+n] + s += 1 + if h / s > 50: + cv2.circle(cimg, (i[0], i[1]), i[2]+10, (0, 255, 0), 2) + cv2.circle(masky, (i[0], i[1]), i[2]+30, (255, 255, 255), 2) + #cv2.putText(cimg,'YELLOW',(i[0], i[1]), font, 1,(255,0,0),2,cv2.LINE_AA) + print("YELLOW") + + + return cimg +if __name__ == '__main__': + + + while True: + context = zmq.Context() + socket = context.socket(zmq.REP) + socket.bind("tcp://*:9000") + message = socket.recv() + print(message) + detectimg =detect(message) + sleep(0.05) + # This function either gives out "RED" "YELLOW" or "GREEN" + + From 63d11df4fd1b11249775e473b2dbab1511efed0c Mon Sep 17 00:00:00 2001 From: littlemountainman Date: Sat, 30 Nov 2019 20:26:29 +0100 Subject: [PATCH 20/31] camera type check --- selfdrive/traffic.py | 1 + 1 file changed, 1 insertion(+) diff --git a/selfdrive/traffic.py b/selfdrive/traffic.py index da6aa12f437208..7a22c540a9225d 100644 --- a/selfdrive/traffic.py +++ b/selfdrive/traffic.py @@ -111,6 +111,7 @@ def detect(source): socket = context.socket(zmq.REP) socket.bind("tcp://*:9000") message = socket.recv() + print("Message is of type {}".format(type(message))) print(message) detectimg =detect(message) sleep(0.05) From 896afc4d6342d4adc0946ec135892e2835229918 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Sat, 30 Nov 2019 20:37:42 +0100 Subject: [PATCH 21/31] Add traffic camera code --- selfdrive/manager.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/selfdrive/manager.py b/selfdrive/manager.py index cce85862025a5f..76757e8074d219 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -97,6 +97,7 @@ def unblock_stdout(): "gpsd": ("selfdrive/sensord", ["./start_gpsd.py"]), "updated": "selfdrive.updated", "mapd": ("selfdrive/mapd", ["./mapd.py"]), + "traffic": ("selfdrive/traffic", ["./traffic.py"]), } daemon_processes = { "manage_athenad": ("selfdrive.athena.manage_athenad", "AthenadPid"), @@ -140,6 +141,7 @@ def get_running(): 'gpsd', 'deleter', 'mapd', + 'traffic' ] def register_managed_process(name, desc, car_started=False): From c6d9ba19bcdc3adf08a4d31f0c82e98b961dafb3 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Sat, 30 Nov 2019 20:40:41 +0100 Subject: [PATCH 22/31] Create traffic.py --- selfdrive/traffic/traffic.py | 120 +++++++++++++++++++++++++++++++++++ 1 file changed, 120 insertions(+) create mode 100644 selfdrive/traffic/traffic.py diff --git a/selfdrive/traffic/traffic.py b/selfdrive/traffic/traffic.py new file mode 100644 index 00000000000000..7a22c540a9225d --- /dev/null +++ b/selfdrive/traffic/traffic.py @@ -0,0 +1,120 @@ +import os +import cv2 +import numpy as np +import zmq + +def detect(source): + + font = cv2.FONT_HERSHEY_SIMPLEX + img = cv2.imread(source) + + cimg = img + hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) + + + lower_red1 = np.array([0,100,100]) + upper_red1 = np.array([10,255,255]) + lower_red2 = np.array([160,100,100]) + upper_red2 = np.array([180,255,255]) + lower_green = np.array([40,50,50]) + upper_green = np.array([90,255,255]) + lower_yellow = np.array([15,150,150]) + upper_yellow = np.array([35,255,255]) + mask1 = cv2.inRange(hsv, lower_red1, upper_red1) + mask2 = cv2.inRange(hsv, lower_red2, upper_red2) + maskg = cv2.inRange(hsv, lower_green, upper_green) + masky = cv2.inRange(hsv, lower_yellow, upper_yellow) + maskr = cv2.add(mask1, mask2) + + size = img.shape + + r_circles = cv2.HoughCircles(maskr, cv2.HOUGH_GRADIENT, 1, 80, + param1=50, param2=10, minRadius=0, maxRadius=30) + + g_circles = cv2.HoughCircles(maskg, cv2.HOUGH_GRADIENT, 1, 60, + param1=50, param2=10, minRadius=0, maxRadius=30) + + y_circles = cv2.HoughCircles(masky, cv2.HOUGH_GRADIENT, 1, 30, + param1=50, param2=5, minRadius=0, maxRadius=30) + + # traffic light detect + r = 5 + bound = 4.0 / 10 + if r_circles is not None: + r_circles = np.uint16(np.around(r_circles)) + + for i in r_circles[0, :]: + if i[0] > size[1] or i[1] > size[0]or i[1] > size[0]*bound: + continue + + h, s = 0.0, 0.0 + for m in range(-r, r): + for n in range(-r, r): + + if (i[1]+m) >= size[0] or (i[0]+n) >= size[1]: + continue + h += maskr[i[1]+m, i[0]+n] + s += 1 + if h / s > 50: + cv2.circle(cimg, (i[0], i[1]), i[2]+10, (0, 255, 0), 2) + cv2.circle(maskr, (i[0], i[1]), i[2]+30, (255, 255, 255), 2) + #cv2.putText(cimg,'RED',(i[0], i[1]), font, 1,(255,0,0),2,cv2.LINE_AA) + print("RED") + if g_circles is not None: + g_circles = np.uint16(np.around(g_circles)) + + for i in g_circles[0, :]: + if i[0] > size[1] or i[1] > size[0] or i[1] > size[0]*bound: + continue + + h, s = 0.0, 0.0 + for m in range(-r, r): + for n in range(-r, r): + + if (i[1]+m) >= size[0] or (i[0]+n) >= size[1]: + continue + h += maskg[i[1]+m, i[0]+n] + s += 1 + if h / s > 100: + cv2.circle(cimg, (i[0], i[1]), i[2]+10, (0, 255, 0), 2) + cv2.circle(maskg, (i[0], i[1]), i[2]+30, (255, 255, 255), 2) + #cv2.putText(cimg,'GREEN',(i[0], i[1]), font, 1,(255,0,0),2,cv2.LINE_AA) + print("GREEN") + if y_circles is not None: + y_circles = np.uint16(np.around(y_circles)) + + for i in y_circles[0, :]: + if i[0] > size[1] or i[1] > size[0] or i[1] > size[0]*bound: + continue + + h, s = 0.0, 0.0 + for m in range(-r, r): + for n in range(-r, r): + + if (i[1]+m) >= size[0] or (i[0]+n) >= size[1]: + continue + h += masky[i[1]+m, i[0]+n] + s += 1 + if h / s > 50: + cv2.circle(cimg, (i[0], i[1]), i[2]+10, (0, 255, 0), 2) + cv2.circle(masky, (i[0], i[1]), i[2]+30, (255, 255, 255), 2) + #cv2.putText(cimg,'YELLOW',(i[0], i[1]), font, 1,(255,0,0),2,cv2.LINE_AA) + print("YELLOW") + + + return cimg +if __name__ == '__main__': + + + while True: + context = zmq.Context() + socket = context.socket(zmq.REP) + socket.bind("tcp://*:9000") + message = socket.recv() + print("Message is of type {}".format(type(message))) + print(message) + detectimg =detect(message) + sleep(0.05) + # This function either gives out "RED" "YELLOW" or "GREEN" + + From 4b1d3fab0cc4f4fb2cd0844a0fca25443aa28c1c Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Sat, 30 Nov 2019 20:40:56 +0100 Subject: [PATCH 23/31] Delete traffic.py --- selfdrive/traffic.py | 120 ------------------------------------------- 1 file changed, 120 deletions(-) delete mode 100644 selfdrive/traffic.py diff --git a/selfdrive/traffic.py b/selfdrive/traffic.py deleted file mode 100644 index 7a22c540a9225d..00000000000000 --- a/selfdrive/traffic.py +++ /dev/null @@ -1,120 +0,0 @@ -import os -import cv2 -import numpy as np -import zmq - -def detect(source): - - font = cv2.FONT_HERSHEY_SIMPLEX - img = cv2.imread(source) - - cimg = img - hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) - - - lower_red1 = np.array([0,100,100]) - upper_red1 = np.array([10,255,255]) - lower_red2 = np.array([160,100,100]) - upper_red2 = np.array([180,255,255]) - lower_green = np.array([40,50,50]) - upper_green = np.array([90,255,255]) - lower_yellow = np.array([15,150,150]) - upper_yellow = np.array([35,255,255]) - mask1 = cv2.inRange(hsv, lower_red1, upper_red1) - mask2 = cv2.inRange(hsv, lower_red2, upper_red2) - maskg = cv2.inRange(hsv, lower_green, upper_green) - masky = cv2.inRange(hsv, lower_yellow, upper_yellow) - maskr = cv2.add(mask1, mask2) - - size = img.shape - - r_circles = cv2.HoughCircles(maskr, cv2.HOUGH_GRADIENT, 1, 80, - param1=50, param2=10, minRadius=0, maxRadius=30) - - g_circles = cv2.HoughCircles(maskg, cv2.HOUGH_GRADIENT, 1, 60, - param1=50, param2=10, minRadius=0, maxRadius=30) - - y_circles = cv2.HoughCircles(masky, cv2.HOUGH_GRADIENT, 1, 30, - param1=50, param2=5, minRadius=0, maxRadius=30) - - # traffic light detect - r = 5 - bound = 4.0 / 10 - if r_circles is not None: - r_circles = np.uint16(np.around(r_circles)) - - for i in r_circles[0, :]: - if i[0] > size[1] or i[1] > size[0]or i[1] > size[0]*bound: - continue - - h, s = 0.0, 0.0 - for m in range(-r, r): - for n in range(-r, r): - - if (i[1]+m) >= size[0] or (i[0]+n) >= size[1]: - continue - h += maskr[i[1]+m, i[0]+n] - s += 1 - if h / s > 50: - cv2.circle(cimg, (i[0], i[1]), i[2]+10, (0, 255, 0), 2) - cv2.circle(maskr, (i[0], i[1]), i[2]+30, (255, 255, 255), 2) - #cv2.putText(cimg,'RED',(i[0], i[1]), font, 1,(255,0,0),2,cv2.LINE_AA) - print("RED") - if g_circles is not None: - g_circles = np.uint16(np.around(g_circles)) - - for i in g_circles[0, :]: - if i[0] > size[1] or i[1] > size[0] or i[1] > size[0]*bound: - continue - - h, s = 0.0, 0.0 - for m in range(-r, r): - for n in range(-r, r): - - if (i[1]+m) >= size[0] or (i[0]+n) >= size[1]: - continue - h += maskg[i[1]+m, i[0]+n] - s += 1 - if h / s > 100: - cv2.circle(cimg, (i[0], i[1]), i[2]+10, (0, 255, 0), 2) - cv2.circle(maskg, (i[0], i[1]), i[2]+30, (255, 255, 255), 2) - #cv2.putText(cimg,'GREEN',(i[0], i[1]), font, 1,(255,0,0),2,cv2.LINE_AA) - print("GREEN") - if y_circles is not None: - y_circles = np.uint16(np.around(y_circles)) - - for i in y_circles[0, :]: - if i[0] > size[1] or i[1] > size[0] or i[1] > size[0]*bound: - continue - - h, s = 0.0, 0.0 - for m in range(-r, r): - for n in range(-r, r): - - if (i[1]+m) >= size[0] or (i[0]+n) >= size[1]: - continue - h += masky[i[1]+m, i[0]+n] - s += 1 - if h / s > 50: - cv2.circle(cimg, (i[0], i[1]), i[2]+10, (0, 255, 0), 2) - cv2.circle(masky, (i[0], i[1]), i[2]+30, (255, 255, 255), 2) - #cv2.putText(cimg,'YELLOW',(i[0], i[1]), font, 1,(255,0,0),2,cv2.LINE_AA) - print("YELLOW") - - - return cimg -if __name__ == '__main__': - - - while True: - context = zmq.Context() - socket = context.socket(zmq.REP) - socket.bind("tcp://*:9000") - message = socket.recv() - print("Message is of type {}".format(type(message))) - print(message) - detectimg =detect(message) - sleep(0.05) - # This function either gives out "RED" "YELLOW" or "GREEN" - - From 8e70a818d22a559e8888a988654fcadc1035f414 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Sat, 30 Nov 2019 20:44:30 +0100 Subject: [PATCH 24/31] make excecutable --- selfdrive/traffic/traffic.py | 1 + 1 file changed, 1 insertion(+) diff --git a/selfdrive/traffic/traffic.py b/selfdrive/traffic/traffic.py index 7a22c540a9225d..ef2e8d87449a63 100644 --- a/selfdrive/traffic/traffic.py +++ b/selfdrive/traffic/traffic.py @@ -1,3 +1,4 @@ +#!/usr/bin/env python2 import os import cv2 import numpy as np From ae4735e9469fc3e0bf5fa69807a73942188c2a39 Mon Sep 17 00:00:00 2001 From: Comma Device Date: Sat, 30 Nov 2019 20:45:20 +0100 Subject: [PATCH 25/31] make excecuatable --- selfdrive/traffic/traffic.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) mode change 100644 => 100755 selfdrive/traffic/traffic.py diff --git a/selfdrive/traffic/traffic.py b/selfdrive/traffic/traffic.py old mode 100644 new mode 100755 From 182d232ce352554654d30d62b7d1049a83219af4 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Sun, 1 Dec 2019 01:55:03 +0100 Subject: [PATCH 26/31] Disable traffic lights detection for now --- selfdrive/manager.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 76757e8074d219..c253abfd2eb5f9 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -97,7 +97,7 @@ def unblock_stdout(): "gpsd": ("selfdrive/sensord", ["./start_gpsd.py"]), "updated": "selfdrive.updated", "mapd": ("selfdrive/mapd", ["./mapd.py"]), - "traffic": ("selfdrive/traffic", ["./traffic.py"]), + #"traffic": ("selfdrive/traffic", ["./traffic.py"]), } daemon_processes = { "manage_athenad": ("selfdrive.athena.manage_athenad", "AthenadPid"), @@ -141,7 +141,7 @@ def get_running(): 'gpsd', 'deleter', 'mapd', - 'traffic' + #'traffic', ] def register_managed_process(name, desc, car_started=False): From 3f5b23e7be0056c3a4c3006b3b6ca2495b8790b6 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Sun, 1 Dec 2019 02:31:57 +0100 Subject: [PATCH 27/31] Create __init__.py --- selfdrive/traffic/__init__.py | 1 + 1 file changed, 1 insertion(+) create mode 100644 selfdrive/traffic/__init__.py diff --git a/selfdrive/traffic/__init__.py b/selfdrive/traffic/__init__.py new file mode 100644 index 00000000000000..8b137891791fe9 --- /dev/null +++ b/selfdrive/traffic/__init__.py @@ -0,0 +1 @@ + From 4c7a61c0fb090efb2e6112ed715c550fcf2c3a0d Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Sun, 1 Dec 2019 09:20:55 +0100 Subject: [PATCH 28/31] At least 2m needed to keep stopping for traffic lights --- selfdrive/mapd/mapd_helpers.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/mapd/mapd_helpers.py b/selfdrive/mapd/mapd_helpers.py index ea91a6beafa6cf..064090c55738c4 100644 --- a/selfdrive/mapd/mapd_helpers.py +++ b/selfdrive/mapd/mapd_helpers.py @@ -428,7 +428,7 @@ def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead): if backwards and (n.tags['traffic_signals:direction']=='backward' or n.tags['traffic_signals:direction']=='both'): print("backward") if way_pts[count, 0] > 0: - speed_ahead_dist = max(0. , way_pts[count, 0] - 1.0) + speed_ahead_dist = max(0. , way_pts[count, 0] - 2.0) print(speed_ahead_dist) speed_ahead = 5/3.6 if n.tags['highway']=='traffic_signals': @@ -438,7 +438,7 @@ def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead): elif not backwards and (n.tags['traffic_signals:direction']=='forward' or n.tags['traffic_signals:direction']=='both'): print("forward") if way_pts[count, 0] > 0: - speed_ahead_dist = max(0. , way_pts[count, 0] - 1.0) + speed_ahead_dist = max(0. , way_pts[count, 0] - 2.0) print(speed_ahead_dist) speed_ahead = 5/3.6 if n.tags['highway']=='traffic_signals': @@ -454,7 +454,7 @@ def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead): if direction > 180: direction = direction - 360 if abs(direction) > 135: - speed_ahead_dist = max(0. , way_pts[count, 0] - 1.0) + speed_ahead_dist = max(0. , way_pts[count, 0] - 2.0) print(speed_ahead_dist) speed_ahead = 5/3.6 if n.tags['highway']=='traffic_signals': From d843120c3f76cbbd4e121fe6bb8f9cfa36541458 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Sun, 1 Dec 2019 14:00:10 +0100 Subject: [PATCH 29/31] Refine one-way logic Now both roads need to be a one way and it will only choose it if there is one way in the right direction and the other is not. --- selfdrive/mapd/mapd_helpers.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/selfdrive/mapd/mapd_helpers.py b/selfdrive/mapd/mapd_helpers.py index 064090c55738c4..1643871bcf057d 100644 --- a/selfdrive/mapd/mapd_helpers.py +++ b/selfdrive/mapd/mapd_helpers.py @@ -604,11 +604,11 @@ def next_way(self, heading): except (KeyError, IndexError): pass try: - if ways[0].tags['oneway'] == 'yes': - if ways[0].nodes[0].id == node.id and ways[1].nodes[0].id != node.id: + if (ways[0].tags['oneway'] == 'yes') and (ways[1].tags['oneway'] == 'yes'): + if (ways[0].nodes[0].id == node.id and ways[1].nodes[0].id != node.id) and not (ways[0].nodes[0].id != node.id and ways[1].nodes[0].id == node.id): way = Way(ways[0], self.query_results) return way - elif ways[0].nodes[0].id != node.id and ways[1].nodes[0].id == node.id: + elif (ways[0].nodes[0].id != node.id and ways[1].nodes[0].id == node.id) and not (ways[0].nodes[0].id == node.id and ways[1].nodes[0].id != node.id): way = Way(ways[1], self.query_results) return way except (KeyError, IndexError): @@ -640,11 +640,11 @@ def next_way(self, heading): except (KeyError, IndexError): pass try: - if ways[0].tags['oneway'] == 'yes': - if ways[0].nodes[0].id == node.id and ways[1].nodes[0].id != node.id: + if (ways[0].tags['oneway'] == 'yes') and (ways[1].tags['oneway'] == 'yes'): + if (ways[0].nodes[0].id == node.id and ways[1].nodes[0].id != node.id) and not (ways[0].nodes[0].id != node.id and ways[1].nodes[0].id == node.id): way = Way(ways[0], self.query_results) return way - elif ways[0].nodes[0].id != node.id and ways[1].nodes[0].id == node.id: + elif (ways[0].nodes[0].id != node.id and ways[1].nodes[0].id == node.id) and not (ways[0].nodes[0].id == node.id and ways[1].nodes[0].id != node.id): way = Way(ways[1], self.query_results) return way except (KeyError, IndexError): From d91dc7215d8a71dca215364aa8c89e462c9e0af5 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Sun, 1 Dec 2019 18:50:02 +0100 Subject: [PATCH 30/31] fix for right hand turns not slowing down. --- selfdrive/mapd/mapd.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py index 522c53b9eff675..7516043701e8b9 100755 --- a/selfdrive/mapd/mapd.py +++ b/selfdrive/mapd/mapd.py @@ -245,7 +245,7 @@ def mapsd_thread(): circles = [circle_through_points(*p, direction=True) for p in zip(pnts, pnts[1:], pnts[2:])] circles = np.asarray(circles) radii = np.nan_to_num(circles[:, 2]) - radii[radii < 15.] = 10000 + radii[abs(radii) < 15.] = 10000 if cur_way.way.tags['highway'] == 'trunk': radii = radii*1.6 # https://media.springernature.com/lw785/springer-static/image/chp%3A10.1007%2F978-3-658-01689-0_21/MediaObjects/298553_35_De_21_Fig65_HTML.gif From a0241dea291ef0afc380b476ed0de2c5aea681a1 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Sun, 1 Dec 2019 20:56:19 +0100 Subject: [PATCH 31/31] remove integrated value on release and not press of brake/gas --- selfdrive/controls/lib/longcontrol.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index ee86cb4cb2c77b..2b6d14a4c682ee 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -184,10 +184,10 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_ self.pid.k_f=1.0 if gas_pressed or brake_pressed: if not self.freeze: - self.pid.i = 0.0 self.freeze = True else: if self.freeze: + self.pid.i = 0.0 self.freeze = False output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=(prevent_overshoot or gas_pressed or brake_pressed))