diff --git a/opendbc/car/ford/radar_interface.py b/opendbc/car/ford/radar_interface.py index 0b53c492d0..b24d1f949c 100644 --- a/opendbc/car/ford/radar_interface.py +++ b/opendbc/car/ford/radar_interface.py @@ -2,6 +2,7 @@ matplotlib.use('Qt5Agg') # Use the Qt5Agg backend matplotlib.rcParams['figure.raise_window'] = False +import copy import numpy as np from math import cos, sin from sklearn.cluster import DBSCAN @@ -77,7 +78,7 @@ def __init__(self, CP): self.frame = 0 - self.dbscan = DBSCAN(eps=2.5, min_samples=1) + self.dbscan = DBSCAN(eps=5, min_samples=1) self.fig, self.ax = plt.subplots() @@ -198,10 +199,7 @@ def _update_delphi_mrr(self): if valid: azimuth = msg[f"CAN_DET_AZIMUTH_{ii:02d}"] # rad [-3.1416|3.13964] distRate = msg[f"CAN_DET_RANGE_RATE_{ii:02d}"] # m/s [-128|127.984] - vRel = cos(azimuth) * distRate yRel = -sin(azimuth) * dist # in car frame's y axis, left is positive - if abs(distRate - vRel) > 0.5: - print('distRate', abs(distRate - vRel), distRate, vRel, yRel) dRel = cos(azimuth) * dist # m from front of car # TODO: multiply yRel by 2 @@ -254,35 +252,49 @@ def _update_delphi_mrr(self): print() new_clusters = [] + + print('prev clusters', [(c.dRel, c.yRel, c.vRel) for c in self.clusters]) + for cluster in clusters: # TODO: make clusters a list of Cluster objects dRel = float(np.mean([p.dRel for p in cluster])) yRel = float(np.mean([p.yRel for p in cluster])) vRel = float(np.mean([p.vRel for p in cluster])) + print() + print('working on cluster', (dRel, yRel, vRel)) closest_previous_cluster = None - closest_euclidean_dist = None + closest_euclidean_dist = 999999999 + print('searching! ...') for idx, c in enumerate(self.clusters): - if c.cluster_id in taken_clusters: - continue + # if c.cluster_id in taken_clusters: + # continue # if this new cluster is close to any previous ones, use its previous cluster id with the new points and mark the old cluster as used + print('comparing with prev cluster', (c.dRel, c.yRel, c.vRel)) euclidean_dist = np.sqrt((c.dRel - dRel) ** 2 + (c.yRel - yRel) ** 2 + (c.vRel - vRel) ** 2) + print('got', euclidean_dist) # print(abs(c.dRel - dRel), abs(c.yRel - yRel), euclidean_dist) # if abs(c.dRel - dRel) < 5 and abs(c.yRel - yRel) < 5:# and abs(c.vRel - vRel) < 5: - if euclidean_dist < 20:# and abs(c.vRel - vRel) < 5: + if euclidean_dist < 5:# and abs(c.vRel - vRel) < 5: if closest_previous_cluster is None or euclidean_dist < closest_euclidean_dist: + print('new low!') closest_previous_cluster = c closest_euclidean_dist = euclidean_dist # new_clusters.append(Cluster(cluster, c.cluster_id)) # taken_clusters.add(idx) # break + print() if closest_previous_cluster is not None: - new_clusters.append(Cluster(cluster, closest_previous_cluster.cluster_id)) + print('settled on', closest_euclidean_dist, (closest_previous_cluster.dRel, closest_previous_cluster.yRel, closest_previous_cluster.vRel)) + # TODO: anything better than deepcopy? + new_clusters.append(Cluster(copy.deepcopy(cluster), closest_previous_cluster.cluster_id)) taken_clusters.add(closest_previous_cluster.cluster_id) # print('new!', self.cluster_id) else: - new_clusters.append(Cluster(cluster, self.cluster_id)) + print('making cluster', self.cluster_id, (dRel, yRel, vRel)) + new_clusters.append(Cluster(copy.deepcopy(cluster), self.cluster_id)) + print(new_clusters[-1].dRel, new_clusters[-1].yRel, new_clusters[-1].vRel) self.cluster_id += 1 if len(temp_points_list) == 60: @@ -290,6 +302,8 @@ def _update_delphi_mrr(self): self.clusters = new_clusters + print('post clusters', [(c.dRel, c.yRel, c.vRel) for c in self.clusters]) + if len(temp_points_list) == 60: # print('new self.clusters') # for c in self.clusters: @@ -310,10 +324,15 @@ def _update_delphi_mrr(self): self.ax.set_title(f'clusters: {len(self.clusters)}') self.ax.scatter([c.closestDRel for c in self.clusters], [c.yRel for c in self.clusters], s=80, label='clusters', c=colors) self.ax.scatter([p.dRel for p in self.temp_pts.values()], [p.yRel for p in self.temp_pts.values()], s=10, label='points', color='red') # c=colors_pts) + # text above each point with its dRel and vRel: + # for p in self.temp_pts.values(): + # self.ax.text(p.dRel, p.yRel, f'{p.dRel:.1f}, {p.vRel:.1f}', fontsize=8) + for c in self.clusters: + self.ax.text(c.closestDRel, c.yRel, f'{c.dRel:.1f}, {c.yRel:.1f}, {c.vRel:.1f}, {c.cluster_id}', fontsize=8) self.ax.legend() self.ax.set_xlim(0, 180) self.ax.set_ylim(-30, 30) - plt.pause(1/15) + plt.pause(1/2) self.pts = {} for i, cluster in enumerate(self.clusters):