From d089c500271a5f44811ea08acdbec49c445d1e42 Mon Sep 17 00:00:00 2001 From: Jason Young Date: Sat, 26 Nov 2022 18:29:26 -0500 Subject: [PATCH 1/6] populate aLead/aLeadK and calc aTau for vision leads --- selfdrive/controls/lib/radar_helpers.py | 16 +++++++++++++--- selfdrive/controls/radard.py | 8 ++++---- 2 files changed, 17 insertions(+), 7 deletions(-) diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index 4bb01792677caa..850643468cb043 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -5,6 +5,9 @@ # Default lead acceleration decay set to 50% at 1s _LEAD_ACCEL_TAU = 1.5 +# Hack to maintain vision lead state +_vision_lead_aTau = {0: _LEAD_ACCEL_TAU, 1: _LEAD_ACCEL_TAU} + # radar tracks SPEED, ACCEL = 0, 1 # Kalman filter states enum @@ -130,15 +133,22 @@ def get_RadarState(self, model_prob=0.0): "aLeadTau": float(self.aLeadTau) } - def get_RadarState_from_vision(self, lead_msg, v_ego): + def get_RadarState_from_vision(self, lead_msg, lead_index, v_ego): + # Learn if constant acceleration + if abs(float(lead_msg.a[0])) < 0.5: + _vision_lead_aTau[lead_index] = _LEAD_ACCEL_TAU + else: + _vision_lead_aTau[lead_index] *= 0.9 + return { "dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA), "yRel": float(-lead_msg.y[0]), "vRel": float(lead_msg.v[0] - v_ego), "vLead": float(lead_msg.v[0]), "vLeadK": float(lead_msg.v[0]), - "aLeadK": float(0), - "aLeadTau": _LEAD_ACCEL_TAU, + "aLead": float(lead_msg.a[0]), + "aLeadK": float(lead_msg.a[0]), + "aLeadTau": _vision_lead_aTau[lead_index], "fcw": False, "modelProb": float(lead_msg.prob), "radar": False, diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 3d958139d6413c..589fda405e97cf 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -64,7 +64,7 @@ def prob(c): return None -def get_lead(v_ego, ready, clusters, lead_msg, low_speed_override=True): +def get_lead(v_ego, ready, clusters, lead_msg, lead_index, low_speed_override=True): # Determine leads, this is where the essential logic happens if len(clusters) > 0 and ready and lead_msg.prob > .5: cluster = match_vision_to_cluster(v_ego, lead_msg, clusters) @@ -75,7 +75,7 @@ def get_lead(v_ego, ready, clusters, lead_msg, low_speed_override=True): if cluster is not None: lead_dict = cluster.get_RadarState(lead_msg.prob) elif (cluster is None) and ready and (lead_msg.prob > .5): - lead_dict = Cluster().get_RadarState_from_vision(lead_msg, v_ego) + lead_dict = Cluster().get_RadarState_from_vision(lead_msg, lead_index, v_ego) if low_speed_override: low_speed_clusters = [c for c in clusters if c.potential_low_speed_lead(v_ego)] @@ -171,8 +171,8 @@ def update(self, sm, rr): leads_v3 = sm['modelV2'].leadsV3 if len(leads_v3) > 1: - radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, leads_v3[0], low_speed_override=True) - radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, leads_v3[1], low_speed_override=False) + radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, leads_v3[0], 0, low_speed_override=True) + radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, leads_v3[1], 1, low_speed_override=False) return dat From 92e86a7f48fdc07f831cb5ec084c736aeef303b9 Mon Sep 17 00:00:00 2001 From: Jason Young Date: Mon, 28 Nov 2022 11:06:18 -0500 Subject: [PATCH 2/6] oops --- selfdrive/controls/lib/radar_helpers.py | 1 - 1 file changed, 1 deletion(-) diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index 850643468cb043..862e4983e4b48e 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -146,7 +146,6 @@ def get_RadarState_from_vision(self, lead_msg, lead_index, v_ego): "vRel": float(lead_msg.v[0] - v_ego), "vLead": float(lead_msg.v[0]), "vLeadK": float(lead_msg.v[0]), - "aLead": float(lead_msg.a[0]), "aLeadK": float(lead_msg.a[0]), "aLeadTau": _vision_lead_aTau[lead_index], "fcw": False, From 0a2c7244acdd973a63912f828334e6c5b4602515 Mon Sep 17 00:00:00 2001 From: Jason Young Date: Tue, 10 Jan 2023 16:47:19 -0500 Subject: [PATCH 3/6] testing --- selfdrive/controls/lib/radar_helpers.py | 11 +---------- 1 file changed, 1 insertion(+), 10 deletions(-) diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index 862e4983e4b48e..93b97cc23aea63 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -5,9 +5,6 @@ # Default lead acceleration decay set to 50% at 1s _LEAD_ACCEL_TAU = 1.5 -# Hack to maintain vision lead state -_vision_lead_aTau = {0: _LEAD_ACCEL_TAU, 1: _LEAD_ACCEL_TAU} - # radar tracks SPEED, ACCEL = 0, 1 # Kalman filter states enum @@ -134,12 +131,6 @@ def get_RadarState(self, model_prob=0.0): } def get_RadarState_from_vision(self, lead_msg, lead_index, v_ego): - # Learn if constant acceleration - if abs(float(lead_msg.a[0])) < 0.5: - _vision_lead_aTau[lead_index] = _LEAD_ACCEL_TAU - else: - _vision_lead_aTau[lead_index] *= 0.9 - return { "dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA), "yRel": float(-lead_msg.y[0]), @@ -147,7 +138,7 @@ def get_RadarState_from_vision(self, lead_msg, lead_index, v_ego): "vLead": float(lead_msg.v[0]), "vLeadK": float(lead_msg.v[0]), "aLeadK": float(lead_msg.a[0]), - "aLeadTau": _vision_lead_aTau[lead_index], + "aLeadTau": 0.3, # FIXME: make this a separate named constant "fcw": False, "modelProb": float(lead_msg.prob), "radar": False, From 50cce4bb47d981448a3c5d6b45a6d4494b8eb093 Mon Sep 17 00:00:00 2001 From: Jason Young Date: Tue, 10 Jan 2023 16:49:51 -0500 Subject: [PATCH 4/6] garbage collection --- selfdrive/controls/lib/radar_helpers.py | 2 +- selfdrive/controls/radard.py | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index 93b97cc23aea63..42bddf3ae4a4f7 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -130,7 +130,7 @@ def get_RadarState(self, model_prob=0.0): "aLeadTau": float(self.aLeadTau) } - def get_RadarState_from_vision(self, lead_msg, lead_index, v_ego): + def get_RadarState_from_vision(self, lead_msg, v_ego): return { "dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA), "yRel": float(-lead_msg.y[0]), diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 589fda405e97cf..3d958139d6413c 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -64,7 +64,7 @@ def prob(c): return None -def get_lead(v_ego, ready, clusters, lead_msg, lead_index, low_speed_override=True): +def get_lead(v_ego, ready, clusters, lead_msg, low_speed_override=True): # Determine leads, this is where the essential logic happens if len(clusters) > 0 and ready and lead_msg.prob > .5: cluster = match_vision_to_cluster(v_ego, lead_msg, clusters) @@ -75,7 +75,7 @@ def get_lead(v_ego, ready, clusters, lead_msg, lead_index, low_speed_override=Tr if cluster is not None: lead_dict = cluster.get_RadarState(lead_msg.prob) elif (cluster is None) and ready and (lead_msg.prob > .5): - lead_dict = Cluster().get_RadarState_from_vision(lead_msg, lead_index, v_ego) + lead_dict = Cluster().get_RadarState_from_vision(lead_msg, v_ego) if low_speed_override: low_speed_clusters = [c for c in clusters if c.potential_low_speed_lead(v_ego)] @@ -171,8 +171,8 @@ def update(self, sm, rr): leads_v3 = sm['modelV2'].leadsV3 if len(leads_v3) > 1: - radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, leads_v3[0], 0, low_speed_override=True) - radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, leads_v3[1], 1, low_speed_override=False) + radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, leads_v3[0], low_speed_override=True) + radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, leads_v3[1], low_speed_override=False) return dat From f2fb35b94c1bd1348a06bba7ad627dae468c445d Mon Sep 17 00:00:00 2001 From: Jason Young Date: Sat, 11 Feb 2023 11:13:48 -0500 Subject: [PATCH 5/6] Revert "garbage collection" This reverts commit 50cce4bb47d981448a3c5d6b45a6d4494b8eb093. --- selfdrive/controls/lib/radar_helpers.py | 2 +- selfdrive/controls/radard.py | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index 42bddf3ae4a4f7..93b97cc23aea63 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -130,7 +130,7 @@ def get_RadarState(self, model_prob=0.0): "aLeadTau": float(self.aLeadTau) } - def get_RadarState_from_vision(self, lead_msg, v_ego): + def get_RadarState_from_vision(self, lead_msg, lead_index, v_ego): return { "dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA), "yRel": float(-lead_msg.y[0]), diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 3d958139d6413c..589fda405e97cf 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -64,7 +64,7 @@ def prob(c): return None -def get_lead(v_ego, ready, clusters, lead_msg, low_speed_override=True): +def get_lead(v_ego, ready, clusters, lead_msg, lead_index, low_speed_override=True): # Determine leads, this is where the essential logic happens if len(clusters) > 0 and ready and lead_msg.prob > .5: cluster = match_vision_to_cluster(v_ego, lead_msg, clusters) @@ -75,7 +75,7 @@ def get_lead(v_ego, ready, clusters, lead_msg, low_speed_override=True): if cluster is not None: lead_dict = cluster.get_RadarState(lead_msg.prob) elif (cluster is None) and ready and (lead_msg.prob > .5): - lead_dict = Cluster().get_RadarState_from_vision(lead_msg, v_ego) + lead_dict = Cluster().get_RadarState_from_vision(lead_msg, lead_index, v_ego) if low_speed_override: low_speed_clusters = [c for c in clusters if c.potential_low_speed_lead(v_ego)] @@ -171,8 +171,8 @@ def update(self, sm, rr): leads_v3 = sm['modelV2'].leadsV3 if len(leads_v3) > 1: - radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, leads_v3[0], low_speed_override=True) - radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, leads_v3[1], low_speed_override=False) + radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, leads_v3[0], 0, low_speed_override=True) + radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, leads_v3[1], 1, low_speed_override=False) return dat From 2f13a00c324b79b3450d9e1e314c7f47f7174e40 Mon Sep 17 00:00:00 2001 From: Jason Young Date: Sat, 11 Feb 2023 11:13:56 -0500 Subject: [PATCH 6/6] Revert "testing" This reverts commit 0a2c7244acdd973a63912f828334e6c5b4602515. --- selfdrive/controls/lib/radar_helpers.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index 93b97cc23aea63..862e4983e4b48e 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -5,6 +5,9 @@ # Default lead acceleration decay set to 50% at 1s _LEAD_ACCEL_TAU = 1.5 +# Hack to maintain vision lead state +_vision_lead_aTau = {0: _LEAD_ACCEL_TAU, 1: _LEAD_ACCEL_TAU} + # radar tracks SPEED, ACCEL = 0, 1 # Kalman filter states enum @@ -131,6 +134,12 @@ def get_RadarState(self, model_prob=0.0): } def get_RadarState_from_vision(self, lead_msg, lead_index, v_ego): + # Learn if constant acceleration + if abs(float(lead_msg.a[0])) < 0.5: + _vision_lead_aTau[lead_index] = _LEAD_ACCEL_TAU + else: + _vision_lead_aTau[lead_index] *= 0.9 + return { "dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA), "yRel": float(-lead_msg.y[0]), @@ -138,7 +147,7 @@ def get_RadarState_from_vision(self, lead_msg, lead_index, v_ego): "vLead": float(lead_msg.v[0]), "vLeadK": float(lead_msg.v[0]), "aLeadK": float(lead_msg.a[0]), - "aLeadTau": 0.3, # FIXME: make this a separate named constant + "aLeadTau": _vision_lead_aTau[lead_index], "fcw": False, "modelProb": float(lead_msg.prob), "radar": False,