zeaslity/WddSuperAgent
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

初始化项目

Browse Source
This commit is contained in:
zeaslity
2025-03-27 16:09:20 +08:00
parent e09a32d1e8
commit fc2d585489
709 changed files with 516391 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

423
agent-common/SplitProject/ranjing-python-devfusion/utils.py Normal file
View File

@@ -0,0 +1,423 @@
import datetime
from transformation import *
import json
import numpy as np
class Pipeline:
def __init__(self, fusion_parameters_topic,reference_point):
self.fusion_parameters_topic = fusion_parameters_topic
self.task_id = '554343465692430336'
self.reference_point = reference_point
# self.deviceId = deviceId
self.sensor_id_list = ["10000000000"]
self.fusionCode = 'DPZYLY'
self.publish_topic = f"bridge/{self.fusionCode}/device_data/fusion/{self.task_id}"
self.priority_dict = {"10000000000":1}
self.uavInfo_bucket = dict()
self.target_bowl = dict()
self.device_info_dict = dict()
self.device_type_mapping = {
"5ga": 0,
"radar": 1,
"spec": 2,
"oe": 3,
"cm": 4,
"dec": 5,
"ifr": 6,
"cv": 7,
"isrs": 8,
"aoa": 9,
"tdoa": 10,
"dcd": 11,
"direct": 100,
"rtk": 101,
"rid": 102,
"fusion": 1000,
"other": 999 # 假设 'other' 对应于未知设备类型
}
self.device_type_speedrank = {
"radar": 1,
"spec": 2,
"oe": 3,
"cm": 4,
"dec": 5,
"ifr": 6,
"cv": 7,
"isrs": 8,
"aoa": 9,
"tdoa": 10,
"dcd": 13,
"direct": 12,
"5ga": 11,
"rid": 14,
"rtk": 15,
"other": 0 # 假设 'other' 对应于未知设备类型
}
import json
def process_json_data(self, json_data):
"""
将 JSON 数据转换为字典，并添加 X 和 Y 属性。
"""
data_dict = json.loads(json_data)
# 安全访问 'ptTime' 键
pt_time = data_dict.get('ptTime')
if pt_time is not None:
print(pt_time)
else:
print("Key 'ptTime' not found in data_dict.")
# 安全访问 'objects' 键
objects = data_dict.get('objects')
if objects is None:
print("Key 'objects' not found in data_dict.")
return data_dict # 如果 'objects' 键不存在，直接返回原始字典或根据需要进行其他处理
# 如果 'objects' 键存在，继续处理
for record in objects:
# 检查 'latitude' 和 'longitude' 键是否存在于 record 中
if 'latitude' in record and 'longitude' in record:
lat = record['latitude']
lon = record['longitude']
X, Y = convert_to_cartesian(lat, lon, self.reference_point)
record['X'] = X
record['Y'] = Y
else:
print("Record is missing 'latitude' or 'longitude' keys.")
return data_dict
def data_encoder(self, data_list):
"""
生成数据矩阵和 ID 列表。
"""
sensor_data = []
for sensor_id in self.sensor_id_list:
temp = {'data_matrix': [],
'id_list': [],
'deviceId': sensor_id,
'latest_time': [],
'priority':1}
for record in data_list:
if record.get('noteData'):
obj = record['noteData']
obj['objectId'] = obj['uasId']
obj['deviceId'] = obj["extension"]['deviceId']
record['objects'] = [obj]
if record['deviceId'] == sensor_id:
temp['priority'] = self.priority_dict[sensor_id]
if record.get('objects'):
for obj in record['objects']:
if obj['objectId'] in temp['id_list']:
position = temp['id_list'].index(obj['objectId'])
if int(record['ptTime']) > int(temp['latest_time'][position]):
temp['data_matrix'][position] = [obj['X'], obj['Y'], obj['altitude']]
else:
temp['data_matrix'].append([obj['X'], obj['Y'], obj['altitude']])
temp['id_list'].append(obj['objectId'])
temp['latest_time'].append(record['ptTime'])
# 把扩展地段写入
if obj.get('extension'):
B_id = str(record['deviceId'])+str(obj['objectId'])
self.uavInfo_bucket[B_id] = obj['extension']
# 如果对象有speed字段，将其添加到extension中
if obj.get('speed'):
self.uavInfo_bucket[B_id]['speed'] = obj['speed']
# 如果对象有height字段，也存储它
if obj.get('height'):
self.uavInfo_bucket[B_id]['height'] = obj['height']
# 写入到数据字典中
temp['data_matrix'] = np.array(temp['data_matrix'])
sensor_data.append(temp)
return sensor_data
def process_extension(self, target):
# 定义一个字典，包含给定的键值对
extension = {
"objectType": 30,
"uavSN": "Un-known",
"uavModel": "Un-known",
"pilotLat": 0.0,
"pilotLon": 0.0,
"speedX": 0.0,
"speedY": 0.0,
"speedZ": 0.0,
"time": 0.0,
"born_time": 0.0
}
# 从target_bowl获取历史值
if target['objectId'] in self.target_bowl.keys():
extension = self.target_bowl[target['objectId']]
result_source = target['source']
# 对数据进行更新
for source in result_source:
id = str(source[0]) + str(source[1])
if self.uavInfo_bucket.get(id):
for key, value in self.uavInfo_bucket[id].items():
# 只有当新值是有效值时才更新
if value not in ["Un-known", 0.0, None, "Unknown", "DJI Mavic"]:
extension[key] = value
extension['born_time'] = int(target['born_time'])
# 更新target_bowl以保持状态
self.target_bowl[target['objectId']] = extension
return extension
def data_decoder(self, filtered_results):
"""
解码过滤后的结果。
"""
current_time = datetime.datetime.now()
timestamp = int(current_time.timestamp() * 1000)
combined_objects = []
for target in filtered_results:
X = target['X']
Y = target['Y']
Z = target['Z'] # 这里的Z实际上是altitude
lat, lon = convert_to_geodetic(X, Y, self.reference_point)
extension = self.process_extension(target)
extension['time'] = int(timestamp)
extension['born_time'] = int(int(target['born_time']) / 1000) # 毫秒单位数据
new_origin_source = []
for source in target['source']:
device_id, object_id = source
# 从 device_info_dict 获取设备缩写
device_abbreviation = self.device_info_dict.get(device_id, {}).get('device_type', 'other')
# 使用映射字典获取设备类型
device_type = self.device_type_mapping.get(device_abbreviation, 999)
new_origin_source.append(f"{device_type}_{device_id}_{object_id}")
# 根据优先级顺序选择速度
highest_priority_speed = None
highest_priority = float('inf')
for source in target['source']:
device_id, object_id = source
B_id = str(device_id) + str(object_id)
if self.uavInfo_bucket.get(B_id):
device_type = self.device_info_dict.get(device_id, {}).get('device_type', 'other')
priority = self.device_type_speedrank.get(device_type, float('inf'))
if priority < highest_priority:
highest_priority = priority
# 获取速度并进行单位转换
speed = self.uavInfo_bucket[B_id].get('speed', target['speed'])
if device_type == "5ga": # 如果设备类型是5ga，进行转换
speed = speed / 3.6 # 从 km/h 转换为 m/s
highest_priority_speed = speed
# 确保 highest_priority_speed 是从设备获取的速度
if highest_priority_speed is None:
# 如果没有找到当前速度，查找历史记录中的速度
for obj in reversed(combined_objects):
if obj["objectId"] == target['objectId']:
highest_priority_speed = obj.get("speed")
break
if highest_priority_speed is None:
print(f"Warning: No speed found for target {target['objectId']}, using default target speed.")
new_speed = target['speed']
else:
new_speed = highest_priority_speed
else:
new_speed = highest_priority_speed
# Debug 输出，检查速度来源
print(f"Selected speed for target {target['objectId']}: {new_speed} from device with priority {highest_priority}")
# 获取height字段
height = None
for source in target['source']:
device_id, object_id = source
B_id = str(device_id) + str(object_id)
if self.uavInfo_bucket.get(B_id):
if self.uavInfo_bucket[B_id].get('height'):
height = self.uavInfo_bucket[B_id]['height']
break
# 如果当前没有获取到height，查找历史记录中的height
if height is None:
for obj in reversed(combined_objects):
if obj["objectId"] == target['objectId']:
prev_height = obj.get("height")
if prev_height is not None: # 如果找到有效的历史height
height = prev_height
break
# 如果仍然没有找到height，保持上一次的最新历史height
if height is None and combined_objects:
for obj in reversed(combined_objects):
if obj["objectId"] == target['objectId']:
height = obj.get("height")
break
temp = {
# "msg_cnt":result['msg_cnt'],#增加msg_cnt用于检测有无丢包
"objectId": target['objectId'],
"X": X,
"Y": Y,
"height": height, # 使用当前height或历史height
"altitude": Z,
"speed": new_speed, # 使用优先级最高的速度
'latitude': lat,
'longitude': lon,
'sigma': target['sigma'],
"extension": {
"origin_source": new_origin_source, # 更新后的 origin_source
# 其他extension字段...
"objectType": extension.get('objectType', 0),
"uavSN": extension.get("uavSN", "Un-known"),
"uavModel": extension.get("uavModel", "Un-known"),
"pilotLat": extension.get("pilotLat", 0.0),
"pilotLon": extension.get("pilotLon", 0.0),
"speedX": 0.0, # 不再使用速度分量
"speedY": 0.0,
"speedZ": 0.0,
"time": int(timestamp),
"born_time": int(int(target['born_time']) / 1000),
},
"time": int(timestamp),
}
# 检查extension中的objectType是否已经被设置为非0值，如果是，则不再覆盖.
if extension.get('objectType', 0) != 0 or target['objectId'] not in [obj['objectId'] for obj in
combined_objects]:
temp["extension"]["objectType"] = extension.get('objectType', 0)
else:
# 查找combined_objects中相同objectId的objectType，如果不存在则使用0
existing_object_types = [obj["extension"].get('objectType', 0) for obj in combined_objects if
obj["objectId"] == target['objectId']]
if existing_object_types and existing_object_types[0] != 0:
temp["extension"]["objectType"] = existing_object_types[0]
else:
temp["extension"]["objectType"] = 0
# 检查并更新uavSN和uavModel
invalid_values = ["Un-known", 0.0, None, "Unknown", "DJI Mavic"]
# 检查uavSN是否为字母数字组合防止其他部分引入奇怪的值
current_sn = extension.get('uavSN', "Un-known")
if isinstance(current_sn, str):
has_letter = any(c.isalpha() for c in current_sn)
has_digit = any(c.isdigit() for c in current_sn)
if not (has_letter and has_digit):
# 先查找相同objectId的历史有效SN
for obj in reversed(combined_objects):
if obj["objectId"] == target['objectId']:
prev_sn = obj["extension"].get("uavSN", "Un-known")
if isinstance(prev_sn, str):
has_letter = any(c.isalpha() for c in prev_sn)
has_digit = any(c.isdigit() for c in prev_sn)
if has_letter and has_digit:
current_sn = prev_sn
break
temp["extension"]["uavSN"] = current_sn
temp["extension"]["uavModel"] = extension.get('uavModel', "Un-known")
combined_objects.append(temp)
data_processed = {
"deviceType": 1000,
"providerCode": "DPZYLY",
"deviceId": self.task_id,
"objects": combined_objects,
"ptTime": int(timestamp)
}
# 筛选有意义的数据
if data_processed and data_processed.get("objects") and len(data_processed["objects"]) > 0:
formatted_time = current_time.strftime('%Y-%m-%d %H:%M:%S')
with open('PB_log.txt', 'a') as log_file: # 以追加模式打开日志文件
log_file.write('=====================\n') # 写入分隔符
log_file.write(f"time: {formatted_time}\n") # 写入时间戳
log_file.write(f"data: {data_processed}\n")
return data_processed
def extract_parms(self, parm_data):
"""
提取参数。
"""
id_list = [] # 存储设备ID
priority_dict = {} # 存储设备优先级
device_info_dict = {} # 新增：存储设备详细信息的字典，用于后续拿到与
data_dict = json.loads(parm_data)
print(data_dict)
self.task_id = data_dict['task_id']
new_topics = [("fromcheck/DPZYLY/fly_data/rtk/#", 0)]
devices = data_dict['devices']
for device in devices:
device_id = device['device_id']
if device_id:
id_list.append(device_id)
new_topics.append((device["device_topic"], 0))
# 存储设备优先级，默认优先级为1
if device.get('priority'):
priority_dict[device_id] = device['priority']
else:
priority_dict[device_id] = 1
# 使用列表存储设备的详细信息（topic、type、sampling_rate），完成一对多
device_info_dict[device_id] = {
'device_topic': device['device_topic'],
'device_type': device['device_type'],
'sampling_rate': device['properties'].get('sampling_rate', 1) # 默认为None，如果没有提供
}
self.priority_dict = priority_dict
self.device_info_dict = device_info_dict # 将设备信息字典存储到实例变量中
self.sensor_id_list = id_list
# 处理参考点
if data_dict.get('reference_point'):
try:
original_reference_point = data_dict['reference_point']
if len(original_reference_point) == 2: # 确保是包含两个元素的元组或列表
self.reference_point = (
float(original_reference_point[0]) + 0,
float(original_reference_point[1]) + 0
)
else:
raise ValueError("Invalid reference_point structure. Must be a tuple or list with two elements.")
except Exception as e:
print(f"Error processing reference_point: {e}")
self.reference_point = None # 或者设置为某个默认值
return new_topics
def extract_fusion_parms(self,parm_data):
data_dict = json.loads(parm_data)
# 定义 fusion_dict 字典，包含需要从 data_dict 中提取的键
fusion_dict = {
"fusion_type": 1,
"gate": 1,
"interval": 1,
"show_thres": 0.4
}
# 检查 data_dict 中是否存在对应的键，并更新 fusion_dict 中的值
if "fusion_type" in data_dict:
fusion_dict["fusion_type"] = data_dict["fusion_type"]
if "gate" in data_dict:
fusion_dict["gate"] = data_dict["gate"]
if "interval" in data_dict:
fusion_dict["interval"] = data_dict["interval"]
if "show_thres" in data_dict:
fusion_dict["show_thres"] = data_dict["show_thres"]
# 返回更新后的 fusion_dict
return fusion_dict