import math import os from collections import deque from iqdbc import DBC_PATH from iqdbc.can import CANParser from iqdbc.car import Bus, structs from iqdbc.car.interfaces import RadarInterfaceBase from iqdbc.car.hyundai.values import DBC, HyundaiFlags, HyundaiExtFlags from iqpilot.common.params import Params from iqdbc.car.hyundai.hyundaicanfd import CanBus from iqpilot.common.filter_simple import MyMovingAverage SCC_TID = 0 RADAR_START_ADDR = 0x500 RADAR_MSG_COUNT = 32 RADAR_MSG_COUNT4 = 8 RADAR_GROUP4_MAX_LONG_DIST = 325.0 RADAR_GROUP4_MAX_YREL = 6.0 RADAR_START_ADDR_CANFD1 = 0x210 RADAR_MSG_COUNT1 = 16 RADAR_START_ADDR_CANFD2 = 0x3A5 # Group 2, Group 1: 0x210 2개씩?어???단 보류. RADAR_MSG_COUNT2 = 32 RADAR_START_ADDR_CANFD3 = 0x400 RADAR_MSG_COUNT3 = 30 CORNER_OBJECT_235_START_ADDR = 0x235 CORNER_OBJECT_235_MSG_COUNT = 20 CORNER_OBJECT_235_TRACK_ID_OFFSET = 200 CORNER_OBJECT_235_DBC = 'hyundai_canfd_corner_radar_235_generated' CORNER_OBJECT_180_START_ADDR = 0x180 CORNER_OBJECT_180_MSG_COUNT = 5 CORNER_OBJECT_180_SLOTS_PER_MSG = 2 CORNER_OBJECT_180_TRACK_ID_OFFSET = 240 CORNER_OBJECT_180_DBC = 'hyundai_canfd_corner_radar_180_generated' CORNER_OBJECT_430_LEFT_START_ADDR = 0x430 CORNER_OBJECT_430_RIGHT_START_ADDR = 0x440 CORNER_OBJECT_430_MSG_COUNT_PER_SIDE = 8 CORNER_OBJECT_430_SLOTS_PER_MSG = 7 CORNER_OBJECT_430_TRACK_ID_OFFSET = 300 CORNER_OBJECT_430_DBC = 'hyundai_canfd_corner_radar_430_generated' CORNER_OBJECT_430_EMPTY_RAW_VALUES = (0x010d1f40, 0x00010d1f) CORNER_OBJECT_430_DEFAULT_DISTANCE_RAW_MIN = 2520 # 126.0 m CORNER_OBJECT_430_DEFAULT_DISTANCE_RAW_MAX = 2600 # 130.0 m CORNER_OBJECT_430_MAX_DREL = 120.0 CORNER_OBJECT_430_MAX_TRACKS_PER_SIDE = 4 CORNER_OBJECT_430_DT = 0.05 CORNER_OBJECT_430_MAX_DREL_DELTA = 1.5 CORNER_OBJECT_430_CANDIDATE_META_BYTE_3 = (2,) CORNER_OBJECT_430_CANDIDATE_EXCLUDED_SLOTS = (1,) CORNER_OBJECT_430_CANDIDATE_RAW_DELTA = 200 CORNER_OBJECT_430_STRONG_META_BYTE_2 = (10,) CORNER_OBJECT_430_WEAK_META_BYTE_2 = (5, 6, 7, 8, 9) CORNER_OBJECT_430_STRONG_MIN_SUPPORT = 2 CORNER_OBJECT_430_WEAK_MIN_SUPPORT = 3 CORNER_OBJECT_430_CLUSTER_RAW_GAP = 200 CORNER_OBJECT_430_TRACK_MATCH_MAX_DREL_DELTA = 3.0 CORNER_OBJECT_430_MAX_ABS_VREL = 20.0 CORNER_OBJECT_430_MAX_ABS_YVREL = 3.0 CORNER_OBJECT_430_VREL_ALPHA = 0.35 CORNER_OBJECT_430_YVREL_ALPHA = 0.35 CORNER_OBJECT_430_LATERAL_CELL_MSG_WEIGHT = 0.35 CORNER_OBJECT_430_LATERAL_CELL_SLOT_WEIGHT = 0.65 CORNER_OBJECT_430_YREL_OFFSET = 5.8 CORNER_OBJECT_430_YREL_SCALE = 1.1 CORNER_OBJECT_430_RIGHT_CELL_MIRROR = 7.0 CORNER_OBJECT_430_MIN_ABS_YREL = 0.8 CORNER_OBJECT_430_MAX_ABS_YREL = 4.2 CORNER_OBJECT_430_HISTORY_SIZE = 8 CORNER_OBJECT_430_MIN_HISTORY = 5 CORNER_OBJECT_430_MIN_INWARD_YREL_DELTA = 0.35 CORNER_OBJECT_430_MIN_RECENT_INWARD_YREL_DELTA = 0.05 CORNER_OBJECT_430_MIN_INWARD_RATIO = 0.65 CORNER_OBJECT_430_INWARD_CENTER_ABS_YREL = 1.55 CORNER_OBJECT_430_INWARD_KEEP_YVREL_ABS_YREL = 2.2 CORNER_OBJECT_430_EARLY_INWARD_NONCENTER_FRAMES = 2 CORNER_OBJECT_430_SIDE_KEEP_ABS_YREL = 2.0 CORNER_OBJECT_STABLE_TRACK_ID_START = 1000 CORNER_SIDE_OBJECT_MAX_DREL = 0.2 CORNER_SIDE_OBJECT_MIN_ABS_YREL = 1.4 CORNER_SIDE_OBJECT_MAX_ABS_YREL = 4.5 # POC for parsing corner radars: https://github.com/commaai/openpilot/pull/24221/ class CornerObjectTrackIdManager: def __init__(self): self.next_track_id = CORNER_OBJECT_STABLE_TRACK_ID_START self.objects: dict[tuple[str, int], tuple[int, int]] = {} def get_track_id(self, source: str, object_id: int, age: int) -> int: key = (source, object_id) previous = self.objects.get(key) if previous is None or age < previous[1]: track_id = self.next_track_id self.next_track_id += 1 else: track_id = previous[0] self.objects[key] = (track_id, age) return track_id def clear_source(self, source: str): self.objects = {key: value for key, value in self.objects.items() if key[0] != source} def corner_object_position_valid(d_rel: float, y_rel: float) -> bool: normal_object = 0.2 < d_rel < 180.0 clipped_side_object = ( 0.0 <= d_rel <= CORNER_SIDE_OBJECT_MAX_DREL and CORNER_SIDE_OBJECT_MIN_ABS_YREL <= abs(y_rel) <= CORNER_SIDE_OBJECT_MAX_ABS_YREL ) return (normal_object or clipped_side_object) and abs(y_rel) < 40.0 def get_radar_can_parser(CP, radar_tracks, msg_start_addr, msg_count, radar_group4=False): if not radar_tracks: return None #if Bus.radar not in DBC[CP.carFingerprint]: # return None print("RadarInterface: RadarTracks...") if CP.flags & HyundaiFlags.CANFD: CAN = CanBus(CP) messages = [(f"RADAR_TRACK_{addr:x}", 20) for addr in range(msg_start_addr, msg_start_addr + msg_count)] return CANParser('hyundai_canfd_radar_generated', messages, CAN.ACAN) else: messages = [(f"RADAR_TRACK_{addr:x}", 20) for addr in range(msg_start_addr, msg_start_addr + msg_count)] #return CANParser(DBC[CP.carFingerprint][Bus.radar], messages, 1) dbc_name = 'hyundai_kia_denso_front_radar_generated' if radar_group4 else 'hyundai_kia_mando_front_radar_generated' return CANParser(dbc_name, messages, 1) def get_corner_object_can_parser(CP, enabled): if not enabled or not (CP.flags & HyundaiFlags.CANFD): return None dbc_path = os.path.join(DBC_PATH, f"{CORNER_OBJECT_235_DBC}.dbc") if not os.path.exists(dbc_path): print(f"RadarInterface: missing {CORNER_OBJECT_235_DBC}.dbc, 0x235 corner radar disabled") return None CAN = CanBus(CP) messages = [(f"CORNER_RADAR_235_OBJECTS_{addr:x}", 33) for addr in range(CORNER_OBJECT_235_START_ADDR, CORNER_OBJECT_235_START_ADDR + CORNER_OBJECT_235_MSG_COUNT)] return CANParser(CORNER_OBJECT_235_DBC, messages, CAN.ACAN) def get_corner_object_180_can_parser(CP, enabled): if not enabled or not (CP.flags & HyundaiFlags.CANFD): return None dbc_path = os.path.join(DBC_PATH, f"{CORNER_OBJECT_180_DBC}.dbc") if not os.path.exists(dbc_path): print(f"RadarInterface: missing {CORNER_OBJECT_180_DBC}.dbc, 0x180 corner radar disabled") return None CAN = CanBus(CP) messages = [(f"CORNER_RADAR_180_OBJECTS_{addr:x}", 33) for addr in range(CORNER_OBJECT_180_START_ADDR, CORNER_OBJECT_180_START_ADDR + CORNER_OBJECT_180_MSG_COUNT)] return CANParser(CORNER_OBJECT_180_DBC, messages, CAN.ACAN) def get_corner_object_430_can_parser(CP, enabled): if not enabled or not (CP.flags & HyundaiFlags.CANFD): return None dbc_path = os.path.join(DBC_PATH, f"{CORNER_OBJECT_430_DBC}.dbc") if not os.path.exists(dbc_path): print(f"RadarInterface: missing {CORNER_OBJECT_430_DBC}.dbc, 0x430/0x440 corner radar disabled") return None CAN = CanBus(CP) messages = [(f"CORNER_RADAR_430_OBJECTS_{addr:x}", 33) for addr in range(CORNER_OBJECT_430_LEFT_START_ADDR, CORNER_OBJECT_430_LEFT_START_ADDR + CORNER_OBJECT_430_MSG_COUNT_PER_SIDE)] messages += [(f"CORNER_RADAR_430_OBJECTS_{addr:x}", 33) for addr in range(CORNER_OBJECT_430_RIGHT_START_ADDR, CORNER_OBJECT_430_RIGHT_START_ADDR + CORNER_OBJECT_430_MSG_COUNT_PER_SIDE)] return CANParser(CORNER_OBJECT_430_DBC, messages, CAN.ACAN) def get_radar_can_parser_scc(CP): CAN = CanBus(CP) if CP.flags & HyundaiFlags.CANFD: messages = [("SCC_CONTROL", 50)] bus = CAN.ECAN else: messages = [("SCC11", 50)] bus = CAN.ECAN print("$$$$$$$$ ECAN = ", CAN.ECAN) bus = CAN.CAM if CP.flags & HyundaiFlags.CAMERA_SCC else bus return CANParser(DBC[CP.carFingerprint][Bus.pt], messages, bus) class RadarInterface(RadarInterfaceBase): def __init__(self, CP, CP_IQ=None): super().__init__(CP, CP_IQ or structs.IQCarParams()) self.v_ego = 0.0 self.canfd = True if CP.flags & HyundaiFlags.CANFD else False self.radar_group1 = False self.radar_group3 = False self.radar_group4 = not self.canfd and bool(CP.extFlags & HyundaiExtFlags.RADAR_GROUP4.value) if self.canfd: if CP.extFlags & HyundaiExtFlags.RADAR_GROUP1.value: self.radar_start_addr = RADAR_START_ADDR_CANFD1 self.radar_msg_count = RADAR_MSG_COUNT1 self.radar_group1 = True elif CP.extFlags & HyundaiExtFlags.RADAR_GROUP3.value: self.radar_start_addr = RADAR_START_ADDR_CANFD3 self.radar_msg_count = RADAR_MSG_COUNT3 self.radar_group3 = True else: self.radar_start_addr = RADAR_START_ADDR_CANFD2 self.radar_msg_count = RADAR_MSG_COUNT2 else: self.radar_start_addr = RADAR_START_ADDR self.radar_msg_count = RADAR_MSG_COUNT4 if self.radar_group4 else RADAR_MSG_COUNT self.params = Params() self.radar_tracks = self.params.get_int("EnableRadarTracks") >= 1 self.corner_object_tracks = bool(CP.extFlags & HyundaiExtFlags.CORNER_RADAR_OBJECTS_235.value) and self.params.get_int("EnableCornerRadar") > 0 self.corner_object_180_tracks = bool(CP.extFlags & HyundaiExtFlags.CORNER_RADAR_OBJECTS_180.value) and self.params.get_int("EnableCornerRadar") > 0 self.corner_object_430_tracks = bool(CP.extFlags & HyundaiExtFlags.CORNER_RADAR_OBJECTS_430.value) and self.params.get_int("EnableCornerRadar") > 0 self.updated_tracks = set() self.updated_scc = set() self.updated_corner_objects = set() self.updated_corner_objects_180 = set() self.updated_corner_objects_430 = set() self.corner_object_missed_updates = 0 self.corner_object_180_missed_updates = 0 self.corner_object_430_missed_updates = 0 self.corner_object_track_ids = CornerObjectTrackIdManager() self.rcp_tracks = get_radar_can_parser(CP, self.radar_tracks, self.radar_start_addr, self.radar_msg_count, self.radar_group4) self.rcp_corner_objects = get_corner_object_can_parser(CP, self.corner_object_tracks) self.rcp_corner_objects_180 = get_corner_object_180_can_parser(CP, self.corner_object_180_tracks) self.rcp_corner_objects_430 = get_corner_object_430_can_parser(CP, self.corner_object_430_tracks) # Enabling raw radar tracks on legacy CAN disables the stock SCC11 stream on # some Hyundai/Kia platforms. Camera-SCC cars may still use SCC11. use_scc_parser = not (self.radar_tracks and not self.canfd and not (CP.flags & HyundaiFlags.CAMERA_SCC)) self.rcp_scc = get_radar_can_parser_scc(CP) if use_scc_parser else None self.trigger_msg_scc = 416 if self.canfd else 0x420 self.trigger_msg_tracks = self.radar_start_addr + self.radar_msg_count - 1 self.trigger_msg_corner_objects = CORNER_OBJECT_235_START_ADDR + CORNER_OBJECT_235_MSG_COUNT - 1 self.trigger_msg_corner_objects_180 = CORNER_OBJECT_180_START_ADDR + CORNER_OBJECT_180_MSG_COUNT - 1 self.trigger_msg_corner_objects_430 = CORNER_OBJECT_430_RIGHT_START_ADDR + CORNER_OBJECT_430_MSG_COUNT_PER_SIDE - 1 self.track_id = 0 self.corner_objects_available = self.rcp_corner_objects is not None or self.rcp_corner_objects_180 is not None or self.rcp_corner_objects_430 is not None self.radar_off_can = CP.radarUnavailable and not self.corner_objects_available print( "RadarInterface: " f"radarUnavailable={CP.radarUnavailable} radarTracks={self.radar_tracks} " f"group4={self.radar_group4} " f"corner235={self.rcp_corner_objects is not None} corner180={self.rcp_corner_objects_180 is not None} " f"corner430={self.rcp_corner_objects_430 is not None} " f"radarOffCan={self.radar_off_can}" ) self.vRel_last = 0 self.dRel_last = 0 self.corner_object_430_prev_d_rel = {} self.corner_object_430_prev_v_rel = {} self.corner_object_430_prev_y_rel = {} self.corner_object_430_prev_yv_rel = {} self.corner_object_430_prev_code = {} self.corner_object_430_history = {} self.corner_object_430_noncenter_inward_frames = {} # Initialize pts if self.rcp_tracks is not None: total_tracks = self.radar_msg_count * (2 if self.radar_group1 else 1) for track_id in range(total_tracks): t_id = track_id + 32 self.pts[t_id] = structs.RadarData.RadarPoint() self.pts[t_id].measured = False self.pts[t_id].trackId = t_id if self.rcp_scc is not None: self.pts[SCC_TID] = structs.RadarData.RadarPoint() self.pts[SCC_TID].trackId = SCC_TID self.pts[SCC_TID].radarSource = "scc" if self.rcp_corner_objects is not None: for slot in range(CORNER_OBJECT_235_MSG_COUNT): t_id = CORNER_OBJECT_235_TRACK_ID_OFFSET + slot self.pts[t_id] = structs.RadarData.RadarPoint() self.pts[t_id].measured = False self.pts[t_id].trackId = t_id self.pts[t_id].radarSource = "corner235" if self.rcp_corner_objects_180 is not None: for slot in range(CORNER_OBJECT_180_MSG_COUNT * CORNER_OBJECT_180_SLOTS_PER_MSG): t_id = CORNER_OBJECT_180_TRACK_ID_OFFSET + slot self.pts[t_id] = structs.RadarData.RadarPoint() self.pts[t_id].measured = False self.pts[t_id].trackId = t_id self.pts[t_id].radarSource = "corner180" if self.rcp_corner_objects_430 is not None: for slot in range(CORNER_OBJECT_430_MSG_COUNT_PER_SIDE * 2 * CORNER_OBJECT_430_SLOTS_PER_MSG): t_id = CORNER_OBJECT_430_TRACK_ID_OFFSET + slot self.pts[t_id] = structs.RadarData.RadarPoint() self.pts[t_id].measured = False self.pts[t_id].trackId = t_id self.frame = 0 def update(self, can_strings): self.frame += 1 if self.radar_off_can or (self.rcp_tracks is None and self.rcp_scc is None and self.rcp_corner_objects is None and self.rcp_corner_objects_180 is None and self.rcp_corner_objects_430 is None): return super().update(None) if self.rcp_scc is not None: vls_s = self.rcp_scc.update(can_strings) self.updated_scc.update(vls_s) track_ready = False if self.radar_tracks and self.rcp_tracks is not None: vls_t = self.rcp_tracks.update(can_strings) self.updated_tracks.update(vls_t) track_ready = self.trigger_msg_tracks in self.updated_tracks corner_ready = False if self.rcp_corner_objects is not None: vls_c = self.rcp_corner_objects.update(can_strings) self.updated_corner_objects.update(vls_c) corner_ready = self.trigger_msg_corner_objects in self.updated_corner_objects corner_180_ready = False if self.rcp_corner_objects_180 is not None: vls_180 = self.rcp_corner_objects_180.update(can_strings) self.updated_corner_objects_180.update(vls_180) corner_180_ready = self.trigger_msg_corner_objects_180 in self.updated_corner_objects_180 corner_430_ready = False if self.rcp_corner_objects_430 is not None: vls_430 = self.rcp_corner_objects_430.update(can_strings) self.updated_corner_objects_430.update(vls_430) corner_430_ready = self.trigger_msg_corner_objects_430 in self.updated_corner_objects_430 scc_ready = not self.radar_tracks and self.frame % 5 == 0 and self.rcp_scc is not None if track_ready: self._update(self.updated_tracks) self.updated_tracks.clear() if corner_ready: self._update_corner_objects(self.updated_corner_objects) self.corner_object_missed_updates = 0 self.updated_corner_objects.clear() if corner_180_ready: self._update_corner_objects_180(self.updated_corner_objects_180) self.corner_object_180_missed_updates = 0 self.updated_corner_objects_180.clear() if corner_430_ready: self._update_corner_objects_430(self.updated_corner_objects_430) self.corner_object_430_missed_updates = 0 self.updated_corner_objects_430.clear() # Corner radar runs at its own cadence. Do not let corner-only frames publish # RadarData, since liveTracks uses a fixed radarTimeStep for aLead/jLead. publish_ready = track_ready or scc_ready if not publish_ready: return None if self.rcp_scc is not None: self._update_scc(self.updated_scc) if self.rcp_corner_objects is not None: if self.updated_corner_objects: self._update_corner_objects(self.updated_corner_objects) self.corner_object_missed_updates = 0 else: self.corner_object_missed_updates += 1 if self.corner_object_missed_updates > 10: self._clear_corner_objects() if self.rcp_corner_objects_180 is not None: if self.updated_corner_objects_180: self._update_corner_objects_180(self.updated_corner_objects_180) self.corner_object_180_missed_updates = 0 else: self.corner_object_180_missed_updates += 1 if self.corner_object_180_missed_updates > 10: self._clear_corner_objects_180() if self.rcp_corner_objects_430 is not None: if self.updated_corner_objects_430: self._update_corner_objects_430(self.updated_corner_objects_430) self.corner_object_430_missed_updates = 0 else: self.corner_object_430_missed_updates += 1 if self.corner_object_430_missed_updates > 10: self._clear_corner_objects_430() self.updated_scc.clear() self.updated_corner_objects.clear() self.updated_corner_objects_180.clear() self.updated_corner_objects_430.clear() ret = structs.RadarData() if ((self.rcp_tracks is not None and self.radar_tracks and not self.rcp_tracks.can_valid) or (self.rcp_scc is not None and not self.corner_objects_available and not self.rcp_scc.can_valid) or (self.rcp_corner_objects is not None and not self.rcp_corner_objects.can_valid) or (self.rcp_corner_objects_180 is not None and not self.rcp_corner_objects_180.can_valid) or (self.rcp_corner_objects_430 is not None and not self.rcp_corner_objects_430.can_valid)): ret.errors.canError = True ret.points = [point for point in self.pts.values() if point.measured] return ret def _update(self, updated_messages): t_id = 32 for addr in range(self.radar_start_addr, self.radar_start_addr + self.radar_msg_count): msg = self.rcp_tracks.vl[f"RADAR_TRACK_{addr:x}"] if self.radar_group1: valid = msg['VALID_CNT1'] > 10 elif self.radar_group3: # Group 3 marks an empty object slot with LONG_DIST raw 0x7ff (204.7 m). valid = msg['LONG_DIST'] < 204.7 elif self.canfd: valid = msg['VALID_CNT'] > 10 elif self.radar_group4: # EN: DNMWR006 exposes eight stable tracked-object slots at 0x500-0x507. # Messages from 0x508 onward are distance-sorted raw detections without # stable IDs, so they are excluded. OBJECT_STATE 3 is a confirmed track; # empty slots use LONG_DIST raw 0xfff8 (409.55 m). Driving logs reached # 317.80 m, so 325 m preserves every observed confirmed track while # retaining margin from the empty-slot sentinel. Keep the +/-6 m # ego/adjacent-lane envelope to suppress farther roadside reflections. # KO: DNMWR006의 안정적인 추적 객체 슬롯은 0x500~0x507의 8개임. # 0x508 이후 메시지는 고정 ID가 없는 거리순 raw detection이므로 제외함. # OBJECT_STATE 3은 확정 추적 객체이며, 빈 슬롯은 LONG_DIST raw # 0xfff8(409.55m)을 사용함. 주행 로그의 최대값은 317.80m였으므로 # 325m 상한으로 관측된 확정 트랙을 모두 보존하면서 빈 슬롯 값과 충분한 # 여유를 확보함. 원거리 도로변 반사를 줄이기 위해 좌우 6m 범위를 유지함. valid = (msg['OBJECT_STATE'] == 3 and 0.2 < msg['LONG_DIST'] < RADAR_GROUP4_MAX_LONG_DIST and abs(msg['LAT_DIST']) <= RADAR_GROUP4_MAX_YREL) else: valid = msg['STATE'] in (3, 4) self.pts[t_id].measured = bool(valid) if not valid: self.pts[t_id].dRel = 0 self.pts[t_id].yRel = 0 self.pts[t_id].vRel = 0 self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego self.pts[t_id].aRel = float('nan') self.pts[t_id].yvRel = 0 elif self.radar_group1: self.pts[t_id].dRel = msg['LONG_DIST1'] self.pts[t_id].yRel = msg['LAT_DIST1'] self.pts[t_id].vRel = msg['REL_SPEED1'] self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego self.pts[t_id].aRel = msg['REL_ACCEL1'] self.pts[t_id].yvRel = msg['LAT_SPEED1'] elif self.canfd: if self.radar_group3: # Group 3 reports the object's center. Convert it to the rear surface to match SCC/vision dRel. self.pts[t_id].dRel = max(0.0, msg['LONG_DIST'] - msg['OBJECT_LENGTH'] * 0.5 - 0.1) else: self.pts[t_id].dRel = msg['LONG_DIST'] self.pts[t_id].yRel = msg['LAT_DIST'] self.pts[t_id].vRel = msg['REL_SPEED'] self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego self.pts[t_id].aRel = float('nan') if self.radar_group3 else msg['REL_ACCEL'] self.pts[t_id].yvRel = 0.0 if self.radar_group3 else msg['LAT_SPEED'] elif self.radar_group4: self.pts[t_id].dRel = msg['LONG_DIST'] self.pts[t_id].yRel = -msg['LAT_DIST'] self.pts[t_id].vRel = msg['REL_SPEED'] self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego self.pts[t_id].aRel = float('nan') self.pts[t_id].yvRel = 0.0 else: azimuth = math.radians(msg['AZIMUTH']) self.pts[t_id].dRel = math.cos(azimuth) * msg['LONG_DIST'] self.pts[t_id].yRel = 0.5 * -math.sin(azimuth) * msg['LONG_DIST'] self.pts[t_id].vRel = msg['REL_SPEED'] self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego self.pts[t_id].aRel = msg['REL_ACCEL'] self.pts[t_id].yvRel = 0.0 t_id += 1 # radar group1? ?나??msg??2개의 ?이?? ?어?음. if self.radar_group1: for addr in range(self.radar_start_addr, self.radar_start_addr + self.radar_msg_count): msg = self.rcp_tracks.vl[f"RADAR_TRACK_{addr:x}"] valid = msg['VALID_CNT2'] > 10 self.pts[t_id].measured = bool(valid) if not valid: self.pts[t_id].dRel = 0 self.pts[t_id].yRel = 0 self.pts[t_id].vRel = 0 self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego self.pts[t_id].aRel = float('nan') self.pts[t_id].yvRel = 0 else: self.pts[t_id].dRel = msg['LONG_DIST2'] self.pts[t_id].yRel = msg['LAT_DIST2'] self.pts[t_id].vRel = msg['REL_SPEED2'] self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego self.pts[t_id].aRel = msg['REL_ACCEL2'] self.pts[t_id].yvRel = msg['LAT_SPEED2'] t_id += 1 def _update_corner_objects(self, updated_messages): if self.rcp_corner_objects is None: return if not updated_messages: self._clear_corner_objects() return candidates = [] for slot, addr in enumerate(range(CORNER_OBJECT_235_START_ADDR, CORNER_OBJECT_235_START_ADDR + CORNER_OBJECT_235_MSG_COUNT)): t_id = CORNER_OBJECT_235_TRACK_ID_OFFSET + slot msg = self.rcp_corner_objects.vl[f"CORNER_RADAR_235_OBJECTS_{addr:x}"] d_rel = msg["OBJ_REL_POS_X"] y_rel = msg["OBJ_REL_POS_Y"] v_rel = msg["OBJ_REL_VEL_X"] yv_rel = msg["OBJ_REL_VEL_Y"] a_rel = msg["OBJ_REL_ACCEL_X"] # Side objects are clipped to x=0 by the corner radar. Quality, identity, # and lateral motion still describe a real object, so keep them for # corner-confirmed front-radar association in radard. valid = msg["OBJ_QUAL_LEVEL"] > 0 and corner_object_position_valid(d_rel, y_rel) and v_rel > -99.0 if not valid: continue candidates.append((t_id, int(msg["OBJ_OBJECT_ID"]), int(msg["OBJ_AGE"]), int(msg["OBJ_QUAL_LEVEL"]), d_rel, y_rel, v_rel, yv_rel, a_rel)) self._apply_corner_objects("corner235", candidates, range(CORNER_OBJECT_235_TRACK_ID_OFFSET, CORNER_OBJECT_235_TRACK_ID_OFFSET + CORNER_OBJECT_235_MSG_COUNT)) def _update_corner_objects_180(self, updated_messages): if self.rcp_corner_objects_180 is None: return if not updated_messages: self._clear_corner_objects_180() return candidates = [] for msg_index, addr in enumerate(range(CORNER_OBJECT_180_START_ADDR, CORNER_OBJECT_180_START_ADDR + CORNER_OBJECT_180_MSG_COUNT)): msg = self.rcp_corner_objects_180.vl[f"CORNER_RADAR_180_OBJECTS_{addr:x}"] for slot_index in range(CORNER_OBJECT_180_SLOTS_PER_MSG): t_id = CORNER_OBJECT_180_TRACK_ID_OFFSET + msg_index * CORNER_OBJECT_180_SLOTS_PER_MSG + slot_index prefix = f"SLOT{slot_index + 1}_" d_rel = msg[f"{prefix}REL_POS_X"] y_rel = msg[f"{prefix}REL_POS_Y"] v_rel = msg[f"{prefix}REL_VEL_X"] yv_rel = msg[f"{prefix}REL_VEL_Y"] a_rel = msg[f"{prefix}REL_ACCEL_X"] valid = msg[f"{prefix}QUAL_LEVEL"] > 0 and corner_object_position_valid(d_rel, y_rel) and v_rel > -99.0 if not valid: continue candidates.append((t_id, int(msg[f"{prefix}OBJECT_ID"]), int(msg[f"{prefix}AGE"]), int(msg[f"{prefix}QUAL_LEVEL"]), d_rel, y_rel, v_rel, yv_rel, a_rel)) self._apply_corner_objects("corner180", candidates, range(CORNER_OBJECT_180_TRACK_ID_OFFSET, CORNER_OBJECT_180_TRACK_ID_OFFSET + CORNER_OBJECT_180_MSG_COUNT * CORNER_OBJECT_180_SLOTS_PER_MSG)) def _apply_corner_objects(self, source, candidates, slot_ids): for t_id in slot_ids: self._clear_point(t_id) # The same object can occupy two CAN slots for one cycle during a slot handoff. # Publish only the newest/highest-quality copy so trackId stays unique. objects = {} for candidate in candidates: object_id = candidate[1] previous = objects.get(object_id) if previous is None or (candidate[2], candidate[3]) > (previous[2], previous[3]): objects[object_id] = candidate for t_id, object_id, age, _, d_rel, y_rel, v_rel, yv_rel, a_rel in objects.values(): point = self.pts[t_id] point.measured = True point.trackId = self.corner_object_track_ids.get_track_id(source, object_id, age) point.radarSource = source point.dRel = d_rel point.yRel = y_rel point.vRel = v_rel point.vLead = v_rel + self.v_ego point.aRel = a_rel point.yvRel = yv_rel def _update_corner_objects_430(self, updated_messages): if self.rcp_corner_objects_430 is None: return if not updated_messages: self._clear_corner_objects_430() return bank_defs = ( (CORNER_OBJECT_430_LEFT_START_ADDR, 1.0, 0), (CORNER_OBJECT_430_RIGHT_START_ADDR, -1.0, CORNER_OBJECT_430_MSG_COUNT_PER_SIDE * CORNER_OBJECT_430_SLOTS_PER_MSG), ) for start_addr, side_sign, track_base in bank_defs: bins = [] for msg_index, addr in enumerate(range(start_addr, start_addr + CORNER_OBJECT_430_MSG_COUNT_PER_SIDE)): msg = self.rcp_corner_objects_430.vl[f"CORNER_RADAR_430_OBJECTS_{addr:x}"] for slot_index in range(CORNER_OBJECT_430_SLOTS_PER_MSG): prefix = f"SLOT{slot_index + 1}_" distance_raw = int(msg[f"{prefix}DISTANCE_RAW"]) raw = ( distance_raw | (int(msg[f"{prefix}META_13_15"]) << 13) | (int(msg[f"{prefix}META_BYTE_2"]) << 16) | (int(msg[f"{prefix}META_BYTE_3"]) << 24) ) code = ( int(msg[f"{prefix}META_13_15"]), int(msg[f"{prefix}META_BYTE_2"]), int(msg[f"{prefix}META_BYTE_3"]), ) d_rel = distance_raw * 0.05 default_distance = CORNER_OBJECT_430_DEFAULT_DISTANCE_RAW_MIN <= distance_raw <= CORNER_OBJECT_430_DEFAULT_DISTANCE_RAW_MAX base_valid = ( raw not in CORNER_OBJECT_430_EMPTY_RAW_VALUES and distance_raw not in (0, 8000, 8191) and not default_distance and 0.2 < d_rel < CORNER_OBJECT_430_MAX_DREL ) candidate_valid = ( base_valid and slot_index + 1 not in CORNER_OBJECT_430_CANDIDATE_EXCLUDED_SLOTS and code[2] in CORNER_OBJECT_430_CANDIDATE_META_BYTE_3 and code[1] in CORNER_OBJECT_430_STRONG_META_BYTE_2 + CORNER_OBJECT_430_WEAK_META_BYTE_2 ) bins.append({ "msg_index": msg_index, "slot_index": slot_index, "distance_raw": distance_raw, "d_rel": d_rel, "code": code, "candidate_valid": candidate_valid, }) supported_bins = [] candidates = [b for b in bins if b["candidate_valid"]] for b in candidates: support = 1 for other in candidates: if other is b: continue if abs(other["msg_index"] - b["msg_index"]) > 1: continue if abs(other["slot_index"] - b["slot_index"]) > 2: continue if abs(other["distance_raw"] - b["distance_raw"]) > CORNER_OBJECT_430_CANDIDATE_RAW_DELTA: continue support += 1 min_support = (CORNER_OBJECT_430_STRONG_MIN_SUPPORT if b["code"][1] in CORNER_OBJECT_430_STRONG_META_BYTE_2 else CORNER_OBJECT_430_WEAK_MIN_SUPPORT) if support >= min_support: supported_bins.append({**b, "support": support}) clusters = [] for b in sorted(supported_bins, key=lambda item: item["distance_raw"]): if not clusters or b["distance_raw"] - clusters[-1][-1]["distance_raw"] > CORNER_OBJECT_430_CLUSTER_RAW_GAP: clusters.append([b]) else: clusters[-1].append(b) clusters = sorted(clusters, key=lambda cluster: sum(b["distance_raw"] for b in cluster) / len(cluster))[:CORNER_OBJECT_430_MAX_TRACKS_PER_SIDE] cluster_objects = [] for cluster in clusters: msg_index = sum(b["msg_index"] for b in cluster) / len(cluster) slot = sum(b["slot_index"] + 1 for b in cluster) / len(cluster) lateral_cell = (CORNER_OBJECT_430_LATERAL_CELL_MSG_WEIGHT * msg_index + CORNER_OBJECT_430_LATERAL_CELL_SLOT_WEIGHT * slot) mapped_cell = lateral_cell if side_sign > 0.0 else CORNER_OBJECT_430_RIGHT_CELL_MIRROR - lateral_cell y_abs = max(CORNER_OBJECT_430_MIN_ABS_YREL, min(CORNER_OBJECT_430_MAX_ABS_YREL, CORNER_OBJECT_430_YREL_OFFSET - CORNER_OBJECT_430_YREL_SCALE * mapped_cell)) cluster_objects.append({ "d_rel": sum(b["d_rel"] for b in cluster) / len(cluster), "y_rel": side_sign * y_abs, "code": max((b["code"] for b in cluster), key=lambda code: sum(1 for item in cluster if item["code"] == code)), }) active_t_ids = set() side_track_ids = [ CORNER_OBJECT_430_TRACK_ID_OFFSET + track_base + slot for slot in range(CORNER_OBJECT_430_MAX_TRACKS_PER_SIDE) ] unmatched_track_ids = {t_id for t_id in side_track_ids if t_id in self.corner_object_430_prev_d_rel} unused_track_ids = [t_id for t_id in side_track_ids if t_id not in unmatched_track_ids] for cluster in cluster_objects: d_rel = cluster["d_rel"] code = cluster["code"] matched_t_id = None if unmatched_track_ids: nearest_t_id = min(unmatched_track_ids, key=lambda t_id: abs(d_rel - self.corner_object_430_prev_d_rel[t_id])) if abs(d_rel - self.corner_object_430_prev_d_rel[nearest_t_id]) <= CORNER_OBJECT_430_TRACK_MATCH_MAX_DREL_DELTA: matched_t_id = nearest_t_id unmatched_track_ids.remove(matched_t_id) if matched_t_id is None and unused_track_ids: matched_t_id = unused_track_ids.pop(0) if matched_t_id is None: continue t_id = matched_t_id active_t_ids.add(t_id) prev_d_rel = self.corner_object_430_prev_d_rel.get(t_id) prev_code = self.corner_object_430_prev_code.get(t_id) self.corner_object_430_prev_d_rel[t_id] = d_rel self.corner_object_430_prev_y_rel[t_id] = cluster["y_rel"] self.corner_object_430_prev_code[t_id] = code reset_track = prev_d_rel is None or code != prev_code or abs(d_rel - prev_d_rel) > CORNER_OBJECT_430_MAX_DREL_DELTA if reset_track: self.corner_object_430_prev_v_rel.pop(t_id, None) self.corner_object_430_prev_yv_rel.pop(t_id, None) self.corner_object_430_history.pop(t_id, None) self.corner_object_430_noncenter_inward_frames.pop(t_id, None) history = self.corner_object_430_history.setdefault(t_id, deque(maxlen=CORNER_OBJECT_430_HISTORY_SIZE)) history.append((d_rel, cluster["y_rel"])) if len(history) < CORNER_OBJECT_430_MIN_HISTORY: self._clear_point(t_id) continue window_dt = CORNER_OBJECT_430_DT * (len(history) - 1) first_d_rel, first_y_rel = history[0] hist_v_rel = (d_rel - first_d_rel) / window_dt if abs(hist_v_rel) > CORNER_OBJECT_430_MAX_ABS_VREL: self.corner_object_430_prev_v_rel.pop(t_id, None) self.corner_object_430_prev_yv_rel.pop(t_id, None) self.corner_object_430_history.pop(t_id, None) self.corner_object_430_noncenter_inward_frames.pop(t_id, None) self._clear_point(t_id) continue prev_v_rel = self.corner_object_430_prev_v_rel.get(t_id, hist_v_rel) v_rel = (1.0 - CORNER_OBJECT_430_VREL_ALPHA) * prev_v_rel + CORNER_OBJECT_430_VREL_ALPHA * hist_v_rel self.corner_object_430_prev_v_rel[t_id] = v_rel inward_steps = 0 usable_steps = 0 prev_abs_y = abs(history[0][1]) for _, y_rel in list(history)[1:]: abs_y = abs(y_rel) delta = prev_abs_y - abs_y if abs(delta) > 1e-3: usable_steps += 1 if delta > 0.0: inward_steps += 1 prev_abs_y = abs_y net_inward_y = abs(first_y_rel) - abs(cluster["y_rel"]) inward_ratio = inward_steps / usable_steps if usable_steps > 0 else 0.0 hist_yv_rel = (cluster["y_rel"] - first_y_rel) / window_dt recent_inward_y = abs(history[-3][1]) - abs(cluster["y_rel"]) if len(history) >= 3 else net_inward_y if (net_inward_y < CORNER_OBJECT_430_MIN_INWARD_YREL_DELTA or recent_inward_y < CORNER_OBJECT_430_MIN_RECENT_INWARD_YREL_DELTA or inward_ratio < CORNER_OBJECT_430_MIN_INWARD_RATIO or abs(hist_yv_rel) > CORNER_OBJECT_430_MAX_ABS_YVREL): hist_yv_rel = 0.0 inward_motion_candidate = hist_yv_rel != 0.0 and abs(cluster["y_rel"]) <= CORNER_OBJECT_430_INWARD_KEEP_YVREL_ABS_YREL inward_center_candidate = inward_motion_candidate and abs(cluster["y_rel"]) <= CORNER_OBJECT_430_INWARD_CENTER_ABS_YREL y_rel = cluster["y_rel"] if inward_motion_candidate: if inward_center_candidate: self.corner_object_430_noncenter_inward_frames[t_id] = 0 prev_yv_rel = self.corner_object_430_prev_yv_rel.get(t_id, hist_yv_rel) yv_rel = (1.0 - CORNER_OBJECT_430_YVREL_ALPHA) * prev_yv_rel + CORNER_OBJECT_430_YVREL_ALPHA * hist_yv_rel else: noncenter_frames = self.corner_object_430_noncenter_inward_frames.get(t_id, 0) + 1 self.corner_object_430_noncenter_inward_frames[t_id] = noncenter_frames if noncenter_frames <= CORNER_OBJECT_430_EARLY_INWARD_NONCENTER_FRAMES: prev_yv_rel = self.corner_object_430_prev_yv_rel.get(t_id, hist_yv_rel) yv_rel = (1.0 - CORNER_OBJECT_430_YVREL_ALPHA) * prev_yv_rel + CORNER_OBJECT_430_YVREL_ALPHA * hist_yv_rel else: yv_rel = 0.0 if not inward_center_candidate and abs(y_rel) < CORNER_OBJECT_430_SIDE_KEEP_ABS_YREL: y_rel = math.copysign(CORNER_OBJECT_430_SIDE_KEEP_ABS_YREL, y_rel) else: hist_yv_rel = 0.0 yv_rel = 0.0 self.corner_object_430_noncenter_inward_frames[t_id] = 0 if abs(y_rel) < CORNER_OBJECT_430_SIDE_KEEP_ABS_YREL: y_rel = math.copysign(CORNER_OBJECT_430_SIDE_KEEP_ABS_YREL, y_rel) self.corner_object_430_prev_yv_rel[t_id] = yv_rel self.pts[t_id].measured = True self.pts[t_id].trackId = t_id self.pts[t_id].dRel = d_rel self.pts[t_id].yRel = y_rel self.pts[t_id].vRel = v_rel self.pts[t_id].vLead = v_rel + self.v_ego self.pts[t_id].aRel = float('nan') self.pts[t_id].yvRel = yv_rel side_track_count = CORNER_OBJECT_430_MSG_COUNT_PER_SIDE * CORNER_OBJECT_430_SLOTS_PER_MSG for slot in range(side_track_count): t_id = CORNER_OBJECT_430_TRACK_ID_OFFSET + track_base + slot if t_id in active_t_ids: continue self.corner_object_430_prev_d_rel.pop(t_id, None) self.corner_object_430_prev_v_rel.pop(t_id, None) self.corner_object_430_prev_y_rel.pop(t_id, None) self.corner_object_430_prev_yv_rel.pop(t_id, None) self.corner_object_430_prev_code.pop(t_id, None) self.corner_object_430_history.pop(t_id, None) self.corner_object_430_noncenter_inward_frames.pop(t_id, None) self._clear_point(t_id) def _clear_point(self, t_id): self.pts[t_id].measured = False self.pts[t_id].dRel = 0 self.pts[t_id].yRel = 0 self.pts[t_id].vRel = 0 self.pts[t_id].vLead = self.v_ego self.pts[t_id].aRel = float('nan') self.pts[t_id].yvRel = 0 def _clear_corner_objects(self): for slot in range(CORNER_OBJECT_235_MSG_COUNT): self._clear_point(CORNER_OBJECT_235_TRACK_ID_OFFSET + slot) self.corner_object_track_ids.clear_source("corner235") def _clear_corner_objects_180(self): for slot in range(CORNER_OBJECT_180_MSG_COUNT * CORNER_OBJECT_180_SLOTS_PER_MSG): self._clear_point(CORNER_OBJECT_180_TRACK_ID_OFFSET + slot) self.corner_object_track_ids.clear_source("corner180") def _clear_corner_objects_430(self): self.corner_object_430_prev_d_rel.clear() self.corner_object_430_prev_v_rel.clear() self.corner_object_430_prev_y_rel.clear() self.corner_object_430_prev_yv_rel.clear() self.corner_object_430_prev_code.clear() self.corner_object_430_history.clear() self.corner_object_430_noncenter_inward_frames.clear() for slot in range(CORNER_OBJECT_430_MSG_COUNT_PER_SIDE * 2 * CORNER_OBJECT_430_SLOTS_PER_MSG): self._clear_point(CORNER_OBJECT_430_TRACK_ID_OFFSET + slot) def _update_scc(self, updated_messages): cpt = self.rcp_scc.vl t_id = SCC_TID if self.canfd: dRel = cpt["SCC_CONTROL"]['ACC_ObjDist'] vRel = cpt["SCC_CONTROL"]['ACC_ObjRelSpd'] new_pts = abs(dRel - self.dRel_last) > 3 or abs(vRel - self.vRel_last) > 1 vLead = vRel + self.v_ego valid = 0 < dRel < 150 and not new_pts #cpt["SCC_CONTROL"]['OBJ_STATUS'] and dRel < 150 self.pts[t_id].measured = bool(valid) if not valid: self.pts[t_id].dRel = 0 self.pts[t_id].yRel = 0 self.pts[t_id].vRel = 0 self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego self.pts[t_id].aRel = float('nan') self.pts[t_id].yvRel = 0 else: self.pts[t_id].dRel = dRel self.pts[t_id].yRel = 0 self.pts[t_id].vRel = vRel self.pts[t_id].vLead = vLead self.pts[t_id].aRel = float('nan') self.pts[t_id].yvRel = 0 #float('nan') else: dRel = cpt["SCC11"]['ACC_ObjDist'] vRel = cpt["SCC11"]['ACC_ObjRelSpd'] new_pts = abs(dRel - self.dRel_last) > 3 or abs(vRel - self.vRel_last) > 1 vLead = vRel + self.v_ego valid = cpt["SCC11"]['ACC_ObjStatus'] and dRel < 150 and not new_pts self.pts[t_id].measured = bool(valid) if not valid: self.pts[t_id].dRel = 0 self.pts[t_id].yRel = 0 self.pts[t_id].vRel = 0 self.pts[t_id].vLead = self.pts[t_id].vRel + self.v_ego self.pts[t_id].aRel = float('nan') self.pts[t_id].yvRel = 0 else: self.pts[t_id].dRel = dRel self.pts[t_id].yRel = -cpt["SCC11"]['ACC_ObjLatPos'] # in car frame's y axis, left is negative self.pts[t_id].vRel = vRel self.pts[t_id].vLead = vLead self.pts[t_id].aRel = float('nan') self.pts[t_id].yvRel = 0 #float('nan') self.dRel_last = dRel self.vRel_last = vRel