498 lines
18 KiB
Python
498 lines
18 KiB
Python
#!/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() |