diff --git a/opendbc/car/ford/radar_interface.py b/opendbc/car/ford/radar_interface.py index 187e8b7ff3..71dd9e0a3f 100644 --- a/opendbc/car/ford/radar_interface.py +++ b/opendbc/car/ford/radar_interface.py @@ -1,12 +1,16 @@ -import matplotlib -matplotlib.use('Qt5Agg') # Use the Qt5Agg backend -matplotlib.rcParams['figure.raise_window'] = False +try: + import matplotlib + matplotlib.use('Qt5Agg') # Use the Qt5Agg backend + matplotlib.rcParams['figure.raise_window'] = False + import matplotlib.pyplot as plt + from sklearn.cluster import DBSCAN +except: + plt = None import math import copy import numpy as np from math import cos, sin -from sklearn.cluster import DBSCAN from opendbc.can.parser import CANParser from opendbc.car import structs from opendbc.car.common.conversions import Conversions as CV @@ -15,7 +19,6 @@ from opendbc.car.interfaces import RadarInterfaceBase # from scipy.cluster.hierarchy import dendrogram, linkage # from scipy.optimize import linear_sum_assignment -import matplotlib.pyplot as plt DELPHI_ESR_RADAR_MSGS = list(range(0x500, 0x540)) @@ -26,7 +29,6 @@ DELPHI_MRR_RADAR_RANGE_COVERAGE = {0: 42, 1: 164, 2: 45, 3: 175} # scan index to detection range (m) MIN_LONG_RANGE_DIST = 30 # meters -cmap = plt.cm.get_cmap('tab20', 20) # 'tab20' colormap with 20 colors PLOT = False class Cluster: @@ -53,7 +55,7 @@ def vRel(self): # TODO: linalg.norm faster? def calc_dist(pt1, pt2): - return math.sqrt(sum([(p1 - p2) ** 2 for p1, p2 in zip(pt1, pt2, strict=True)])) + return sum([(p1 - p2) ** 2 for p1, p2 in zip(pt1, pt2, strict=True)]) def cluster_points(pts: list[list[float]], max_dist: float): @@ -118,10 +120,11 @@ def __init__(self, CP): # TODO: 2.5 good enough? # TODO: write simple cluster function - self.dbscan = DBSCAN(eps=5, min_samples=1) + # self.dbscan = DBSCAN(eps=5, min_samples=1) if PLOT: self.fig, self.ax = plt.subplots() + self.cmap = plt.cm.get_cmap('tab20', 20) # 'tab20' colormap with 20 colors self.temp_pts = {} @@ -267,7 +270,7 @@ def _update_delphi_mrr(self): temp_points_list = list(self.temp_pts.values()) keys = [[p.dRel, p.yRel, p.vRel] for p in temp_points_list] # labels = self.dbscan.fit_predict(keys) - labels = cluster_points(keys, 5) + labels = cluster_points(keys, 25) # TODO: can be empty clusters = [[] for _ in range(max(labels) + 1)] @@ -360,11 +363,11 @@ def _update_delphi_mrr(self): # print('track_id', self.track_id) # print('cluster_id', self.cluster_id) - if PLOT := False: + if PLOT: self.ax.clear() - colors = [cmap(c.cluster_id % 20) for c in self.clusters] - colors_pts = [cmap(c.trackId % 20) for c in self.temp_pts.values()] + colors = [self.cmap(c.cluster_id % 20) for c in self.clusters] + colors_pts = [self.cmap(c.trackId % 20) for c in self.temp_pts.values()] 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)