From 6542142bebf5246e9097fafc75e69227e9fdadbf Mon Sep 17 00:00:00 2001 From: luxiaotao1123 <t1341870251@163.com> Date: 星期二, 24 十二月 2024 15:44:22 +0800 Subject: [PATCH] # --- zy-acs-manager/src/main/resources/agv1.py | 242 ++++++++++++++++++++++++++++++++++++++++++++++++ 1 files changed, 242 insertions(+), 0 deletions(-) diff --git a/zy-acs-manager/src/main/resources/agv1.py b/zy-acs-manager/src/main/resources/agv1.py new file mode 100644 index 0000000..47af53d --- /dev/null +++ b/zy-acs-manager/src/main/resources/agv1.py @@ -0,0 +1,242 @@ +import ast +import sys +import numpy as np +import json +import time +import redis +from collections import deque, defaultdict + +########################## +# 宸ュ叿鍑芥暟 +########################## +def convert_to_float_array(str_array): + """ + 灏嗗瓧绗︿覆鎴栧彲杩唬瀵硅薄杞崲涓烘诞鐐瑰瀷鏁扮粍銆� + """ + if str_array == "-": + return np.array([], dtype=float) + if isinstance(str_array, str): + return np.array(ast.literal_eval(str_array), dtype=float) + elif isinstance(str_array, list) or isinstance(str_array, np.ndarray): + return np.array(str_array, dtype=float) + return np.array([], dtype=float) + +def initWaveMatrix(codeMatrix): + """ + 鏍规嵁 codeMatrix 鍒濆鍖� waveMatrix銆� + 鑻ユ牸瀛愪负 'NONE'锛屽垯 waveMatrix[x][y] = "-" + 鍚﹀垯涓� "[]" + """ + waveMatrix = np.empty_like(codeMatrix, dtype=object) + for x in range(codeMatrix.shape[0]): + for y in range(codeMatrix.shape[1]): + if codeMatrix[x][y] == 'NONE': + waveMatrix[x][y] = "-" + else: + waveMatrix[x][y] = "[]" + return waveMatrix + +def mergeWave(originWave, vehicle): + """ + originWave 鏄� waveMatrix[x][y] 涓瓨鍌ㄧ殑瀛楃涓诧紙鍐呴儴鏄竴涓� JSON 鏁扮粍锛夛紝 + 灏嗘柊杞﹁締 vehicle 鍚堝苟杩涘幓锛堢敤 set 鍘婚噸锛夛紝 + 骞惰繑鍥炴洿鏂板悗鐨� JSON 瀛楃涓层�� + """ + try: + set_data = set(ast.literal_eval(originWave)) + except (ValueError, SyntaxError): + set_data = set() + set_data.add(vehicle) + return json.dumps(list(set_data)) + +def convert_to_structured_array(dynamicMatrix): + """ + 灏� dynamicMatrix锛堝祵濂楀垪琛紝閲岃竟鏄� dict锛夎浆鎹负 numpy 缁撴瀯鍖栨暟缁勶紝鏂逛究涔嬪悗杩涜鎺╃爜绛涢�夈�� + """ + # 瀹氫箟缁撴瀯鍖栨暟缁勭殑 dtype + dtype = [('serial', int), ('vehicle', 'U2'), ('time', int)] + structured_list = [] + for row in dynamicMatrix: + for d in row: + serial = d.get('serial', 0) + vehicle = d.get('vehicle', '0') + time_val = d.get('time', 0) + structured_list.append((serial, vehicle, time_val)) + # 杞崲涓虹粨鏋勫寲鏁扮粍锛屽苟閲嶅涓哄師濮嬩簩缁村舰鐘� + structured_array = np.array(structured_list, dtype=dtype) + return structured_array.reshape(len(dynamicMatrix), -1) + + +########################## +# 澶氭簮 BFS 鏍稿績鍑芥暟 +########################## +def process_dynamic_matrix_multi_source_bfs(dynamicMatrix, codeMatrix, cdaMatrix, radiusLen): + """ + 浣跨敤澶氭簮 BFS锛屼竴娆℃�у皢鎵�鏈夊惈鏈夎溅杈嗙殑鏍煎瓙浣滀负璧风偣鍚屾椂杩涘叆闃熷垪銆� + - 濡傛灉鏍煎瓙涓� 'NONE'锛屽垯娌垮悓涓�鏂瑰悜缁х画鐩寸嚎鎵╂暎锛� + - 鑻ラ亣鍒伴潪 'NONE' 鐨勬牸瀛愶紝鍒欒繘琛屾鍑犻噷寰楄窛绂诲垽瀹� <= radiusLen 鏃跺彲鍒拌揪锛屽苟涓斾粠姝ゅ鍐嶆鏈濆洓鏂瑰悜鎵╂暎锛堟柟鍚戦噸缃负 None锛夈�� + """ + # 1. 杞崲 dynamicMatrix 涓虹粨鏋勫寲鏁扮粍 + dynamicMatrix = convert_to_structured_array(dynamicMatrix) + rows, cols = dynamicMatrix.shape + + # 2. 鍒濆鍖� waveMatrix锛堟渶缁堣瀛樺偍鍥� Redis 鐨勭煩闃碉級 + waveMatrix = initWaveMatrix(codeMatrix) + + # 3. 寤虹珛涓�涓笌 waveMatrix 绛夊ぇ灏忕殑 2D 鏁扮粍 waveSets锛岀敤浜庝繚瀛樿溅杈嗛泦鍚堬紙鐢� set 瀛樺偍锛� + waveSets = np.empty((rows, cols), dtype=object) + for i in range(rows): + for j in range(cols): + waveSets[i, j] = set() + + # 4. 鍑嗗 BFS 闃熷垪锛岄槦鍒楀厓绱犳牸寮�: (x, y, direction, vehicle) + queue = deque() + + # 5. 鎵惧埌鎵�鏈夋湁杞﹁締鐨勬牸瀛愶紝缁熶竴鍏ラ槦鍒� + mask = (dynamicMatrix['vehicle'] != '0') & (dynamicMatrix['vehicle'] != '-1') + x_indices, y_indices = np.where(mask) + for x, y in zip(x_indices, y_indices): + v = dynamicMatrix[x][y]['vehicle'] + waveSets[x, y].add(v) + queue.append((x, y, None, v)) # 璧峰鏃� direction=None + + # 6. 寤虹珛 visited 鏉ラ伩鍏嶉噸澶嶈闂� + # 鍥犱负涓�涓綅缃� (x, y) 鍙兘琚涓溅杈嗙敤澶氱鏂瑰悜璁块棶锛岄渶瑕佽褰� (x, y, direction, vehicle) + visited = set() + + # 7. BFS 涓诲惊鐜� + while queue: + x, y, direction, vehicle = queue.popleft() + + if (x, y, direction, vehicle) in visited: + continue + visited.add((x, y, direction, vehicle)) + + # 鍒ゆ柇涓嬩竴涓鎵╁睍鐨勬柟鍚� + if direction is None: + # 鏃犳柟鍚戞椂锛屽洓涓柟鍚戝悓鏃舵墿鏁� + neighbors_info = [ + (x + 1, y, 'right'), + (x - 1, y, 'left'), + (x, y + 1, 'down'), + (x, y - 1, 'up') + ] + else: + # 鏈夋柟鍚戞椂锛屽彧寰�鍚屼竴涓柟鍚戞墿灞� + if direction == 'right': + neighbors_info = [(x + 1, y, 'right')] + elif direction == 'left': + neighbors_info = [(x - 1, y, 'left')] + elif direction == 'down': + neighbors_info = [(x, y + 1, 'down')] + elif direction == 'up': + neighbors_info = [(x, y - 1, 'up')] + else: + neighbors_info = [] + + # 閬嶅巻閭诲眳 + for nx, ny, ndir in neighbors_info: + # 杈圭晫妫�鏌� + if nx < 0 or nx >= rows or ny < 0 or ny >= cols: + continue + + neighbor_code = codeMatrix[nx, ny] + + if neighbor_code == 'NONE': + # 濡傛灉鏄� 'NONE'锛屽垯娌垮綋鍓嶆柟鍚戠户缁墿鏁� + if vehicle not in waveSets[nx, ny]: + waveSets[nx, ny].add(vehicle) + queue.append((nx, ny, ndir, vehicle)) + else: + # 闈� 'NONE'锛岄渶瑕佺敤娆у嚑閲屽緱璺濈鍒ゅ畾 + c1 = convert_to_float_array(cdaMatrix[x, y]) # 褰撳墠鍧愭爣 + c2 = convert_to_float_array(cdaMatrix[nx, ny]) # 閭诲眳鍧愭爣 + if c1.size < 2 or c2.size < 2: + continue + dist_sqr = (c1[0] - c2[0])**2 + (c1[1] - c2[1])**2 + if dist_sqr <= radiusLen**2: + # 鍙互鍒拌揪锛屽垯鎶婅溅杈嗗姞鍏� waveSets + if vehicle not in waveSets[nx, ny]: + waveSets[nx, ny].add(vehicle) + # 鏂瑰悜閲嶇疆涓� None锛岃〃绀哄洓鍚戞墿鏁� + queue.append((nx, ny, None, vehicle)) + + # 8. BFS 瀹屾垚鍚庯紝灏� waveSets 鐨勪俊鎭啓鍏� waveMatrix锛堝瓧绗︿覆褰㈠紡锛� + for i in range(rows): + for j in range(cols): + if waveSets[i, j]: + origin_str = waveMatrix[i][j] + for v in waveSets[i, j]: + origin_str = mergeWave(origin_str, v) + waveMatrix[i][j] = origin_str + + return waveMatrix + + +########################## +# 涓诲嚱鏁板叆鍙� +########################## +def main(): + global radiusLen, codeMatrix, cdaMatrix, waveMatrix # 澹版槑涓哄叏灞�鍙橀噺 + + if len(sys.argv) != 6: + print("Usage: python script.py <radiusLen> <redisHost> <redisPwd> <redisPort> <redisIdx>") + sys.exit(1) + + radiusLenStr = sys.argv[1] + try: + radiusLen = float(radiusLenStr) + except ValueError: + print("Error: radiusLen must be a float.") + sys.exit(1) + + redisHost = sys.argv[2] + redisPwd = sys.argv[3] + redisPort = sys.argv[4] + redisIdx = sys.argv[5] + + startTime = time.perf_counter() + + try: + # 1) 杩炴帴 Redis + pool = redis.ConnectionPool(host=redisHost, port=int(redisPort), password=redisPwd, db=int(redisIdx)) + r = redis.Redis(connection_pool=pool) + + # 2) 鑾峰彇骞跺姞杞� codeMatrix + codeMatrixStr = r.get('KV.AGV_MAP_ASTAR_CODE_FLAG.1') + if codeMatrixStr is None: + print("Error: 'KV.AGV_MAP_ASTAR_CODE_FLAG.1' not found in Redis.") + sys.exit(1) + codeMatrix = np.array(json.loads(codeMatrixStr.decode('utf-8')), dtype=str) + + # 3) 鑾峰彇骞跺姞杞� cdaMatrix + cdaMatrixStr = r.get('KV.AGV_MAP_ASTAR_CDA_FLAG.1') + if cdaMatrixStr is None: + print("Error: 'KV.AGV_MAP_ASTAR_CDA_FLAG.1' not found in Redis.") + sys.exit(1) + cdaMatrix = np.array(json.loads(cdaMatrixStr.decode('utf-8')), dtype=object) + + # 4) 鑾峰彇骞跺姞杞� dynamicMatrix + dynamicMatrixStr = r.get('KV.AGV_MAP_ASTAR_DYNAMIC_FLAG.1') + if dynamicMatrixStr is None: + print("Error: 'KV.AGV_MAP_ASTAR_DYNAMIC_FLAG.1' not found in Redis.") + sys.exit(1) + dynamicMatrix = np.array(json.loads(dynamicMatrixStr.decode('utf-8')), dtype=object) + + # 5) 浣跨敤澶氭簮 BFS 璁$畻 waveMatrix + waveMatrix = process_dynamic_matrix_multi_source_bfs(dynamicMatrix, codeMatrix, cdaMatrix, radiusLen) + + # 6) 灏� waveMatrix 杞负 JSON 骞跺啓鍥� Redis + waveMatrixList = waveMatrix.tolist() + waveMatrixJsonStr = json.dumps(waveMatrixList) + r.set("KV.AGV_MAP_ASTAR_WAVE_FLAG.1", waveMatrixJsonStr) + + end = time.perf_counter() + print(f"绋嬪簭杩愯鏃堕棿涓�: {end - startTime} Seconds") + print("1") + except Exception as e: + print(f"An error occurred: {e}") + sys.exit(1) + +if __name__ == "__main__": + main() \ No newline at end of file -- Gitblit v1.9.1