From 664ece9df4adcd594fac02b177bee9310b64070e Mon Sep 17 00:00:00 2001
From: luxiaotao1123 <t1341870251@163.com>
Date: 星期五, 27 十二月 2024 15:30:23 +0800
Subject: [PATCH] #
---
zy-acs-manager/src/main/resources/agv1.py | 313 +++++++++++++++++++++++++++++----------------------
1 files changed, 176 insertions(+), 137 deletions(-)
diff --git a/zy-acs-manager/src/main/resources/agv1.py b/zy-acs-manager/src/main/resources/agv1.py
index 245bb6e..47af53d 100644
--- a/zy-acs-manager/src/main/resources/agv1.py
+++ b/zy-acs-manager/src/main/resources/agv1.py
@@ -4,140 +4,178 @@
import json
import time
import redis
-from collections import deque
+from collections import deque, defaultdict
-radiusLen = None
-
-# 灏嗗瓧绗︿覆杞崲涓烘诞鐐瑰瀷鏁扮粍
+##########################
+# 宸ュ叿鍑芥暟
+##########################
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)
- return str_array
+ elif isinstance(str_array, list) or isinstance(str_array, np.ndarray):
+ return np.array(str_array, dtype=float)
+ return np.array([], dtype=float)
-def getWaveScopeByCode_iterative(x, y):
+def initWaveMatrix(codeMatrix):
"""
- 浣跨敤骞垮害浼樺厛鎼滅储锛圔FS锛夋潵浠ｆ浛閫掑綊锛屼互閬垮厤閫掑綊娣卞害杩囧ぇ鐨勯棶棰樸��
+ 鏍规嵁 codeMatrix 鍒濆鍖� waveMatrix銆�
+ 鑻ユ牸瀛愪负 'NONE'锛屽垯 waveMatrix[x][y] = "-"
+ 鍚﹀垯涓� "[]"
"""
- includeList = []
- existNodes = set()
- queue = deque()
-
- originNode = {"x": x, "y": y}
- currNode = {"x": x, "y": y}
- queue.append(currNode)
- existNodes.add((x, y))
-
- while queue:
- node = queue.popleft()
- node_x, node_y = node['x'], node['y']
- neighbors = [
- (node_x + 1, node_y),
- (node_x - 1, node_y),
- (node_x, node_y + 1),
- (node_x, node_y - 1)
- ]
-
- for neighbor in neighbors:
- nx, ny = neighbor
- # 妫�鏌ヨ竟鐣屾潯浠�
- if (nx < 0 or nx >= codeMatrix.shape[0] or ny < 0 or ny >= codeMatrix.shape[1]):
- continue
- if (nx, ny) in existNodes:
- continue
-
- existNodes.add((nx, ny))
- neighbor_code = codeMatrix[nx, ny]
-
- if neighbor_code == 'NONE':
- queue.append({"x": nx, "y": ny})
- else:
- o1Cda = convert_to_float_array(cdaMatrix[x, y])
- o2Cda = convert_to_float_array(cdaMatrix[nx, ny])
-
- num1 = (o1Cda[0] - o2Cda[0]) ** 2
- num2 = (o1Cda[1] - o2Cda[1]) ** 2
- if num1 + num2 <= radiusLen ** 2:
- includeList.append({
- "x": int(nx),
- "y": int(ny),
- "code": str(codeMatrix[nx, ny])
- })
- queue.append({"x": nx, "y": ny})
-
- return includeList
-
-def find_value_in_matrix(value):
- indices = np.where(codeMatrix == value)
- return list(zip(indices[0], indices[1]))
-
-def initWaveMatrix():
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] = '[]'
-
+ 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()
- # 濡傛灉 vehicle 涓嶅湪闆嗗悎涓紝鍒欐坊鍔�
set_data.add(vehicle)
- # 杩斿洖搴忓垪鍖栧悗鐨勫瓧绗︿覆
return json.dumps(list(set_data))
-# 灏� dynamicMatrix 杞崲涓� numpy 缁撴瀯鍖栨暟缁�
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:
- # 鎻愬彇瀛楁锛岀‘淇� 'time' 瀛樺湪锛屽惁鍒欒缃负榛樿鍊硷紙渚嬪 0.0锛�
- serial = d.get('serial', 0)
- vehicle = d.get('vehicle', '0')
- time_val = d.get('time', 0)
- structured_list.append((serial, vehicle, time_val))
-
- # 灏嗗祵濂楃殑鍒楄〃杞崲涓虹粨鏋勫寲鏁扮粍
+ 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)
-# 浣跨敤 numpy 鍔犻�熺殑浠ｇ爜
-def process_dynamic_matrix(dynamicMatrix, codeMatrix):
- global waveMatrix # 纭繚 waveMatrix 鏄叏灞�鍙橀噺
- # 灏� dynamicMatrix 杞崲涓虹粨鏋勫寲鏁扮粍
+##########################
+# 澶氭簮 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)
-
- # 鑾峰彇 dynamicMatrix 鐨勫舰鐘�
rows, cols = dynamicMatrix.shape
- # 鍒涘缓涓�涓竷灏旀帺鐮侊紝鐢ㄤ簬绛涢�夊嚭 vehicle 涓嶄负 '0' 鍜� '-1' 鐨勫厓绱�
+ # 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 鍜� y 鍧愭爣
x_indices, y_indices = np.where(mask)
-
- # 閬嶅巻婊¤冻鏉′欢鐨勫潗鏍�
for x, y in zip(x_indices, y_indices):
- vehicle = dynamicMatrix[x][y]['vehicle']
- includeList = getWaveScopeByCode_iterative(x, y)
- for include in includeList:
- originWave = waveMatrix[include['x']][include['y']]
- waveMatrix[include['x']][include['y']] = mergeWave(originWave, vehicle)
+ 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 # 澹版槑涓哄叏灞�鍙橀噺
@@ -146,7 +184,11 @@
sys.exit(1)
radiusLenStr = sys.argv[1]
- radiusLen = float(radiusLenStr)
+ try:
+ radiusLen = float(radiusLenStr)
+ except ValueError:
+ print("Error: radiusLen must be a float.")
+ sys.exit(1)
redisHost = sys.argv[2]
redisPwd = sys.argv[3]
@@ -155,49 +197,46 @@
startTime = time.perf_counter()
- # 鍒涘缓涓�涓繛鎺ユ睜
- pool = redis.ConnectionPool(host=redisHost, port=int(redisPort), password=redisPwd, db=int(redisIdx))
- r = redis.Redis(connection_pool=pool)
+ try:
+ # 1) 杩炴帴 Redis
+ pool = redis.ConnectionPool(host=redisHost, port=int(redisPort), password=redisPwd, db=int(redisIdx))
+ r = redis.Redis(connection_pool=pool)
- # 鑾峰彇骞跺姞杞� 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.")
+ # 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)
- codeMatrix = np.array(json.loads(codeMatrixStr))
-
- # 鑾峰彇骞跺姞杞� 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))
-
- # 鑾峰彇骞跺姞杞� 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))
-
- # 鍒濆鍖� waveMatrix
- waveMatrix = initWaveMatrix()
-
- # 澶勭悊 dynamicMatrix
- process_dynamic_matrix(dynamicMatrix, codeMatrix)
-
- # 灏� numpy.ndarray 杞崲涓哄祵濂楀垪琛�
- waveMatrixList = waveMatrix.tolist()
- # 灏嗗祵濂楀垪琛ㄨ浆鎹负 JSON 瀛楃涓�
- waveMatrixJsonStr = json.dumps(waveMatrixList)
-
- # 灏嗙粨鏋滀繚瀛樺洖 Redis
- r.set("KV.AGV_MAP_ASTAR_WAVE_FLAG.1", waveMatrixJsonStr)
-
- end = time.perf_counter()
- # 鎵撳嵃绋嬪簭杩愯鏃堕棿
-# print(f"绋嬪簭杩愯鏃堕棿涓�: {end - startTime} Seconds")
- print("1")
if __name__ == "__main__":
main()
\ No newline at end of file
--
Gitblit v1.9.1