#!/usr/bin/env python3 import fcntl import json import math import os import socket import struct import threading import time import traceback import zmq from datetime import datetime import cereal.messaging as messaging from openpilot.common.realtime import Ratekeeper from openpilot.common.params import Params from openpilot.system.hardware import PC, TICI try: from selfdrive.car.car_helpers import interfaces HAS_CAR_INTERFACES = True except ImportError: HAS_CAR_INTERFACES = False interfaces = None class CommaAssist: def __init__(self): print("初始化 CommaAssist 服务...") # 初始化参数 self.params = Params() # 订阅需要的数据源 self.sm = messaging.SubMaster(['deviceState', 'carState', 'controlsState', 'longitudinalPlan', 'liveLocationKalman', 'navInstruction', 'modelV2']) # 网络相关配置 self.broadcast_ip = self.get_broadcast_address() if self.broadcast_ip is None: self.broadcast_ip = "255.255.255.255" # 使用通用广播地址作为备选 self.broadcast_port = 8088 self.ip_address = "0.0.0.0" self.is_running = True # 获取车辆信息 self.car_info = {} self.load_car_info() # 启动广播线程 threading.Thread(target=self.broadcast_data).start() def load_car_info(self): """加载车辆基本信息""" try: # 获取车辆型号信息 try: car_model = self.params.get("CarModel", encoding='utf8') self.car_info["car_name"] = car_model if car_model else "Unknown" except Exception as e: print(f"无法获取CarModel参数: {e}") try: # 尝试获取其他可能的车辆参数 car_params = self.params.get("CarParamsCache", encoding='utf8') self.car_info["car_name"] = "通过CarParamsCache获取" if car_params else "Unknown" except Exception: self.car_info["car_name"] = "Unknown Model" car_model = None # 检查车辆接口是否可用 if not HAS_CAR_INTERFACES: print("车辆接口模块不可用") elif not car_model: print("无有效的车型信息") elif not isinstance(interfaces, list): print("车辆接口不是列表类型，尝试转换...") # 尝试获取车辆接口的具体实现 if hasattr(interfaces, '__call__'): # 如果interfaces是一个函数，尝试直接获取车辆指纹 try: self.car_info["car_fingerprint"] = f"直接从车型{car_model}获取" print(f"直接从车型识别: {car_model}") except Exception as e: print(f"无法从车型直接获取指纹: {e}") else: # 正常遍历接口列表 print("尝试从车辆接口中获取指纹信息...") for interface in interfaces: if not hasattr(interface, 'CHECKSUM'): continue try: if isinstance(interface.CHECKSUM, dict) and 'pt' in interface.CHECKSUM: if car_model in interface.CHECKSUM["pt"]: platform = interface self.car_info["car_fingerprint"] = platform.config.platform_str # 获取车辆规格参数 specs = platform.config.specs if specs: if hasattr(specs, 'mass'): self.car_info["mass"] = specs.mass if hasattr(specs, 'wheelbase'): self.car_info["wheelbase"] = specs.wheelbase if hasattr(specs, 'steerRatio'): self.car_info["steerRatio"] = specs.steerRatio break except Exception as e: print(f"处理特定车辆接口异常: {e}") except Exception as e: print(f"加载车辆信息失败: {e}") traceback.print_exc() # 确保基本字段存在，避免后续访问出错 if "car_name" not in self.car_info: self.car_info["car_name"] = "Unknown Model" if "car_fingerprint" not in self.car_info: self.car_info["car_fingerprint"] = "Unknown Fingerprint" print(f"车辆信息加载完成: {self.car_info}") def get_broadcast_address(self): """获取广播地址""" try: if PC: iface = b'br0' else: iface = b'wlan0' with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as s: ip = fcntl.ioctl( s.fileno(), 0x8919, # SIOCGIFADDR struct.pack('256s', iface) )[20:24] ip_str = socket.inet_ntoa(ip) print(f"获取到IP地址: {ip_str}") # 从IP地址构造广播地址 ip_parts = ip_str.split('.') return f"{ip_parts[0]}.{ip_parts[1]}.{ip_parts[2]}.255" except (OSError, Exception) as e: print(f"获取广播地址失败: {e}") return None def get_local_ip(self): """获取本地IP地址""" try: with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as s: s.connect(("8.8.8.8", 80)) return s.getsockname()[0] except Exception as e: print(f"获取本地IP失败: {e}") return "127.0.0.1" def make_data_message(self): """构建广播消息内容""" # 基本消息结构 message = { "timestamp": int(time.time()), "device": { "ip": self.ip_address, "battery": {}, "mem_usage": 0, "cpu_temp": 0, "free_space": 0 }, "car": { "speed": 0, "cruise_speed": 0, "gear_shifter": "unknown", "steering_angle": 0, "steering_torque": 0, "brake_pressed": False, "gas_pressed": False, "door_open": False, "left_blinker": False, "right_blinker": False }, "location": { "latitude": 0, "longitude": 0, "bearing": 0, "speed": 0, "altitude": 0, "accuracy": 0, "gps_valid": False }, "car_info": { "basic": { "car_model": self.car_info.get("car_name", "Unknown"), "fingerprint": self.car_info.get("car_fingerprint", "Unknown"), "weight": f"{self.car_info.get('mass', 0):.0f} kg" if 'mass' in self.car_info else "Unknown", "wheelbase": f"{self.car_info.get('wheelbase', 0):.3f} m" if 'wheelbase' in self.car_info else "Unknown", "steering_ratio": f"{self.car_info.get('steerRatio', 0):.1f}" if 'steerRatio' in self.car_info else "Unknown" } } } # 安全地获取设备信息 try: if self.sm.updated['deviceState'] and self.sm.valid['deviceState']: device_state = self.sm['deviceState'] # 获取设备信息 - 使用getattr安全地访问属性 message["device"]["mem_usage"] = getattr(device_state, 'memoryUsagePercent', 0) message["device"]["free_space"] = getattr(device_state, 'freeSpacePercent', 0) # CPU温度 cpu_temps = getattr(device_state, 'cpuTempC', []) if isinstance(cpu_temps, list) and len(cpu_temps) > 0: message["device"]["cpu_temp"] = cpu_temps[0] # 电池信息 try: # 额外的错误处理，因为这是常见错误点 message["device"]["battery"]["percent"] = getattr(device_state, 'batteryPercent', 0) message["device"]["battery"]["status"] = getattr(device_state, 'batteryCurrent', 0) message["device"]["battery"]["voltage"] = getattr(device_state, 'batteryVoltage', 0) message["device"]["battery"]["charging"] = getattr(device_state, 'chargingError', False) except Exception as e: print(f"获取电池信息失败: {e}") except Exception as e: print(f"获取设备状态出错: {e}") # 安全地获取车辆信息 try: if self.sm.updated['carState'] and self.sm.valid['carState']: CS = self.sm['carState'] # 基本车辆信息 message["car"]["speed"] = getattr(CS, 'vEgo', 0) * 3.6 # m/s转km/h message["car"]["gear_shifter"] = str(getattr(CS, 'gearShifter', "unknown")) message["car"]["steering_angle"] = getattr(CS, 'steeringAngleDeg', 0) message["car"]["steering_torque"] = getattr(CS, 'steeringTorque', 0) message["car"]["brake_pressed"] = getattr(CS, 'brakePressed', False) message["car"]["gas_pressed"] = getattr(CS, 'gasPressed', False) message["car"]["door_open"] = getattr(CS, 'doorOpen', False) message["car"]["left_blinker"] = getattr(CS, 'leftBlinker', False) message["car"]["right_blinker"] = getattr(CS, 'rightBlinker', False) # 扩展的车辆状态信息 is_car_started = getattr(CS, 'vEgo', 0) > 0.1 is_car_engaged = False # 详细车辆信息 car_details = {} # 车辆状态 status = { "running_status": "Moving" if is_car_started else "Stopped", "door_open": getattr(CS, 'doorOpen', False), "seatbelt_unlatched": getattr(CS, 'seatbeltUnlatched', False), } # 引擎信息 engine_info = {} if hasattr(CS, 'engineRpm') and CS.engineRpm > 0: engine_info["rpm"] = f"{CS.engineRpm:.0f}" car_details["engine"] = engine_info # 巡航控制 cruise_info = {} if hasattr(CS, 'cruiseState'): is_car_engaged = getattr(CS.cruiseState, 'enabled', False) cruise_info["enabled"] = getattr(CS.cruiseState, 'enabled', False) cruise_info["available"] = getattr(CS.cruiseState, 'available', False) cruise_info["speed"] = getattr(CS.cruiseState, 'speed', 0) * 3.6 if hasattr(CS, 'pcmCruiseGap'): cruise_info["gap"] = CS.pcmCruiseGap status["cruise_engaged"] = is_car_engaged car_details["cruise"] = cruise_info # 车轮速度 wheel_speeds = {} if hasattr(CS, 'wheelSpeeds'): ws = CS.wheelSpeeds wheel_speeds["fl"] = getattr(ws, 'fl', 0) * 3.6 wheel_speeds["fr"] = getattr(ws, 'fr', 0) * 3.6 wheel_speeds["rl"] = getattr(ws, 'rl', 0) * 3.6 wheel_speeds["rr"] = getattr(ws, 'rr', 0) * 3.6 car_details["wheel_speeds"] = wheel_speeds # 方向盘信息 steering = { "angle": getattr(CS, 'steeringAngleDeg', 0), "torque": getattr(CS, 'steeringTorque', 0), } if hasattr(CS, 'steeringRateDeg'): steering["rate"] = CS.steeringRateDeg car_details["steering"] = steering # 踏板状态 pedals = { "gas_pressed": getattr(CS, 'gasPressed', False), "brake_pressed": getattr(CS, 'brakePressed', False), } if hasattr(CS, 'gas'): pedals["throttle_position"] = CS.gas * 100 if hasattr(CS, 'brake'): pedals["brake_pressure"] = CS.brake * 100 car_details["pedals"] = pedals # 安全系统 safety_systems = {} if hasattr(CS, 'espDisabled'): safety_systems["esp_disabled"] = CS.espDisabled if hasattr(CS, 'absActive'): safety_systems["abs_active"] = CS.absActive if hasattr(CS, 'tcsActive'): safety_systems["tcs_active"] = CS.tcsActive if hasattr(CS, 'collisionWarning'): safety_systems["collision_warning"] = CS.collisionWarning car_details["safety_systems"] = safety_systems # 车门状态 doors = { "driver": getattr(CS, 'doorOpen', False) } if hasattr(CS, 'passengerDoorOpen'): doors["passenger"] = CS.passengerDoorOpen if hasattr(CS, 'trunkOpen'): doors["trunk"] = CS.trunkOpen if hasattr(CS, 'hoodOpen'): doors["hood"] = CS.hoodOpen car_details["doors"] = doors # 灯光状态 lights = { "left_blinker": getattr(CS, 'leftBlinker', False), "right_blinker": getattr(CS, 'rightBlinker', False), } if hasattr(CS, 'genericToggle'): lights["high_beam"] = CS.genericToggle if hasattr(CS, 'lowBeamOn'): lights["low_beam"] = CS.lowBeamOn car_details["lights"] = lights # 盲点监测 blind_spot = {} if hasattr(CS, 'leftBlindspot'): blind_spot["left"] = CS.leftBlindspot if hasattr(CS, 'rightBlindspot'): blind_spot["right"] = CS.rightBlindspot if blind_spot: car_details["blind_spot"] = blind_spot # 其他可选信息 other_info = {} if hasattr(CS, 'outsideTemp'): other_info["outside_temp"] = CS.outsideTemp if hasattr(CS, 'fuelGauge'): other_info["fuel_range"] = CS.fuelGauge if hasattr(CS, 'odometer'): other_info["odometer"] = CS.odometer if hasattr(CS, 'instantFuelConsumption'): other_info["fuel_consumption"] = CS.instantFuelConsumption if other_info: car_details["other"] = other_info # 更新状态和详细信息 message["car_info"]["status"] = status message["car_info"]["details"] = car_details if self.sm.updated['controlsState'] and self.sm.valid['controlsState']: controls_state = self.sm['controlsState'] message["car"]["cruise_speed"] = getattr(controls_state, 'vCruise', 0) # 额外的控制状态信息 controls_info = {} if hasattr(controls_state, 'enabled'): controls_info["enabled"] = controls_state.enabled if hasattr(controls_state, 'active'): controls_info["active"] = controls_state.active if hasattr(controls_state, 'alertText1'): controls_info["alert_text"] = controls_state.alertText1 if controls_info: message["car_info"]["controls"] = controls_info except Exception as e: print(f"获取车辆信息出错: {e}") traceback.print_exc() # 安全地获取GPS位置信息 try: if self.sm.updated['liveLocationKalman'] and self.sm.valid['liveLocationKalman']: location = self.sm['liveLocationKalman'] # 检查GPS是否有效 location_status = getattr(location, 'status', -1) position_valid = False if hasattr(location, 'positionGeodetic'): position_valid = getattr(location.positionGeodetic, 'valid', False) gps_valid = (location_status == 0) and position_valid message["location"]["gps_valid"] = gps_valid if gps_valid and hasattr(location, 'positionGeodetic') and hasattr(location.positionGeodetic, 'value'): # 获取位置信息 pos_value = location.positionGeodetic.value if len(pos_value) >= 3: message["location"]["latitude"] = pos_value[0] message["location"]["longitude"] = pos_value[1] message["location"]["altitude"] = pos_value[2] # 获取精度信息 if hasattr(location, 'positionGeodeticStd') and hasattr(location.positionGeodeticStd, 'value'): std_value = location.positionGeodeticStd.value if len(std_value) > 0: message["location"]["accuracy"] = std_value[0] # 获取方向信息 if hasattr(location, 'calibratedOrientationNED') and hasattr(location.calibratedOrientationNED, 'value'): orientation = location.calibratedOrientationNED.value if len(orientation) > 2: message["location"]["bearing"] = math.degrees(orientation[2]) # 设置速度信息 car_state = self.sm['carState'] if self.sm.valid['carState'] else None if car_state and hasattr(car_state, 'vEgo'): message["location"]["speed"] = car_state.vEgo * 3.6 except Exception as e: print(f"获取位置信息出错: {e}") # 如果有导航指令，添加导航信息 try: if self.sm.valid['navInstruction']: nav_instruction = self.sm['navInstruction'] nav_info = {} nav_info["distance_remaining"] = getattr(nav_instruction, 'distanceRemaining', 0) nav_info["time_remaining"] = getattr(nav_instruction, 'timeRemaining', 0) nav_info["speed_limit"] = getattr(nav_instruction, 'speedLimit', 0) * 3.6 nav_info["maneuver_distance"] = getattr(nav_instruction, 'maneuverDistance', 0) nav_info["maneuver_type"] = getattr(nav_instruction, 'maneuverType', "") nav_info["maneuver_modifier"] = getattr(nav_instruction, 'maneuverModifier', "") nav_info["maneuver_text"] = getattr(nav_instruction, 'maneuverPrimaryText', "") message["navigation"] = nav_info except Exception as e: print(f"获取导航信息出错: {e}") try: return json.dumps(message) except Exception as e: print(f"序列化消息出错: {e}") return "{}" def broadcast_data(self): """定期发送数据到广播地址""" sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) rk = Ratekeeper(10, print_delay_threshold=None) # 10Hz广播频率 print(f"开始广播数据到 {self.broadcast_ip}:{self.broadcast_port}") while self.is_running: try: # 更新数据 self.sm.update(0) # 更新IP地址 ip_address = self.get_local_ip() if ip_address != self.ip_address: self.ip_address = ip_address print(f"IP地址已更新: {ip_address}") # 构建并发送消息 msg = self.make_data_message() dat = msg.encode('utf-8') sock.sendto(dat, (self.broadcast_ip, self.broadcast_port)) # 减少日志输出频率 if rk.frame % 50 == 0: # 每5秒打印一次日志 print(f"广播数据: {self.broadcast_ip}:{self.broadcast_port}") rk.keep_time() except Exception as e: print(f"广播数据错误: {e}") traceback.print_exc() time.sleep(1) def main(gctx=None): """主函数 支持作为独立程序运行或由process_config启动 gctx参数用于与openpilot进程管理系统兼容 """ comma_assist = CommaAssist() # 保持主线程运行 try: while True: time.sleep(10) # 主线程休眠 except KeyboardInterrupt: comma_assist.is_running = False print("CommaAssist服务已停止") if __name__ == "__main__": main()