Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

radard: add missing accel data for vision-only leads #26619

Closed
wants to merge 6 commits into from
Closed

radard: add missing accel data for vision-only leads #26619

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
d089c50
populate aLead/aLeadK and calc aTau for vision leads
jyoung8607 Nov 26, 2022
92e86a7
oops
jyoung8607 Nov 28, 2022
0a2c724
testing
jyoung8607 Jan 10, 2023
50cce4b
garbage collection
jyoung8607 Jan 10, 2023
f2fb35b
Revert "garbage collection"
jyoung8607 Feb 11, 2023
2f13a00
Revert "testing"
jyoung8607 Feb 11, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 12 additions & 3 deletions selfdrive/controls/lib/radar_helpers.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down Expand Up @@ -130,15 +133,21 @@ 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,
"aLeadK": float(lead_msg.a[0]),
"aLeadTau": _vision_lead_aTau[lead_index],
"fcw": False,
"modelProb": float(lead_msg.prob),
"radar": False,
Expand Down
8 changes: 4 additions & 4 deletions selfdrive/controls/radard.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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)]
Expand Down Expand Up @@ -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


Expand Down