From 7d476301542af39b9b110c65bd6adba2e5cae9e5 Mon Sep 17 00:00:00 2001 From: Diego Freitas Date: Thu, 18 Sep 2025 09:56:17 -0300 Subject: [PATCH] adicionad scripts --- .../modulos/__pycache__/lora.cpython-311.pyc | Bin 0 -> 4778 bytes .../modulos/__pycache__/pid.cpython-311.pyc | Bin 0 -> 3994 bytes .../workers/manager_worker/modulos/mpc_old.py | 1782 +++++++++++++++++ .../__pycache__/costmap_fuser.cpython-311.pyc | Bin 0 -> 31781 bytes 4 files changed, 1782 insertions(+) create mode 100644 AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/__pycache__/lora.cpython-311.pyc create mode 100644 AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/__pycache__/pid.cpython-311.pyc create mode 100644 AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/mpc_old.py create mode 100644 AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/__pycache__/costmap_fuser.cpython-311.pyc diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/__pycache__/lora.cpython-311.pyc b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/__pycache__/lora.cpython-311.pyc new file mode 100644 index 0000000000000000000000000000000000000000..48df11c5590b3f2f86da5c6a035522b04b9117c2 GIT binary patch literal 4778 zcmbVQUrZax8K1SiwgDS#V;kE*a02AcTta&EF1dsBdOk2Ty#Nv9+J+KYi)SGwwwK#o zCxly{qUs*}&Hn5^p}`BAxE3-|S*L zOMsw`*WY~e%{RaKzHi1epZ80rvk^i0F#GkD#TJDALmSn??l8}%fVqt@!b}Ry(Atz@ z(xw>`LwR$`oVLtZ((DYYwJj-Y+BRcjkO_?;%zlBemA5@IBlH-)dYQ3fW*S9o+q5|m zVMyz+kdgUiSr|(Rb6jeQ$4Mz-B93X9le5yefU_xqSTDwh1a!AT)=E|Zw$7DQoAkmGaU`fL;$GzN8M zE}5BKKK;(@2tSuyn7wdAz9M91rxRjwNtR}>3*uE?lxDB+TuQzY*PF9xEzZ(xN)WmJ zr5nT^k0&!pIUZkWt;eXpDzt%7QYVm`sO<0f(_-H8`=*jN1XV++EdZ4x51;3tfDCbU zfI z3)_sDoJFQ~?G(kdPQ81jU=ub!g7tn}TXW-_IrdN}u=5T?u^Ld!``E(12E``3>kjPe zkrmqxvSo?Eww(P?NcKY@H5>}50XH6GQ?Bt)C{3?HX_8y(@xhG>qcpC-S!kR&=Xgb{ zX%8)@LCb-iO2Z25wq01wuV~r#&}zZWIoATpxfNCl;+C9Caj$D9gjwX;P$rnnUEYvSosoY;TQg4-1D zJ%j@fSfiy3QhWwH-)r!Kiem@Qw>RF)@rU(pF?y48$sP6B(wki`IX~&sVqY#GpQ=l6=gXLjfoc!L z{fH8{haOmVujM7c%>_OWP;{NU&Rj>!=1b^06X}}Rz5o!UAt7Y=gv?=Klr#Z9pIpd_ z&?0uYR1--FliC7)-!G}9z?H_JqKoRNv zL>!oxYBp>SfGA7XI`fae3gMUR*&YSq_M1Rf98EZHlL8jvTWJ?Qj@|qwKm)3@~1ShX0ah6LFKe&P+1LUQ+05fn2NRPNA7=?M6OI_im z2qQ+QbH`$$D1@_Zx<%y?ed^`d10>@=y zh2-pG{sw7?EhqRT8T=>gl1L}BWVkdR35s;ti8&)&Crp|&)D3n`M?V>f#mCQ0#Pw;k z;EVLMChn?RGzX;Hl_RV?2fI+K#AT75-6B0?z(Lpv(4p0^UmNSYpw7$7+A@fJbc9pe zWv~nQ6zl{;6L2g`R0tHt(0xg4OWX~JCngLp(d#3;UlHjsMOx}X(pGpS^iuk|tIq)g zq<_Nd-b5ACub74lOxfdCJ>5l5uj=W|50_ots;hrPe)3t-HKe+R3a;o=F&`}l-ckea z6$686U@(7ftLtdN(eW&FWbM<3*}K{G-){C6yWUf~-YbUQS3~dT&y{Okmp5a@u0geH zuoxOrLqqwG%i*5&OJ838%jM0%r`cj}Ozn*o!=q|=wBYD`VMV^s+R=x-cYD`cHd~9K zch%6lMek|Vd%EEL+0*WPtn3XQ*bfM89c$5tvAePL$i{TB<88I$?V^7`^$+C7%KrBK zx}Nz0YprXE^|3qYqVI(2JCPqPzbXRsQ0Lmk^`CC|{ybC+o>GIS^1mzxJ9dk|3Itc#lB?}@^R4E!&x@{p2y(&I56o8Eu?jM|&y>KksXu(Y8Y_9* zZ%^EsD0J66KfTr3Q$cQbAI#tT=7xD=elz=3=4*5Dvq$$5&&|d`DGZUw*Vi zgUUKjmV>=&aA4KCYAw0_Ye#Q|RzsTTtsPMz82Q#6DYzq=^u(&QVns*ZD22lJPn3d3 zO6|u=J;zIZC$_@JOYrpc(x}9;Q}TCKShKVFI|Sr=N-9S3!wWld1uKaD+Fqt*hr8gJ*p&bNnR;dnIC%c`wq6$< zld5B~u-Eegl!}>R0VC1d`*-L2&kvw)1{%kYo4+~RHGb6m@1rd66A_mFr=^EtJWd+p z`cY1KM?C&1Tnc)RNV6F66Cht9k!CR3sUq4bM+y9GMbcRyH*24Y$;^U7HPZNNhNZsk zU+KUUF47?OLq$5IE1byVez?!HS0V8Q?Oh0*k?=A^zkeJ%DtsoJmbB|be}@n<{EzR~ zZz!psz8MgywYu$>C|C36NBdb#kN-ft3>`G*ll}+GwO=Y}u< literal 0 HcmV?d00001 diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/__pycache__/pid.cpython-311.pyc b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/__pycache__/pid.cpython-311.pyc new file mode 100644 index 0000000000000000000000000000000000000000..9215e1cab2e8058298872875f799669949ddb81e GIT binary patch literal 3994 zcma(UOKcm*b(SBI5O*~P{TVxw8%b_!IF)P*HEf`SZ590pL^mrgEmNXMb#|pX zD%HAx4{W1@3OERD_@DxYE5S&h9tx;H4>`6suDgfr0s_?LQ20heEl}jt_hz|6u9F5C zl5gI7GxKKV&HM59p->Zn_O~Y=+`APZTQ(Z01vqbuv+;8-Ax90v@g)6o|-&3vQr*Z?%FKDEkPc-~tL)vR3c_ zWnYvO8i4ZhF&|J3obm(Zvnd((^E)VhIyT1CisOs|)mn{FK?fDGDYehW2w4f+_;WyB z*aU%Hn{7hIo3wBW@7&5Mf=`k7mrXBQ$c?$>Cy%lHbv7O`eXHx@lBA>o$=HZ&A+FVR z)3us`pKLZNQbv}qq(tXp*xTar8tLynpPBjCYQZ`RTN!^gLFfqR-iD^pnW(c7u z<FiyKuR~Gs6kmp-(kZ?QMTJw;Gexy3$U9@V&-2$VB^T21vuP$u%h_ zEvKny(aE*Eyphc-$@PppvHrg47sX62ql)5Ihr_-nY(Cnf@&SMc#OUlVdUk?ubde}9 zeTJhui46iXY*f4UE})uDx_Y1s89g9H_jehwUN)!i43gc(;0X2u$AhEK;=eoh>>Q&` z&J1JUXPjkyf7y@UQ#cMuaW4V_waq;6pMZQsRPJwR^D6f+0=_A`w%{?|3>ePvu?^O| z!sW|u*;8=I-Ys~^E%3-KcvkGe#<^q|Z&+pCN+FcwnqfWnWZ4a)KDN5HvgX-jh5I$^ z?=I}@)T>I%~r?%J6q+P&c^+w zdktL24X*Un#u|6i7|1f)H`CdC3cOEFi|c8cO6Qn&37ZOP0Zf>7$EH9D6UYp0X!hb!RB4}hYRJ{#9_M@yLjZ*+Ar(vm$l%#dhp$%a1!ZwbbfDW z-@Pv#e&gWoK~Ni<)*>@tLOvtbT}nOaDs}A+?GEj!&kBdi(d|!%{`8|i{6ZW1E}K!e zc6f zT`DP%`@#y|oi$kJVFnN3D&?vroceo$WS)8g#Bpa*C#i0h^QKP9aTe#P;U;hzaNX?& zwaq>u@V+qXIXM3d9sp$TD$^PD78=0yBe)*U4AQJtXkhwq@K9?o2llcW(*dAxI?k)t zf$XclH@I-!TIcH$3+WU)u_CbHDu5M&{xX+KzJv5S&5KfdCIoaTkhaM8P5mh3gX2EiEau3&|(~ zG!ZLrvy_e5+REFg(YT)lMN1A!G`-Sto=T=mR!tA9HJapfR;|_@O;*{Wrq|XLbQe3} zTOqTN)hSSA(5=olq9uk{95@S$6a(%FfwCp~sPVUfM}eaEIMlKm{ZJ{*>`m)Km$aTs zN7uB@IlXgE3tiDeSBfrUaLmf1fwppBz~~(=&hLEhq-PYFT}E@;&UQ)F2PU=V$)cyy zKtip%(|X&87981ceHnPG9C*us^gO?lfU(9D634OLlC1YnmHVdt zxNsExbN^qW`i-AxGon5tYO(k9*n61aVQptm>HB)$q}DNc_>R^-Q*1g8gp2B~taprS z&Er}it_R}#^DhGz%7F_;u%k4j1^bFO*@5)n;NGGZoHRn=-KgF=riI4#r}cAJwcyp) zZXt9{sJP*1P&oX@PliVK?tYv)965^W-@c_y+}0;=lZVv>x2<( zLF7?R3l5`{krRudcf73y-+t|Ng(gsJTO~~3G@hs7mx0l8VAP29J?$^vgw@5wPNF35 z-FckRf@4S&6Go`BH2iepWoWz{8vlx!C+!2El_sO_ES{?S1Ri0l?2oY=l;U9vo*V6) zrJ&yc0)KfN|0)>8XEUl+ui#9cI@lOb8;s)VLFEAks(kK*&a?)yc<5oZhs&xxzyp{Q zF?T|jusn(R6!RbEd+2d-f%%Gp$E_9#KAMJF9qhf`Y426`0?bsd0f3^@E*GoSqg6|O zt>UrlAAm1YY+OjdUpOdsbJAK`6wM}4WQ8!|K~elEc$qaqdtg^Ogy1}a2>=I#$zZ-p zQIiayf|VZtc)&juSJXRFAsn3ddcRd69E2};yDP*AWWnd1sE{uLYfIcM?y|xd+xZ>b y9?KPWn$P{0t!sRTP5}=UtlR`paSMWAkiY}>sd#=U2(ZbQ!`F=e@BcH(_WB=8_eMbg literal 0 HcmV?d00001 diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/mpc_old.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/mpc_old.py new file mode 100644 index 000000000..600baefef --- /dev/null +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/mpc_old.py @@ -0,0 +1,1782 @@ +import math +import time +import numpy as np +from copy import deepcopy +from bisect import bisect_right +from concurrent.futures import ThreadPoolExecutor, as_completed + +from shared.enums import StatusCarroMapa, TipoMovimentoDirecional, TipoPontoRua +from manager_worker.config import mostrar_log +from shared.gps_handler import GPSHandler +#from shared.visualizador_trajetoria import VisualizadorTrajetoria + +_mpc = None +_iniciado = False + +def inicializar(parametros, mapa, p_ref, forcar): + global _mpc, _iniciado + if not _iniciado or len(_mpc.pontos_info) != len(mapa) or forcar: + _mpc = ControladorMPC(parametros_mpc=parametros, pontos_mapa=mapa, p_ref=p_ref) + mostrar_log(f"MPC iniciado! Trajetoria com {len(_mpc.pontos_info)} pontos") + _iniciado = True + +def get_mpc(): + global _mpc + return _mpc + +def comando_parado(): + return 0.0, TipoMovimentoDirecional.RodasDianteiras + +class ControladorMPC: + def __init__(self, parametros_mpc, pontos_mapa, p_ref): + try: + self.gps_handler = GPSHandler(p_ref[0], p_ref[1]) + self.lat0 = p_ref[0] + self.lon0 = p_ref[1] + #self.visualizador = VisualizadorTrajetoria() + + self.ultima_atualizacao = None + self.dt = 0 + + self.angulo_max_graus = parametros_mpc.get("angulo_max_graus", 30.0) + self.velocidade_min = parametros_mpc.get("velocidade_min", 0.4) + self.velocidade_max = parametros_mpc.get("velocidade_max", 1.9) + self.horizonte = parametros_mpc.get("horizonte", 2.5) + self.horizonte_passos = 10 + + self.pontos_info = [] + self.visitados_execucao = [] + + self.pontos_info = pontos_mapa + + if pontos_mapa: + self.visitados_execucao = [False] * len(self.pontos_info) + + self.executor = ThreadPoolExecutor(max_workers=20) + except Exception as e: + mostrar_log(f"Erro ao inicializar MPC: {e}") + + + # UTILS + + def _proximo_nao_visitado(self, visitados): + for i, v in enumerate(visitados): + if not v: + return i + return len(visitados) - 1 + + def _corrigir_pontos_visitados(self, x, y, pontos_visitados, idx_atual=-1, limite_max_avanço=5.0): + if (idx_atual > -1): + for idx, ponto in enumerate(self.pontos_info): + if (idx < idx_atual): + pontos_visitados[idx] = True + else: + break + return idx_atual + + for idx, ponto in enumerate(self.pontos_info): + if pontos_visitados[idx]: + continue + pos = ponto["xy"] + margem = ponto.get("distanciaMargem", 0.7) + dist = np.linalg.norm([x - pos[0], y - pos[1]]) + if dist < margem: + for i in range(idx + 1): + pontos_visitados[i] = True + return idx + if idx > 0 and not pontos_visitados[idx - 1] and idx >= limite_max_avanço: + break + return self._proximo_nao_visitado(pontos_visitados) + + def _calcular_pesos_movimento(self, contexto, erro_ori_rad): + peso_erro_pos = 1.2 # 0 + peso_erro_ori = 1.0 # 1 + peso_suavidade = 1.4 # 2 + peso_fator_re = 0.2 # 3 + peso_ideal = 0.4 # 4 + + CARRO = contexto.get("Carro", {}) + status = StatusCarroMapa(CARRO.get("Status", StatusCarroMapa.Parado.value)) + dentro = CARRO.get("DentroCorredor", False) + manobrando = CARRO.get("ManobrandoEntreRuas", False) + velocidade = CARRO.get("Velocidade", self.velocidade_min) + + # Penalidade proporcional à velocidade + penalidade_por_velocidade = (velocidade / self.velocidade_max) / 10.0 + erro_ori_deg = np.degrees(erro_ori_rad) + erro_ori_clipped = max(min(erro_ori_deg, self.angulo_max_graus), -self.angulo_max_graus) + erro_ori_norm = abs(erro_ori_clipped / self.angulo_max_graus) + + custo_movimento = { + TipoMovimentoDirecional.RodasDianteiras: 0.0, + TipoMovimentoDirecional.MovimentoArco: 0.0 + } + + if manobrando or status in [ StatusCarroMapa.EntrandoRua, StatusCarroMapa.SaindoRua, StatusCarroMapa.Manobrando ]: + custo_movimento[TipoMovimentoDirecional.MovimentoArco] += 0.0 + custo_movimento[TipoMovimentoDirecional.RodasDianteiras] += 1.0 + else: + custo_movimento[TipoMovimentoDirecional.MovimentoArco] += 0.0 if erro_ori_norm >= 0.4 else 1.0 + custo_movimento[TipoMovimentoDirecional.RodasDianteiras] += 1.2 if erro_ori_norm >= 0.4 else 0.0 + + return (peso_erro_pos, peso_erro_ori, peso_suavidade, peso_fator_re, peso_ideal, custo_movimento) + + def _gerar_angulos_candidatos(self, angulo_ideal_rad, erro_ori, contexto): + def clamp(v, vmin, vmax): + return max(vmin, min(vmax, v)) + + CARRO = contexto.get("Carro", {}) + status = CARRO.get("Status", 0) + manobrando = CARRO.get("ManobrandoEntreRuas", False) + + if manobrando or status in [ StatusCarroMapa.EntrandoRua, StatusCarroMapa.SaindoRua, StatusCarroMapa.Manobrando ]: + tipos_validos = [ TipoMovimentoDirecional.MovimentoArco ] + else: + tipos_validos = [ TipoMovimentoDirecional.RodasDianteiras, TipoMovimentoDirecional.MovimentoArco ] + + erro_ori_deg = np.degrees(erro_ori) + angulo_ideal_deg = np.degrees(angulo_ideal_rad) + + # Passo varia conforme erro angular + if erro_ori_deg > 10: + passo = 5.0 + faixa = 15.0 + elif erro_ori_deg > 5: + passo = 2.5 + faixa = 7.5 + else: + passo = 1.5 + faixa = 4.5 + + # Geração em torno do ideal + candidatos_graus = list(np.arange(angulo_ideal_deg - faixa, angulo_ideal_deg + faixa + 0.1, passo)) + + extras = [ + -self.angulo_max_graus, # mínimo possível + -self.angulo_max_graus * 0.75, # -75% + self.angulo_max_graus * 0.75, # +75% + self.angulo_max_graus # máximo possível + ] + + # Clamping e eliminação de duplicatas + candidatos_radianos = sorted(set( + np.radians(clamp(g, -self.angulo_max_graus, self.angulo_max_graus)) + for g in (candidatos_graus + extras) + )) + + #print(f"Ideal: {round(angulo_ideal_deg, 2)}°, Erro Ori: {round(erro_ori_deg, 2)}°, Obstáculos: {len(obstaculos)}, Candidatos: {[round(np.degrees(a), 2) for a in candidatos_radianos]}") + return tipos_validos, candidatos_radianos + + def _erro_angular(self, angulo_alvo, angulo_atual, angulo_caminho, peso_local=0.5): + erro_com_ponto = (angulo_alvo - angulo_atual + np.pi) % (2 * np.pi) - np.pi + erro_com_caminho = (angulo_caminho - angulo_atual + np.pi) % (2 * np.pi) - np.pi + + erro_combinado = (1 - peso_local) * erro_com_caminho + peso_local * erro_com_ponto + return abs(erro_combinado) + + def _atualiza_posicao_obstaculos(self, contexto, new_x, new_y, new_theta, omega): + obstaculos = deepcopy(contexto.get("VisualWorker", {}).get("Obstaculos", [])) + velocidade = contexto.get("Carro", {}).get("Velocidade", 0) + for obst in obstaculos: + obst["distancia_m"] = max(0.0, obst["distancia_m"] - velocidade * min(0.2, self.dt)) + # Assume que o obstáculo está a 'distancia_m' à frente, em um ângulo correspondente ao centro_percentual original + fov = contexto["VisualWorker"]["Camera"]["FovH"] # exemplo: np.radians(70) + centro_antigo = obst.get("centro_percentual", [0.5, 0.5])[0] + angulo_original = (centro_antigo - 0.5) * fov + # Estima a posição real do obstáculo + x_obst = new_x + obst["distancia_m"] * np.cos(new_theta + angulo_original) + y_obst = new_y + obst["distancia_m"] * np.sin(new_theta + angulo_original) + # Recalcula o centro_percentual estimado + centro_x = centro_antigo - (omega / fov) + obst["centro_percentual"] = [centro_x, obst.get("centro_percentual", [0.5, 0.5])[1]] + #print(f"📍 (x: {new_x}, y: {new_y}, t: {new_theta}) Obstáculo simulado em x: {x_obst:.2f}, y: {y_obst:.2f}, omega: {np.degrees(omega):.2f}°, centro_x: {obst['centro_percentual'][0]:.3f} ({centro_x:.3f}) / ({centro_antigo:.3f})") + return obstaculos + + def _simular_trajetoria_matriz_custo(self, matriz_custo, linhas_info, tipo, angulo, distancia_m, theta_rad, largura_equipamento): + altura = len(matriz_custo) + largura = len(matriz_custo[0]) + if largura == 0 or altura == 0 or distancia_m == 0: + return 0.0, True + + # 2. Ordena por profundidade crescente + linhas_ordenadas = sorted(linhas_info.values(), key=lambda item: item["distancia"]) + distancia_min_m = linhas_ordenadas[0]["distancia"] + distancia_max_m = linhas_ordenadas[-1]["distancia"] + + # 3. Simula trajetória ponto a ponto e acumula custo + x_sim = self.x_ant # no referencial do robô: (0, 0) + y_sim = self.y_ant + custo_total = 0.0 + passos = 1 + matriz_concluida = False + fora_matriz_horizontal = False + celulas_ocupadas = [] + + for _ in range(passos): + # Avança no referencial do robô + x_sim += distancia_m * np.sin(-theta_rad) + y_sim += distancia_m * np.cos(theta_rad) + + # Verifica limites de profundidade + if y_sim < distancia_min_m: + continue # fora da matriz + elif y_sim > distancia_max_m: + matriz_concluida = True + break + + # Localiza a linha mais próxima da profundidade atual + linha_alvo = next( + (linha["idx"] for linha in reversed(linhas_ordenadas) if y_sim >= linha["distancia"]), + linhas_ordenadas[0]["idx"] + ) + escala_x = linhas_info[linha_alvo]["escala_x"] + largura_total = escala_x * largura + x_centralizado = x_sim + largura_total / 2 + coluna_alvo = int(x_centralizado / escala_x) + + #if 0 <= linha_alvo < altura and 0 <= coluna_alvo < largura: + # custo_total += matriz_custo[linha_alvo][coluna_alvo]["custo"] + + escala_x = matriz_custo[linha_alvo][0]["escala_x"] + colunas_ocupadas = int(np.ceil(largura_equipamento / escala_x)) + colunas_por_lado = colunas_ocupadas // 2 + + # 2. Soma os custos ao longo do caminho (de trás até a frente) + for i in range(altura - 1, linha_alvo - 1, -1): + for j in range(coluna_alvo - colunas_por_lado, coluna_alvo + colunas_por_lado + 1): + if 0 <= i < altura and 0 <= j < largura: + celulas_ocupadas.append((i, j)) + custo_total += matriz_custo[i][j]["custo"] + #else: + # custo_total += 20.0 + # fora_matriz_horizontal = True + + mostrar_log(f"d: {self.y_ant:.2f} m, {tipo.name} {np.degrees(angulo):.2f} - Posicao ({linha_alvo}, {coluna_alvo}), x_sim: {x_sim}, y_sim: {y_sim}, Custo: {round(custo_total, 4)}, Theta: {np.degrees(theta_rad):.2f}, Passo: {round(passos * distancia_m, 2)} m") #, celulas: {celulas_ocupadas} + + #if fora_matriz_horizontal and len(celulas_ocupadas) == 0: + # matriz_concluida = True + # custo_total = float('inf') + + return custo_total, matriz_concluida + + def corrigir_pose_por_latencia( + self, + x, y, theta, visitados, + latency_s, # atraso a compensar (s) + dt, # dt nominal do seu controle + nova_posicao_fn, # self._nova_posicao + u_hold, # {'v': m/s, 'omega': rad/s} (ou forneça 'tipo' e 'angulo' + calc_omega) + cmd_seq=None, # opcional: [(dur_s, u_dict), ...] que cobre latency_s + time_left_ms=lambda: 1e9, + calc_omega_fn=None # opcional: fn(tipo, angulo_rad, v) -> omega + ): + """ + Propaga (x,y,theta) por 'latency_s' usando 'nova_posicao_fn' que depende de self.dt. + Retorna (x, y, theta) estimados no presente. + """ + visitados_copy = visitados.copy() + latency_s = max(0.0, float(latency_s)) + if latency_s == 0.0: + return x, y, theta, visitados_copy + + # --- constrói sequência efetiva de comandos --- + if not cmd_seq: + cmd_seq = [(latency_s, u_hold)] + else: + total = sum(max(0.0, d) for d, _ in cmd_seq) + if total < latency_s: + cmd_seq = list(cmd_seq) + [(latency_s - total, cmd_seq[-1][1])] + + # --- adaptador para usar dt_eff com fn que usa self.dt --- + def aplicar_passo_dt(xk, yk, thetak, u, dt_eff): + # garantir que temos omega e v + v = u.get('v', 0.0) + if 'omega' in u: + omega = u['omega'] + else: + if calc_omega_fn is not None and 'tipo' in u and 'angulo' in u: + omega = calc_omega_fn(v, u['angulo'], u['tipo']) + else: + omega = 0.0 # fallback: reta + old_dt = self.dt + try: + self.dt = dt_eff + return nova_posicao_fn(xk, yk, thetak, omega, v, u.get('tipo'), u.get('angulo', 0.0)) + finally: + self.dt = old_dt + + # --- integra por tempo, quebrando cada trecho em subpassos ~dt --- + t_rem = latency_s + for dur_s, u in cmd_seq: + if t_rem <= 0.0: + break + seg = min(dur_s, t_rem) + if seg <= 0.0: + continue + + n = max(1, int(round(seg / dt))) + dt_eff = seg / n + + for _ in range(n): + if time_left_ms() <= 0.0: + return x, y, theta, visitados_copy + x, y, theta = aplicar_passo_dt(x, y, theta, u, dt_eff) + idx_alvo = self._corrigir_pontos_visitados(x, y, visitados_copy) + + t_rem -= seg + + # normaliza theta se quiser + # theta = (theta + np.pi) % (2*np.pi) - np.pi + return x, y, theta, visitados_copy + + + + # MPC MIOPE + + def compute(self, contexto, comando_anterior): + try: + agora = time.time() + self.dt = 0.2 if self.ultima_atualizacao == None else agora - self.ultima_atualizacao + self.ultima_atualizacao = agora + comando = self._processar_mpc(contexto, comando_anterior) + mostrar_log(f"Estatisticas MPC: dt = {round(self.dt, 2)} s, freq = {round(1.0 / self.dt, 2)} Hz") + return comando + + except Exception as e: + mostrar_log(f"❌ Erro ao processar compute MPC: {e}") + + def _processar_mpc(self, contexto, comando_anterior): + try: + if not self.pontos_info: + return None + + GPS = contexto.get("GPS", {}) + pos_lat = GPS.get("Latitude", 0) + pos_lon = GPS.get("Longitude", 0) + theta = np.radians(GPS.get("AnguloCarro", 0)) + velocidade = contexto.get("Carro", {}).get("Velocidade", 0) + + x, y = self.gps_handler.converter_latlon_para_xz(pos_lat, pos_lon) + + x_plot = x + y_plot = y + sim = [(x, y, theta)] + x_temp, y_temp, theta_temp = x, y, theta + + atraso_simulado_passos = 0 # Delay estimado de resposta do robô + + distancia_m = velocidade * self.dt + + if distancia_m > 0: + pontos_horizonte = int(self.horizonte / (distancia_m)) + else: + pontos_horizonte = 10 + + pontos_horizonte = max(10, min(pontos_horizonte, 120)) # entre 10 e 120 pontos + + pontos_horizonte = self.horizonte_passos + + angulo_final = None + tipo_final = None + self.x_ant = 0 + self.y_ant = 0 + self.dist_acumulada = 0 + + for i in range(pontos_horizonte): + primeira_execucao = angulo_final is None and tipo_final is None + if primeira_execucao: + idx_alvo = self._corrigir_pontos_visitados(x_temp, y_temp, self.visitados_execucao) + else: + idx_alvo = self._corrigir_pontos_visitados(x_temp, y_temp, deepcopy(self.visitados_execucao)) + if idx_alvo >= len(self.pontos_info): + angulo_final, tipo_final = comando_parado() + break + if i < atraso_simulado_passos and comando_anterior: + angulo_mpc = np.radians(comando_anterior["angulo"]) + tipo_mpc = TipoMovimentoDirecional(comando_anterior["tipo"]) + else: + ponto_info = self.pontos_info[idx_alvo] + tipo_ponto = TipoPontoRua(ponto_info.get("tipo")) + ponto_alvo = ponto_info["xy"] + angulo_mpc, tipo_mpc = self._calcular_melhor_candidato(ponto_alvo, x_temp, y_temp, theta_temp, deepcopy(self.visitados_execucao), contexto, comando_anterior, primeira_execucao) + if primeira_execucao: + angulo_final = angulo_mpc + tipo_final = tipo_mpc + + omega = self.gps_handler.calcular_omega(velocidade, angulo_mpc, tipo_mpc) + + # Simula a movimentação usando theta com base rotacionada (-theta + 90) + theta_plot = (-theta_temp) + np.radians(90) + x_temp += velocidade * np.cos(theta_temp) * self.dt + y_temp += velocidade * np.sin(theta_temp) * self.dt + theta_ant = theta_temp + theta_temp += omega * self.dt + delta_theta = (theta_ant - theta_temp + np.pi) % (2 * np.pi) - np.pi + + self.x_ant += distancia_m * np.sin(-delta_theta) + self.y_ant += distancia_m * np.cos(delta_theta) + + comando_anterior["angulo"] = angulo_mpc + comando_anterior["tipo"] = tipo_mpc + + x_plot += velocidade * np.cos(theta_plot) * self.dt + y_plot += velocidade * np.sin(theta_plot) * self.dt + + sim.append((x_plot, y_plot, theta_plot)) + + # Converte os pontos simulados para lat/lon + simulacao_latlon = [] + for sx, sy, theta in sim: + dlat = sy / self.gps_handler.raio_terra + dlon = sx / (self.gps_handler.raio_terra * np.cos(np.radians(self.lat0))) + lat = self.lat0 + np.degrees(dlat) + lon = self.lon0 + np.degrees(dlon) + simulacao_latlon.append([lat, lon, theta]) + + #angulo_final, tipo_final = self._calcular_melhor_candidato(sim[0][0], sim[0][1], theta, self.visitados_execucao, contexto, comando_anterior, False) + + return { + "angulo": np.degrees(angulo_final), + "tipo": tipo_final, + "simulacao": simulacao_latlon, + "parada_necessaria": False + } + + except Exception as e: + mostrar_log(f"❌ Erro ao processar MPC: {e}") + + def _calcular_melhor_candidato(self, ponto_alvo, x, y, theta, visitados, contexto, comando_anterior, primeira_execucao): + try: + orient = self.gps_handler.calcular_orientacao(ponto_alvo, (x, y)) + delta_theta = (orient - theta + np.pi) % (2 * np.pi) - np.pi + erro_ori = abs(delta_theta) + angulo = np.clip(delta_theta, -np.radians(self.angulo_max_graus), np.radians(self.angulo_max_graus)) + + melhor_custo = float('inf') + melhor_tipo = TipoMovimentoDirecional.RodasDianteiras + melhor_angulo = angulo + + tipos_validos, angulos_candidatos = self._gerar_angulos_candidatos(angulo, erro_ori, contexto) + + velocidade = velocidade = contexto.get("Carro", {}).get("Velocidade", 0) + deslocamento = velocidade * self.dt + + ctx_vw = contexto.get("VisualWorker", {}) + vw_ativado = ctx_vw.get("Ativado", False) + vw_operante = ctx_vw.get("Operante", False) + com_matriz_custo = contexto.get("RegrasAtivas", {}).get("matriz_custo", False) + filtrar_matriz = vw_ativado and vw_operante and com_matriz_custo + + if filtrar_matriz: + matriz_custo = contexto.get("VisualWorker", {}).get("MatrizCusto", [[]]) + # 1. Analisa profundidade média por linha + from shared.utils import analisar_linhas_por_profundidade + linhas_info = analisar_linhas_por_profundidade(matriz_custo, "distancia_m", contexto.get("VisualWorker", {}).get("Camera", {}).get("FovH", 0.7)) + + for tipo in tipos_validos: + for angulo_testado in angulos_candidatos: + omega = self.gps_handler.calcular_omega(velocidade, angulo_testado, tipo) + theta_sim = theta + omega * self.dt + delta_theta_sim = (theta - theta_sim + np.pi) % (2 * np.pi) - np.pi + x_sim = x + (np.cos(theta_sim) * deslocamento) + y_sim = y + (np.sin(theta_sim) * deslocamento) + + custo = 0.0 + + if filtrar_matriz: + custo_espaco, matriz_concluida = self._simular_trajetoria_matriz_custo(matriz_custo, linhas_info, tipo, angulo_testado, deslocamento, delta_theta_sim) + custo += custo_espaco + + if custo > melhor_custo: + continue + + idx_alvo_sim = self._corrigir_pontos_visitados(x_sim, y_sim, deepcopy(visitados)) + + ponto_alvo_sim = self.pontos_info[idx_alvo_sim]["xy"] + orient_sim = self.gps_handler.calcular_orientacao((x_sim, y_sim), ponto_alvo_sim) + orient_sim = (orient_sim + np.pi) % (2 * np.pi) + erro_pos = np.linalg.norm([x_sim - ponto_alvo_sim[0], y_sim - ponto_alvo_sim[1]]) + erro_ori_sim = self._erro_angular(orient_sim, theta_sim) + pesos = self._calcular_pesos_movimento(contexto, erro_ori_sim) + peso_erro_pos, peso_erro_ori, peso_suavidade, peso_fator_re, peso_ideal, peso_movimento = pesos[0], pesos[1], pesos[2], pesos[3], pesos[4], pesos[5] + vetor_alvo = np.array(ponto_alvo_sim) - np.array([x_sim, y_sim]) + vetor_alvo_norm = vetor_alvo / np.linalg.norm(vetor_alvo) + vetor_movel = np.array([np.cos(theta_sim), np.sin(theta_sim)]) + cos_angulo = -np.dot(vetor_movel, vetor_alvo_norm) + delta_angulo = abs(angulo_testado - np.radians(comando_anterior.get("angulo", 0))) + custo_pos = erro_pos * peso_erro_pos + custo_ori = ((erro_ori_sim / np.pi) * erro_pos) * peso_erro_ori + custo_suavidade = ((delta_angulo / np.pi) * erro_pos) * peso_suavidade + custo_tipo_movimento = peso_movimento[tipo] # * erro_pos + erro_re = (1 - cos_angulo) if cos_angulo > 0 else cos_angulo + custo_re = -erro_re * erro_pos * peso_fator_re + erro_ideal = abs(angulo_testado - angulo) + custo_ideal = peso_ideal * erro_ideal + custo_mapa = (custo_pos + custo_ori + custo_suavidade + custo_tipo_movimento + custo_ideal + custo_re) / 10.0 + custo += custo_mapa + + #if primeira_execucao: + # mostrar_log(f"{tipo.name} {round(np.degrees(angulo_testado), 2)}°, d: {round(deslocamento, 2)} m - iPA: {idx_alvo_sim}, Custo: {round(custo, 4)}, Omega: {round(np.degrees(omega), 2)}°, Theta: {round(np.degrees(theta_sim), 2)}°, Ori: {round(np.degrees(orient_sim), 2)}°, E Ori: {round(np.degrees(erro_ori_sim), 3)}°, E Pos: {round(erro_pos, 3)}, F Re: {round(erro_re, 3)}, d°: {round(np.degrees(delta_angulo), 2)}, CE: {round(custo_espaco, 4)}, CP: {round(custo_pos, 4)}, CO: {round(custo_ori, 4)}, CM: {round(custo_tipo_movimento, 4)}, CS: {round(custo_suavidade, 4)}, CR: {round(custo_re, 4)}, CI: {round(custo_ideal, 4)}") + if custo < melhor_custo: + melhor_custo = custo + melhor_tipo = tipo + melhor_angulo = angulo_testado + + #if primeira_execucao: + # mostrar_log(f"COMANDO: {tipo.name}, Angulo ideal: {round(np.degrees(angulo), 2)}, Melhor angulo: {round(np.degrees(melhor_angulo), 2)}, Custo: {round(melhor_custo, 3)}") + + mostrar_log(f"z: {self.y_ant:.3f} m, x: {self.x_ant:.3f} m, COMANDO: {melhor_tipo.name}, Angulo ideal: {round(np.degrees(angulo), 2)}, Melhor angulo: {round(np.degrees(melhor_angulo), 2)}, Custo: {round(melhor_custo, 3)}") + return melhor_angulo, melhor_tipo + + except Exception as e: + mostrar_log(f"❌ Erro ao calcular melhor candidato: {e}") + + + + # MPC COM CANDIDATO FIXO DURANTE O HORIZONTE + + def compute_fixo(self, contexto, comando_anterior): + try: + agora = time.time() + self.dt = 0.2 if self.ultima_atualizacao == None else agora - self.ultima_atualizacao + self.ultima_atualizacao = agora + comando = self._processar_mpc_fixo(contexto, comando_anterior) + mostrar_log(f"Estatisticas MPC: dt = {round(self.dt, 2)} s, freq = {round(1.0 / self.dt, 2)} Hz") + return comando + + except Exception as e: + mostrar_log(f"❌ Erro ao processar compute MPC: {e}") + + def _processar_mpc_fixo(self, contexto, comando_anterior): + try: + if not self.pontos_info: + return None + GPS = contexto.get("GPS", {}) + pos_lat = GPS.get("Latitude", 0) + pos_lon = GPS.get("Longitude", 0) + theta = np.radians(GPS.get("AnguloCarro", 0)) + velocidade = contexto.get("Carro", {}).get("Velocidade", 0) + + x, y = self.gps_handler.converter_latlon_para_xz(pos_lat, pos_lon) + + x_temp, y_temp, theta_temp = x, y, theta + + distancia_m = velocidade * self.dt + + if distancia_m > 0: + pontos_horizonte = int(self.horizonte / (distancia_m)) + else: + pontos_horizonte = 10 + + pontos_horizonte = max(10, min(pontos_horizonte, 50)) # entre 10 e 50 pontos + + self.horizonte_passos = pontos_horizonte + + angulo_final = None + tipo_final = None + self.x_ant = 0 + self.y_ant = 0 + self.dist_acumulada = 0 + idx_alvo = contexto.get("Carro", {}).get("IdxProximoPonto", 0) + if idx_alvo >= len(self.pontos_info): + angulo_final, tipo_final = comando_parado() + else: + ponto_info = self.pontos_info[idx_alvo] + ponto_alvo = ponto_info["xy"] + angulo_final, tipo_final, sim = self._calcular_melhor_candidato_horizonte_fixo(ponto_alvo, x_temp, y_temp, theta_temp, deepcopy(self.visitados_execucao), contexto, comando_anterior) + + comando_anterior["angulo"] = angulo_final + comando_anterior["tipo"] = tipo_final + + # Converte os pontos simulados para lat/lon + simulacao_latlon = [] + for sx, sy in sim: + dlat = sy / self.gps_handler.raio_terra + dlon = sx / (self.gps_handler.raio_terra * np.cos(np.radians(self.lat0))) + lat = self.lat0 + np.degrees(dlat) + lon = self.lon0 + np.degrees(dlon) + simulacao_latlon.append([lat, lon]) + + return { + "angulo": np.degrees(angulo_final), + "tipo": tipo_final, + "simulacao": simulacao_latlon + } + + except Exception as e: + mostrar_log(f"❌ Erro ao processar MPC: {e}") + + def _calcular_melhor_candidato_horizonte_fixo(self, ponto_alvo, x, y, theta, visitados, contexto, comando_anterior): + try: + orient = self.gps_handler.calcular_orientacao(ponto_alvo, (x, y)) + delta_theta = (orient - theta + np.pi) % (2 * np.pi) - np.pi + erro_ori = abs(delta_theta) + angulo = np.clip(delta_theta, -np.radians(self.angulo_max_graus), np.radians(self.angulo_max_graus)) + + melhor_custo = float('inf') + melhor_tipo = TipoMovimentoDirecional.RodasDianteiras + melhor_angulo = angulo + + tipos_validos, angulos_candidatos = self._gerar_angulos_candidatos(angulo, erro_ori, contexto) + velocidade = contexto.get("Carro", {}).get("Velocidade", 0) + manobrando = contexto.get("Carro", {}).get("ManobrandoEntreRuas", False) + distancia_m = velocidade * self.dt + matriz_custo = contexto.get("VisualWorker", {}).get("MatrizCusto", [[]]) + + simulacoes = {} + + # 1. Analisa profundidade média por linha + from shared.utils import analisar_linhas_por_profundidade + linhas_info = analisar_linhas_por_profundidade(matriz_custo, "distancia_m", contexto.get("VisualWorker", {}).get("Camera", {}).get("FovH", 0.7)) + + for tipo in tipos_validos: + for angulo_testado in angulos_candidatos: + custo_total = 0.0 + x_sim, y_sim, theta_sim = x, y, theta + x_plot, y_plot = x, y + self.x_ant, self.y_ant = 0.0, 0.0 + chave = (tipo, angulo_testado) + simulacoes[chave] = [] + comando_anterior_copy = deepcopy(comando_anterior) + omega = self.gps_handler.calcular_omega(velocidade, angulo_testado, tipo) + for _ in range(self.horizonte_passos): + theta_sim += omega * self.dt + x_sim += np.cos(theta_sim) * distancia_m + y_sim += np.sin(theta_sim) * distancia_m + + + theta_plot = (-theta_sim) + np.radians(90) + theta_ant = theta_sim - omega * self.dt + delta_theta = (theta_ant - theta_sim + np.pi) % (2 * np.pi) - np.pi + x_plot += velocidade * np.cos(theta_plot) * self.dt + y_plot += velocidade * np.sin(theta_plot) * self.dt + simulacoes[chave].append((x_plot, y_plot)) + + + delta_theta_sim = (theta - theta_sim + np.pi) % (2 * np.pi) - np.pi + custo_espaco, matriz_concluida = self._simular_trajetoria_matriz_custo(matriz_custo, linhas_info, tipo, angulo_testado, distancia_m, delta_theta_sim) + + custo_total += custo_espaco + + self.x_ant += distancia_m * np.sin(-delta_theta_sim) + self.y_ant += distancia_m * np.cos(delta_theta_sim) + + # Avaliação final da posição no final da simulação + idx_alvo_sim = self._corrigir_pontos_visitados(x_sim, y_sim, deepcopy(visitados)) + ponto_alvo_sim = self.pontos_info[idx_alvo_sim]["xy"] + orient_sim = self.gps_handler.calcular_orientacao((x_sim, y_sim), ponto_alvo_sim) + orient_sim = (orient_sim + np.pi) % (2 * np.pi) + + erro_pos = np.linalg.norm([x_sim - ponto_alvo_sim[0], y_sim - ponto_alvo_sim[1]]) + erro_ori_sim = self._erro_angular(orient_sim, theta_sim) + + peso_erro_pos, peso_erro_ori, peso_suavidade, peso_fator_re, peso_movimento, peso_ideal = self._calcular_pesos_movimento(contexto, erro_ori_sim, erro_pos) + + vetor_alvo = np.array(ponto_alvo_sim) - np.array([x_sim, y_sim]) + vetor_alvo_norm = vetor_alvo / np.linalg.norm(vetor_alvo) + vetor_movel = np.array([np.cos(theta_sim), np.sin(theta_sim)]) + cos_angulo = -np.dot(vetor_movel, vetor_alvo_norm) + delta_angulo = abs(angulo_testado - comando_anterior_copy.get("angulo", 0)) + + custo_pos = erro_pos * peso_erro_pos + custo_ori = ((erro_ori_sim / np.pi) * erro_pos) * peso_erro_ori + custo_suavidade = ((delta_angulo / np.pi) * erro_pos) * peso_suavidade + custo_tipo_movimento = peso_movimento[tipo] + erro_re = (1 - cos_angulo) if cos_angulo > 0 else cos_angulo + custo_re = -erro_re * erro_pos * peso_fator_re + erro_ideal = abs(angulo_testado - angulo) + custo_ideal = peso_ideal * erro_ideal + custo_mapa = (custo_pos + custo_ori + custo_suavidade + custo_tipo_movimento + custo_ideal + custo_re) / 10.0 + if manobrando: + custo_mapa *= 20.0 + + custo_total += custo_mapa + + comando_anterior_copy["angulo"] = angulo_testado + comando_anterior_copy["tipo"] = tipo + + if matriz_concluida: + break + + if custo_total < melhor_custo: + melhor_custo = custo_total + melhor_tipo = tipo + melhor_angulo = angulo_testado + + melhor_trajetoria = simulacoes.get((melhor_tipo, melhor_angulo), []) + mostrar_log(f"📈 HORIZONTE FIXO | COMANDO: {melhor_tipo.name}, Angulo ideal: {round(np.degrees(angulo), 2)}, Melhor angulo: {round(np.degrees(melhor_angulo), 2)}, Custo Total: {round(melhor_custo, 4)}") + return melhor_angulo, melhor_tipo, melhor_trajetoria + + except Exception as e: + mostrar_log(f"❌ Erro no MPC de horizonte fixo: {e}") + + + + # MPC AVANCADO COM PREVISAO DE HORIZONTE + + def compute_horizonte(self, contexto, comando_anterior): + agora = time.time() + self.dt = 0.2 if self.ultima_atualizacao == None else agora - self.ultima_atualizacao + self.ultima_atualizacao = agora + + GPS = contexto.get("GPS", {}) + pos_lat = GPS.get("Latitude", 0) + pos_lon = GPS.get("Longitude", 0) + theta = np.radians(GPS.get("AnguloCarro", 0)) + x, y = self._latlon_to_xy(pos_lat, pos_lon, self.lat0, self.lon0) + angulo_final, tipo_final = self._calcular_melhor_candidato_horizonte( + x, y, theta, + self.visitados_execucao.copy(), + contexto, + comando_anterior + ) + comando = { + "angulo": np.degrees(angulo_final), + "tipo": tipo_final, + "simulacao": [] + } + return comando + + def _calcular_melhor_candidato_horizonte(self, x, y, theta, visitados_sim, contexto, comando_anterior): + idx_alvo = self._corrigir_pontos_visitados(x, y, visitados_sim) + if idx_alvo >= len(self.pontos_info): + return comando_parado() + + ponto_info = self.pontos_info[idx_alvo] + ponto_alvo = ponto_info["xy"] + orient = self.gps_handler.calcular_orientacao(ponto_alvo, (x, y)) + delta_theta = (orient - theta + np.pi) % (2 * np.pi) - np.pi + erro_ori = abs(delta_theta) + angulo_ideal = np.clip(delta_theta, -np.radians(self.angulo_max_graus), np.radians(self.angulo_max_graus)) + + melhor_custo = float("inf") + melhor_tipo = None + melhor_angulo = None + + tipos_validos, angulos_candidatos = self._gerar_angulos_candidatos(angulo_ideal, erro_ori, contexto) + + for tipo in tipos_validos: + for angulo in angulos_candidatos: + #custo, simulacoes = self.simular_passos_recursivo(x, y, theta, self.horizonte_passos, 0, visitados_sim.copy(), contexto, 0, melhor_custo, comando_anterior, angulo_ideal, erro_ori) + custo = self.simular_passos(x, y, theta, self.horizonte_passos, visitados_sim.copy(), contexto, { "angulo": angulo, "tipo": tipo }, 0, melhor_custo, comando_anterior, angulo_ideal) + #print(f"Tipo: {tipo.name}, Angulo: {np.degrees(angulo)}, Simulacoes: {simulacoes}: Custo: {custo}") + #input("⏸️ Pressione Enter para continuar...") + if custo < melhor_custo: + melhor_custo = custo + melhor_tipo = tipo + melhor_angulo = angulo + + return melhor_angulo, melhor_tipo + + def simular_passos(self, x, y, theta, passos_restantes, visitados_sim, contexto, comando, custo_acumulado, custo_melhor, comando_anterior, angulo_ideal): + if passos_restantes == 0: + return custo_acumulado + + tipo = comando["tipo"] + angulo_testado = comando["angulo"] + velocidade = contexto["Carro"].get("Velocidade", 0) + dt = self.dt + + idx_alvo = self._corrigir_pontos_visitados(x, y, visitados_sim.copy()) + ponto_alvo = self.pontos_info[idx_alvo]["xy"] + orient = self.gps_handler.calcular_orientacao(ponto_alvo, (x, y)) + delta_theta = (orient - theta + np.pi) % (2 * np.pi) - np.pi + angulo = np.clip(delta_theta, -np.radians(self.angulo_max_graus), np.radians(self.angulo_max_graus)) + + omega = self.gps_handler.calcular_omega(velocidade, angulo_testado, tipo) + theta_sim = theta + omega * dt + x_sim = x - velocidade * np.cos(theta_sim) * dt + y_sim = y - velocidade * np.sin(theta_sim) * dt + + idx_alvo_sim = self._corrigir_pontos_visitados(x_sim, y_sim, visitados_sim) + ponto_alvo_sim = self.pontos_info[idx_alvo_sim]["xy"] + if np.linalg.norm([x_sim - ponto_alvo_sim[0], y_sim - ponto_alvo_sim[1]]) < self.pontos_info[idx_alvo_sim].get("distanciaMargem", 0.7): + visitados_sim[idx_alvo_sim] = True + orient_sim = self.gps_handler.calcular_orientacao(ponto_alvo_sim, (x_sim, y_sim)) + + erro_pos = np.linalg.norm([x_sim - ponto_alvo_sim[0], y_sim - ponto_alvo_sim[1]]) + erro_ori_sim = self._erro_angular(orient_sim, theta_sim) + + vetor_alvo = np.array([x_sim, y_sim]) - np.array(ponto_alvo_sim) + vetor_alvo_norm = vetor_alvo / np.linalg.norm(vetor_alvo) + vetor_movel = np.array([np.cos(theta_sim), np.sin(theta_sim)]) + cos_angulo = -np.dot(vetor_movel, vetor_alvo_norm) + + delta_angulo = abs(angulo_testado - comando_anterior.get("angulo", 0)) + + peso_erro_pos, peso_erro_ori, peso_suavidade, peso_fator_re, peso_movimento, peso_ideal = self._calcular_pesos_movimento(contexto, erro_ori_sim, erro_pos) + + custo_pos = erro_pos * peso_erro_pos + custo_ori = ((erro_ori_sim / np.pi) * erro_pos) * peso_erro_ori + custo_suavidade = ((delta_angulo / np.pi) * erro_pos) * peso_suavidade + erro_re = (1 - cos_angulo) if cos_angulo > 0 else cos_angulo + custo_re = -erro_re * erro_pos * peso_fator_re + if passos_restantes == self.horizonte_passos: + custo_tipo_movimento = peso_movimento[tipo] + else: + custo_tipo_movimento = 0 + + erro_ideal = abs(angulo_testado - angulo) + custo_ideal = peso_ideal * erro_ideal + + custo = custo_pos + custo_ori + custo_suavidade + custo_tipo_movimento + custo_ideal + custo_re + + custo_acumulado += custo + + print(f"{passos_restantes} {tipo.name} {round(np.degrees(angulo_testado), 2)}°, iPA: {idx_alvo_sim}, Custo: {round(custo_acumulado, 4)}, Omega: {round(np.degrees(omega), 2)}°, Theta: {round(np.degrees(theta_sim), 2)}°, Ori: {round(np.degrees(orient_sim), 2)}°, E Pos: {round(erro_pos, 3)}, E Ori: {round(np.degrees(erro_ori_sim), 3)}°, F Re: {round(erro_re, 3)}, d°: {round(np.degrees(delta_angulo), 2)}, CP: {round(custo_pos, 4)}, CO: {round(custo_ori, 4)}, CM: {round(custo_tipo_movimento, 4)}, CS: {round(custo_suavidade, 4)}, CR: {round(custo_re, 4)}, CI: {round(custo_ideal, 4)}") + + if custo_acumulado > custo_melhor: + return float('inf') + + novo_comando_anterior = { + "angulo": comando["angulo"], + "tipo": comando["tipo"] + } + + return self.simular_passos(x_sim, y_sim, theta_sim, passos_restantes - 1, visitados_sim, contexto, comando, custo_acumulado, custo_melhor, novo_comando_anterior, angulo_ideal) + + def simular_passos_recursivo(self, x, y, theta, passos_restantes, simulacoes, visitados_sim, contexto, custo_acumulado, custo_melhor, comando_anterior, angulo_ideal, erro_ori): + if passos_restantes == 0: + return custo_acumulado, simulacoes + + velocidade = contexto["Carro"].get("Velocidade", 0) + dt = self.dt + melhor_custo = float("inf") + + tipos_validos, angulos_candidatos = self._gerar_angulos_candidatos(angulo_ideal, erro_ori, contexto) + + for tipo in tipos_validos: + for angulo in angulos_candidatos: + #print(f"Simulando {tipo.name} {np.degrees(angulo)}") + omega = self.gps_handler.calcular_omega(velocidade, angulo, tipo) + theta_temp = theta + omega * dt + x_temp = x + velocidade * np.cos(theta) * dt + y_temp = y + velocidade * np.sin(theta) * dt + visitados_temp = visitados_sim.copy() + + idx_alvo = self._corrigir_pontos_visitados(x_temp, y_temp, visitados_temp) + if idx_alvo >= len(self.pontos_info): + continue + + ponto_alvo = self.pontos_info[idx_alvo]["xy"] + if np.linalg.norm([x_temp - ponto_alvo[0], y_temp - ponto_alvo[1]]) < self.pontos_info[idx_alvo].get("distanciaMargem", 0.7): + visitados_temp[idx_alvo] = True + + orient_sim = self.gps_handler.calcular_orientacao(ponto_alvo, (x_temp, y_temp)) + erro_ori_sim = self._erro_angular(orient_sim, theta_temp) + erro_pos = np.linalg.norm([x_temp - ponto_alvo[0], y_temp - ponto_alvo[1]]) + + vetor_alvo = np.array([x_temp, y_temp]) - np.array(ponto_alvo) + vetor_alvo_norm = vetor_alvo / np.linalg.norm(vetor_alvo) + vetor_movel = np.array([np.cos(theta_temp), np.sin(theta_temp)]) + cos_angulo = -np.dot(vetor_movel, vetor_alvo_norm) + erro_re = (1 - cos_angulo) if cos_angulo > 0 else cos_angulo + + delta_angulo = abs(angulo - comando_anterior.get("angulo", 0)) + peso_erro_pos, peso_erro_ori, peso_suavidade, peso_fator_re, peso_movimento, peso_ideal = self._calcular_pesos_movimento(contexto, erro_ori_sim, erro_pos) + + custo_pos = erro_pos * peso_erro_pos + custo_ori = ((erro_ori_sim / np.pi) * erro_pos) * peso_erro_ori + custo_suav = ((delta_angulo / np.pi) * erro_pos) * peso_suavidade + custo_mov = peso_movimento[tipo] + custo_re = -erro_re * erro_pos * peso_fator_re + custo_ideal = peso_ideal * abs(angulo - angulo_ideal) + + custo_total = custo_acumulado + custo_pos + custo_ori + custo_suav + custo_mov + custo_re + custo_ideal + + if custo_total > custo_melhor: + continue + + custo, simulacoes = self.simular_passos_recursivo( + x_temp, y_temp, theta_temp, passos_restantes - 1, simulacoes + 1, visitados_temp, contexto, + custo_total, custo_melhor, {"angulo": angulo, "tipo": tipo}, angulo_ideal, erro_ori + ) + + #print(f"{tipo.name} {round(np.degrees(angulo), 2)}°, iPA: {idx_alvo}, Custo: {round(custo_total, 4)}, Omega: {round(np.degrees(omega), 2)}°, Theta: {round(np.degrees(theta), 2)}°, Ori: {round(np.degrees(orient_sim), 2)}°, E Pos: {round(erro_pos, 3)}, E Ori: {round(np.degrees(erro_ori_sim), 3)}°, F Re: {round(erro_re, 3)}, d°: {round(np.degrees(delta_angulo), 2)}, CP: {round(custo_pos, 4)}, CO: {round(custo_ori, 4)}, CM: {round(custo_mov, 4)}, CS: {round(custo_suav, 4)}, CR: {round(custo_re, 4)}, CI: {round(custo_ideal, 4)}") + + if custo < melhor_custo: + melhor_custo = custo + + return melhor_custo, simulacoes + + + + # MPC COM GERACAO DE CANDIDATOS INTELIGENTE + + def compute_inteligente(self, contexto, comando_anterior): + try: + agora = time.time() + self.dt = 0.2 if self.ultima_atualizacao == None else agora - self.ultima_atualizacao + self.ultima_atualizacao = agora + comando = self._processar_mpc_inteligente(contexto, comando_anterior) + mostrar_log(f"Estatisticas MPC: dt = {round(self.dt, 2)} s, freq = {round(1.0 / self.dt, 2)} Hz") + return comando + + except Exception as e: + mostrar_log(f"❌ Erro ao processar compute MPC: {e}") + + def _processar_mpc_inteligente(self, contexto, comando_anterior): + try: + if not self.pontos_info: + return None + GPS = contexto.get("GPS", {}) + pos_lat = GPS.get("Latitude", 0) + pos_lon = GPS.get("Longitude", 0) + theta = np.radians(GPS.get("AnguloCarro", 0)) + velocidade = contexto.get("Carro", {}).get("Velocidade", 0) + + x, y = self._latlon_to_xy(pos_lat, pos_lon, self.lat0, self.lon0) + + x_temp, y_temp, theta_temp = x, y, theta + + distancia_m = velocidade * self.dt + + self.horizonte = 1.14 + + if distancia_m > 0: + pontos_horizonte = min(int(self.horizonte / (distancia_m)), 50) + else: + pontos_horizonte = 1 + + self.horizonte_passos = pontos_horizonte + #self.horizonte_passos = 5 + + angulo_final = None + tipo_final = None + #self.x_ant = 0 + #self.y_ant = 0 + #self.dist_acumulada = 0 + idx_alvo = contexto.get("Carro", {}).get("IdxProximoPonto", 0) + if idx_alvo >= len(self.pontos_info): + angulo_final, tipo_final = comando_parado() + else: + ponto_info = self.pontos_info[idx_alvo] + ponto_alvo = ponto_info["xy"] + idx_alvo_atual = self._corrigir_pontos_visitados(x, y, self.visitados_execucao, idx_alvo) + print(f"idx_alvo: {idx_alvo}, idx_alvo_visitados: {idx_alvo_atual}") + angulo_final, tipo_final, sim = self._calcular_melhor_candidato_inteligente(ponto_alvo, x_temp, y_temp, theta_temp, self.visitados_execucao, contexto, comando_anterior) + + comando_anterior["angulo"] = angulo_final + comando_anterior["tipo"] = tipo_final + + # Converte os pontos simulados para lat/lon + simulacao_latlon = [] + for sx, sy in sim: + dlat = sy / self.gps_handler.raio_terra + dlon = sx / (self.gps_handler.raio_terra * np.cos(np.radians(self.lat0))) + lat = self.lat0 + np.degrees(dlat) + lon = self.lon0 + np.degrees(dlon) + simulacao_latlon.append([lat, lon]) + + return { + "angulo": np.degrees(angulo_final), + "tipo": tipo_final, + "simulacao": simulacao_latlon + } + + except Exception as e: + mostrar_log(f"❌ Erro ao processar MPC: {e}") + + def _calcular_melhor_candidato_inteligente(self, ponto_alvo, x, y, theta, visitados, contexto, comando_anterior): + try: + orient = self._calcular_orientacao(ponto_alvo, (x, y)) + delta_theta = (orient - theta + np.pi) % (2 * np.pi) - np.pi + erro_ori = abs(delta_theta) + angulo = np.clip(delta_theta, -np.radians(self.angulo_max_graus), np.radians(self.angulo_max_graus)) + + melhor_custo = float('inf') + melhor_tipo = TipoMovimentoDirecional.RodasDianteiras + melhor_angulo = angulo + + tipos_validos, angulos_candidatos = self._gerar_angulos_candidatos_inteligente(angulo, erro_ori, contexto) + velocidade = contexto.get("Carro", {}).get("Velocidade", 0) + distancia_m = velocidade * self.dt + #manobrando = contexto.get("Carro", {}).get("ManobrandoEntreRuas", False) + #matriz_custo = contexto.get("VisualWorker", {}).get("MatrizCusto", [[]]) + + simulacoes = {} + + # 1. Analisa profundidade média por linha + #linhas_info = analisar_linhas_por_profundidade(matriz_custo, "distancia_m", contexto.get("VisualWorker", {}).get("Camera", {}).get("FovH", 0.7)) + + for tipo in tipos_validos: + for angulo_testado in angulos_candidatos: + custo_total = 0.0 + x_sim, y_sim, theta_sim = x, y, theta + self.x_ant, self.y_ant = 0.0, 0.0 + chave = (tipo, angulo_testado) + simulacoes[chave] = [] + comando_anterior_copy = deepcopy(comando_anterior) + pontos_visitados = deepcopy(visitados) + omega = self._calcular_omega(velocidade, angulo_testado, tipo) + + for passo in range(self.horizonte_passos): + old_theta = theta_sim + theta_sim += omega * self.dt + x_sim += np.sin(theta_sim) * distancia_m + y_sim += np.cos(theta_sim) * distancia_m + + simulacoes[chave].append((x_sim, y_sim)) + + #delta_theta = (old_theta - theta_sim + np.pi) % (2 * np.pi) - np.pi + #largura = contexto.get("Equipamento", {}).get("largura", 0.85) + #custo_espaco, matriz_concluida = self._simular_trajetoria_matriz_custo(matriz_custo, linhas_info, tipo, angulo_testado, distancia_m, delta_theta, largura) + #custo_total += custo_espaco + #self.x_ant += distancia_m * np.sin(-delta_theta) + #self.y_ant += distancia_m * np.cos(delta_theta) + + # Avaliação final da posição no final da simulação + idx_alvo_sim = self._corrigir_pontos_visitados(x_sim, y_sim, pontos_visitados) + ponto_alvo_sim = self.pontos_info[idx_alvo_sim]["xy"] + orient_sim = self._calcular_orientacao((x_sim, y_sim), ponto_alvo_sim) + orient_sim = (orient_sim + np.pi) % (2 * np.pi) + + erro_pos = np.linalg.norm([x_sim - ponto_alvo_sim[0], y_sim - ponto_alvo_sim[1]]) + erro_ori_sim = self._erro_angular(orient_sim, theta_sim) + + peso_erro_pos, peso_erro_ori, peso_suavidade, peso_fator_re, peso_movimento, peso_ideal = self._calcular_pesos_movimento(contexto, erro_ori_sim, erro_pos) + + vetor_alvo = np.array(ponto_alvo_sim) - np.array([x_sim, y_sim]) + vetor_alvo_norm = vetor_alvo / np.linalg.norm(vetor_alvo) + vetor_movel = np.array([np.cos(theta_sim), np.sin(theta_sim)]) + cos_angulo = -np.dot(vetor_movel, vetor_alvo_norm) + delta_angulo = abs(angulo_testado - comando_anterior_copy.get("angulo", 0)) + + custo_pos = erro_pos * peso_erro_pos + custo_ori = ((erro_ori_sim / np.pi) * erro_pos) * peso_erro_ori + custo_suavidade = ((delta_angulo / np.pi) * erro_pos) * peso_suavidade + custo_tipo_movimento = peso_movimento[tipo] + erro_re = (1.0 - cos_angulo) if cos_angulo > 0 else cos_angulo + custo_re = -erro_re * erro_pos * peso_fator_re + erro_ideal = abs(angulo_testado - angulo) + custo_ideal = peso_ideal * erro_ideal + custo_mapa = (custo_pos + custo_ori + custo_suavidade + custo_tipo_movimento + custo_ideal + custo_re) + + custo_total += custo_mapa + + comando_anterior_copy["angulo"] = angulo_testado + comando_anterior_copy["tipo"] = tipo + + #if matriz_concluida: + # break + + #print(f"Passo: {passo}, Alvo: {idx_alvo_sim}, Distancia: {((passo + 1) * distancia_m):.2f} m, Theta: {theta_sim:.2f}°, DX: {(x - x_sim):.2f} m, DY: {(y - y_sim):.2f} m, y_sim: {y_sim:.4f}, x_sim: {x_sim:.4f}, Tipo: {tipo.name}, Angulo: {np.degrees(angulo_testado):.2f}, custo: {custo_total}") + #mostrar_log(f"Omega: {np.degrees(omega):.2f}°, Ori: {np.degrees(orient_sim):.2f}°, E Ori: {np.degrees(erro_ori_sim):.3f}°, E Pos: {erro_pos:.3f}, F Re: {erro_re:.3f}, d°: {np.degrees(delta_angulo):.2f}, CM: {custo_mapa:.4f}, CP: {custo_pos:.4}, CO: {custo_ori:.4f}, CT: {custo_tipo_movimento:.4f}, CS: {custo_suavidade:.4f}, CR: {custo_re:.4f}, CI: {custo_ideal:.4f}") + + if custo_total < melhor_custo: + melhor_custo = custo_total + melhor_tipo = tipo + melhor_angulo = angulo_testado + + melhor_trajetoria = simulacoes.get((melhor_tipo, melhor_angulo), []) + mostrar_log(f"📈 HORIZONTE FIXO INTELIGENTE | COMANDO: {melhor_tipo.name}, Angulo ideal: {np.degrees(angulo):.2f}, Melhor angulo: {np.degrees(melhor_angulo):.2f}, Custo Total: {melhor_custo:.4f}") + return melhor_angulo, melhor_tipo, melhor_trajetoria + + except Exception as e: + mostrar_log(f"❌ Erro no MPC de horizonte fixo inteligente: {e}") + + def _gerar_angulos_candidatos_inteligente(self, angulo_ideal_rad, erro_ori, contexto): + def clamp(v, vmin, vmax): + return max(vmin, min(vmax, v)) + + CARRO = contexto.get("Carro", {}) + status = CARRO.get("Status", 0) + manobrando = CARRO.get("ManobrandoEntreRuas", False) + + if manobrando or status in [ StatusCarroMapa.EntrandoRua, StatusCarroMapa.SaindoRua, StatusCarroMapa.Manobrando ]: + tipos_validos = [ TipoMovimentoDirecional.MovimentoArco ] + else: + tipos_validos = [ TipoMovimentoDirecional.RodasDianteiras, TipoMovimentoDirecional.MovimentoArco ] + + erro_ori_deg = np.degrees(erro_ori) + angulo_ideal_deg = np.degrees(angulo_ideal_rad) + + # Passo varia conforme erro angular + if erro_ori_deg > 10: + passo = 5.0 + faixa = 15.0 + elif erro_ori_deg > 5: + passo = 2.5 + faixa = 7.5 + else: + passo = 1.5 + faixa = 4.5 + + # Geração em torno do ideal + candidatos_graus = list(np.arange(angulo_ideal_deg - faixa, angulo_ideal_deg + faixa + 0.1, passo)) + + # Clamping e eliminação de duplicatas + candidatos_radianos = sorted(set( + np.radians(clamp(g, -self.angulo_max_graus, self.angulo_max_graus)) + for g in (candidatos_graus) + )) + + #print(f"Ideal: {round(angulo_ideal_deg, 2)}°, Erro Ori: {round(erro_ori_deg, 2)}°, Obstáculos: {len(obstaculos)}, Candidatos: {[round(np.degrees(a), 2) for a in candidatos_radianos]}") + return tipos_validos, candidatos_radianos + + + + # MPC COM GERACAO DE CANDIDATOS RECEDING + + def compute_receding(self, contexto, comando_anterior): + try: + agora = time.time() + self.dt = 0.25 if self.ultima_atualizacao is None else agora - self.ultima_atualizacao + #self.dt = max(min(self.dt, 0.4), 0.1) + _dt = self.dt + freq_tick = 1.0 / _dt + #self.dt = 0.5 + self.ultima_atualizacao = agora + comando = self._processar_mpc_receding(contexto, comando_anterior) + + t_exec = max((time.time() - self.ultima_atualizacao), 1e-3) + if comando['erro']: + mostrar_log(f"🧭 Direcional MPC | Erro: {comando['erro']} | Parada: {comando['parada_necessaria']} | Movimento: {TipoMovimentoDirecional(comando['tipo']).name} | Ângulo: {comando['angulo']}° | Horizonte: {len(comando['simulacao'])} | dt: {_dt:.2f} s | freq = {(freq_tick):.2f} Hz | t_exec: {t_exec:.2} s | f_exec: {(1.0 / t_exec):.2f} Hz") + return comando + + except Exception as e: + mostrar_log(f"❌ Erro ao processar compute MPC: {e}") + + def _processar_mpc_receding(self, contexto, comando_anterior): + t_init = time.time() + + now = time.perf_counter + t0 = now() + SLA_MS = getattr(self, "_mpc_sla_ms", 200.0) # 200 ms ⇒ 5 Hz + HEAD_MS = getattr(self, "_mpc_headroom_ms", 25.0) + deadline = t0 + (SLA_MS - HEAD_MS) / 1000.0 + + try: + if not self.pontos_info: + return None + + GPS = contexto.get("GPS", {}) + pos_lat = GPS.get("Latitude", 0) + pos_lon = GPS.get("Longitude", 0) + pos_theta = GPS.get("AnguloCarro", 0) + pos_passos_atraso = GPS.get("PassosAtraso", 0) + pos_timestamp = GPS.get("Timestamp", 0.0) + pos_latencia = now() - pos_timestamp + + LAT_MAX = 2.0 + if pos_latencia >= LAT_MAX: + mostrar_log(f"[GPS] Grande latencia entre coordenadas detectado ({pos_latencia:.3f}/{LAT_MAX}): parando por fallback") + return self._comando_fallback_hot_stop(comando_anterior, latencia=pos_latencia) + if (pos_passos_atraso > 3): + mostrar_log(f"[GPS] Atraso detectado de {(pos_passos_atraso / 3.0):.1f} passos, {pos_latencia:.3f} s") + + x, y = self.gps_handler.converter_latlon_para_xz(pos_lat, pos_lon) + theta = np.radians(pos_theta) + x0, y0, t0 = x, y, theta + velocidade = contexto.get("Carro", {}).get("Velocidade", 0) + status_carro = contexto.get("Carro", {}).get("Status", StatusCarroMapa.Parado.value) + idx_proximo_ponto_real = contexto.get("Carro", {}).get("IdxProximoPonto", 0) + idx_ponto_alvo = contexto.get("Carro", {}).get("IdxPontoAlvo", 0) + Nmax = 20 if status_carro == StatusCarroMapa.CaminhandoRua.value else 40 + + # Comando de hold (se não tiver buffer de comandos aplicados) + u_hold = { + "tipo": comando_anterior.get("tipo"), + "angulo": np.radians(comando_anterior.get("angulo", 0.0)), + "v": velocidade, + "omega": self.gps_handler.calcular_omega(velocidade, np.radians(comando_anterior.get("angulo", 0.0)), comando_anterior.get("tipo")) + } + # Opcional: sequência real de comandos aplicados durante a latência (se você tiver) + cmd_seq = None # ou algo como [(0.12, u1), (0.08, u2), ...] + x, y, theta, pts_visitados = self.corrigir_pose_por_latencia( + x, y, theta, self.visitados_execucao, + latency_s=min(pos_latencia, LAT_MAX), + dt=self.dt, + nova_posicao_fn=self._nova_posicao, + u_hold=u_hold, + cmd_seq=cmd_seq, + time_left_ms=lambda: (deadline - now()) * 1000.0, + calc_omega_fn=self.gps_handler.calcular_omega + ) + if self._verifica_tempo_maximo_execucao(t_init, f"corrigir_pose_latencia"): + return self._comando_fallback_hot_stop(comando_anterior) + idx_alvo_correcao = self._corrigir_pontos_visitados(x, y, pts_visitados, idx_proximo_ponto_real) # marca pontos antigos como visitados + + distancia_m = velocidade * self.dt + ultimo_ponto = idx_proximo_ponto_real >= len(self.pontos_info) + self.tempo_execucao_local = contexto.get("Carro", {}).get("TempoEntreComandos", 0.5) + if ultimo_ponto: + self.passos_horizonte_local = 1 + self.qtd_comandos_sucessivos = 3 + elif distancia_m > 0: + passos_total = min(int(self.horizonte / distancia_m), Nmax) + self.passos_horizonte_local = max(1, math.ceil(self.tempo_execucao_local / self.dt)) + self.qtd_comandos_sucessivos = max(1, passos_total // self.passos_horizonte_local) + else: + self.passos_horizonte_local = 0 + self.qtd_comandos_sucessivos = 0 + + t_0 = time.time() + angulo_anterior = np.radians(comando_anterior.get("angulo", 0)) + tipo_anterior = TipoMovimentoDirecional(comando_anterior.get("tipo", TipoMovimentoDirecional.RodasDianteiras.value)) + + simulacao_latlon = [] + t_2 = time.time() + + #mostrar_log(f"previsao_futura em {(t_1 - t_0):.4f}s, correcao_pontos_visitados em {(t_2 - t_1):.4f}") + + if ultimo_ponto: + angulo_final, tipo_final = comando_parado() + parada_necessaria = True + else: + candidatos_ativos = [{ + "x": x, "y": y, "theta": theta, + "custo": 0.0, + "comandos": [(tipo_anterior, angulo_anterior)], + "trajetoria": [], + "visitados": deepcopy(pts_visitados), + "inicial": True + }] + + t0 = time.time() + for passo in range(self.qtd_comandos_sucessivos): + #mostrar_log(f"simulacao_horizonte passo {passo}") + if self._verifica_tempo_maximo_execucao(t_init, f"simulacao_horizonte passo {passo}"): + return self._comando_fallback_hot_stop(comando_anterior) + t1 = time.time() + novos_candidatos = [] + for candidato in candidatos_ativos: + t1_0 = time.time() + x_atual, y_atual, theta_atual = candidato["x"], candidato["y"], candidato["theta"] + visitados = deepcopy(candidato["visitados"]) + cmd_anterior = { + "tipo": candidato["comandos"][-1][0], + "angulo": candidato["comandos"][-1][1], + } + idx_alvo = self._corrigir_pontos_visitados(x_atual, y_atual, visitados) + ponto_alvo = self.pontos_info[idx_alvo]["xy"] + t2 = time.time() + tipos, angs, custos_candidatos = self._gerar_angulos_candidatos_receding(ponto_alvo, (x_atual, y_atual, theta_atual), (x, y, theta), contexto) + t3 = time.time() + + #mostrar_log(f"corrigir_pontos_visitados em {(t2 - t1_0):.4f}s, gerar_angulos_candidatos em {(t3 - t2):.4f}s") + + def simular_e_gerar(candidato, tipo, angulo): + if self._verifica_tempo_maximo_execucao(t_init, f"simular_e_gerar, tipo {tipo.name}, angulo {np.degrees(angulo):.2f}"): + return self._comando_fallback_hot_stop(comando_anterior) + try: + custo, sim, valido = self._simular_passo(x_atual, y_atual, theta_atual, tipo, angulo, custos_candidatos, cmd_anterior, contexto, deepcopy(visitados)) + #print(f"{tipo.name} | angulo: {np.degrees(angulo):.2f} : Custo: {custo}") + x_f, y_f, theta_f = sim[-1] + if valido: + return { + "x": x_f, "y": y_f, "theta": theta_f, + "custo": candidato["custo"] + custo, + "comandos": candidato["comandos"] + [(tipo, angulo)], + "trajetoria": candidato["trajetoria"] + sim, + "visitados": deepcopy(visitados), + "inicial": False + } + else: + return None + except Exception as e: + mostrar_log(f"Erro ao simular e gerar passo para {tipo.name} | {np.degrees(angulo):.2f}: {e}") + return None + + futures = [self.executor.submit(simular_e_gerar, candidato, tipo, angulo) for tipo in tipos for angulo in angs] + try: + for fut in as_completed(futures, timeout=0.3): + try: + novo = fut.result(timeout=0.05) + if novo: + novos_candidatos.append(novo) + except Exception as e: + mostrar_log(f"⚠️ Timeout ou erro em future: {e}") + except Exception as e: + mostrar_log(f"Uma tarefa estourou timeout durante o processo de simular passo no horizonte: {e}") + + t4 = time.time() + # Filtro opcional: limitar para os N melhores candidatos + t5 = time.time() + candidatos_ativos = self._selecionar_melhores(novos_candidatos, N=5) + t6 = time.time() + candidatos_ativos = self._filtrar_por_margem_angular(candidatos_ativos, margem_graus=2.0) + t7 = time.time() + #mostrar_log(f"loop_candidatos em {(t5 - t1):.4f}s, melhores em {(t6 - t5):.4f}s, margem em {(t7 - t6):.4f}s") + #print("Melhores candidatos filtrados:") + #for i, c in enumerate(candidatos_ativos): + # comando = c["comandos"][-1] if c["comandos"] else ("-", 0) + # tipo = comando[0] + # angulo = np.degrees(comando[1]) + # print(f"{i+1}. Tipo: {tipo.name if hasattr(tipo, 'name') else tipo}, Angulo: {round(angulo, 2)}, Custo: {round(c['custo'], 4)}") + #t5 = time.time() + #print(f"Passo {passo} simulado em {t5 - t1} segundos") + + t100 = time.time() + #print(f"Simulacao completa: {t100 - t0} segundos") + candidatos_validos = [c for c in candidatos_ativos if not c.get("inicial", False)] + if len(candidatos_validos) > 0: + try: + melhor = min(candidatos_validos, key=lambda c: c["custo"]) + except Exception as e: + mostrar_log(f"Erro ao selecionar melhor candidato: {e}") + melhor = None + else: + melhor = None + #comandos_formatados = [(tipo.name, round(float(np.degrees(angulo)), 2)) for tipo, angulo in melhor["comandos"]] + #print(comandos_formatados) + #trajetoria_foramtada = [(round(float(_x), 2), round(float(_y), 2), round(float(np.degrees(_theta)), 2)) for _x, _y, _theta in melhor["trajetoria"]] + #print(trajetoria_foramtada) + #self.visualizador.atualizar(melhor["trajetoria"]) + parada_necessaria = melhor is None + if not parada_necessaria: + _comandos = melhor.get("comandos", []) + if len(_comandos) == 0: + pass + else: + idx_cmd = 0 if len(_comandos) == 1 else 1 + tipo_final, angulo_final = _comandos[idx_cmd] + simulacao_latlon = list(self.gps_handler.converter_trajetoria_para_latlon(melhor["trajetoria"]) or []) + simulacao_latlon.insert(0, (pos_lat, pos_lon, pos_theta)) + else: + angulo_final = 0 + tipo_final = TipoMovimentoDirecional.RodasDianteiras + simulacao_latlon = [] + #print(f"Simulacao: {len(simulacao_latlon)}") + _cmd = { + "enviar_comando": True, + "parada_necessaria": parada_necessaria, + "erro": False, + "latencia": pos_latencia, + "angulo": float(round(np.degrees(angulo_final), 2)), + "tipo": tipo_final.value, + "simulacao": simulacao_latlon + } + return _cmd + + except Exception as e: + mostrar_log(f"❌ Erro ao processar MPC: {e}") + return self._comando_fallback_hot_stop(comando_anterior) + + def _verifica_tempo_maximo_execucao(self, t_init, processo): + limite = 1.0 / 5.0 + delta = time.time() - t_init + hot_stop = delta > limite + if hot_stop: + mostrar_log(f"🚨 Tempo maximo de execucao excedido: {delta}/{limite}, processo: {processo}") + self.executor.shutdown(wait=False) + self.executor = ThreadPoolExecutor(max_workers=20) + return hot_stop + + def _comando_fallback_hot_stop(self, comando_anterior, latencia=0): + _cmd = { + "enviar_comando": True, + "parada_necessaria": True, + "erro": True, + "latencia": latencia, + "angulo": comando_anterior.get("angulo", 0), + "tipo": comando_anterior.get("tipo", TipoMovimentoDirecional.RodasDianteiras.value), + "simulacao": [] + } + return _cmd + + def _gerar_angulos_candidatos_receding(self, ponto_alvo, ponto_atual, ponto_ref, contexto): + try: + orient = self.gps_handler.calcular_orientacao(ponto_alvo, ponto_atual) + delta_theta = (orient - ponto_atual[2] + np.pi) % (2 * np.pi) - np.pi + angulo_ideal_rad = np.clip(delta_theta, -np.radians(self.angulo_max_graus), np.radians(self.angulo_max_graus)) + angulo_ideal_deg = np.degrees(angulo_ideal_rad) + + CARRO = contexto.get("Carro", {}) + status = CARRO.get("Status", 0) + manobrando = CARRO.get("ManobrandoEntreRuas", False) + + if manobrando or status in [ StatusCarroMapa.EntrandoRua, StatusCarroMapa.SaindoRua, StatusCarroMapa.Manobrando ]: + tipos_validos = [ TipoMovimentoDirecional.MovimentoArco ] + elif np.degrees(delta_theta) > 10.0: + tipos_validos = [ TipoMovimentoDirecional.RodasDianteiras, TipoMovimentoDirecional.MovimentoArco ] + else: + tipos_validos = [ TipoMovimentoDirecional.RodasDianteiras ] + + dados_matriz_custo = contexto.get("VisualWorker", {}).get("MatrizCusto", {}) + matriz_custo_valida = dados_matriz_custo.get("Valida", False) + regra_matriz_custo = contexto.get("RegrasAtivas", {}).get("matriz_custo", False) + filtrar_matriz = matriz_custo_valida and regra_matriz_custo + + angulos_raw = self._gerar_candidatos_brutos(angulo_ideal_deg, filtrar_matriz) + #print(f"Angulos Brutos: {[round(float(a), 2) for a in angulos_raw]}") + tipos_final, angulos_final, custos_candidatos = self._filtrar_candidatos_validos_por_matriz(filtrar_matriz, tipos_validos, angulos_raw, angulo_ideal_rad, ponto_atual, ponto_ref, contexto) + #print(f"Angulos Filtrados Matriz: {[round(float(np.degrees(a)), 2) for a in angulos_final]}") + angulos_final = self._uniformizar_angulos_candidatos(angulos_final, angulo_ideal_rad, 10) + #print(f"Angulos Uniformizados: {[round(float(np.degrees(a)), 2) for a in angulos_final]}") + + return tipos_final, angulos_final, custos_candidatos + except Exception as e: + mostrar_log(f"❌ Erro ao gerar candidatos receding com contexto visual: {e}") + return [], [] + + def _gerar_candidatos_brutos(self, angulo_ideal_deg, filtrar_matriz_custo): + try: + # 1. Geração em torno do ideal + if abs(angulo_ideal_deg) > 10: + passo_ideal = 5.0 + passo_total = 3.0 + faixa_ideal = 15.0 + elif abs(angulo_ideal_deg) > 5: + passo_ideal = 2.5 + passo_total = 7.5 + faixa_ideal = 7.5 + else: + passo_ideal = 1.5 + passo_total = 10.0 + faixa_ideal = 4.5 + + angulos_ideais = list(np.arange(angulo_ideal_deg - faixa_ideal, + angulo_ideal_deg + faixa_ideal + 0.1, + passo_ideal)) + angulos_ideais = sorted(set(np.clip(angulos_ideais, -self.angulo_max_graus, self.angulo_max_graus))) + + #print(f"Angulos ideais: {[f'{float(a):.2f}' for a in angulos_ideais]}") + + # 2. Se for com matriz de custo, gera todo o sweep + if filtrar_matriz_custo: + sweep = list(np.arange(-self.angulo_max_graus, self.angulo_max_graus + 0.1, passo_total)) + + # 3. Remove ângulos muito próximos dos ideais + def esta_proximo(g, lista, margem=1.5): + return any(abs(g - i) < margem for i in lista) + + sweep_filtrado = [g for g in sweep if not esta_proximo(g, angulos_ideais)] + #print(f"Sweep filtrado: {[f'{float(a):.2f}' for a in sweep_filtrado]}") + candidatos_graus = sorted(set(angulos_ideais + sweep_filtrado)) + #print(f"Candidatos graus: {[f'{float(a):.2f}' for a in candidatos_graus]}") + else: + candidatos_graus = angulos_ideais + + return candidatos_graus + + except Exception as e: + mostrar_log(f"❌ Erro ao gerar candidatos brutos: {e}") + return [] + + def _filtrar_candidatos_validos_por_matriz(self, filtrar_matriz, tipos, angulos_deg, angulo_ideal_rad, p_atual, p_ref, contexto): + pesos = self._calcular_pesos_movimento(contexto, abs(angulo_ideal_rad)) + peso_angulo_ideal = pesos[4] + try: + angulos_rad = [np.radians(g) for g in angulos_deg] + dados_matriz_custo = contexto.get("VisualWorker", {}).get("MatrizCusto", {}) + if isinstance(dados_matriz_custo, dict): + custo_grid = dados_matriz_custo.get("Custo", None) + else: + custo_grid = dados_matriz_custo + + if (not filtrar_matriz) or (custo_grid is None) or (np.size(custo_grid) == 0): + custos_candidatos = {} + for tipo in tipos: + for ang in angulos_rad: + erro_ideal = abs(ang - angulo_ideal_rad) + custo_ideal = peso_angulo_ideal * erro_ideal + custos_candidatos[(tipo, ang)] = custo_ideal + return tipos, angulos_rad, custos_candidatos + + tipos_validos = [] + angulos_validos = [] + + distancias_ref = dados_matriz_custo.get("DistanciasRef", None) if isinstance(dados_matriz_custo, dict) else None + if distancias_ref is not None and len(distancias_ref) > 1: + distancia_sim_m = float(distancias_ref[-2]) + else: + distancia_sim_m = 1.0 + largura = contexto.get("Equipamento", {}).get("largura", 0.85) + velocidade = contexto.get("Carro", {}).get("Velocidade", 1.0) + + tarefas = [] + custos_candidatos = {} + + def avaliar(tipo, ang): + #print(f"{tipo.name} | angulo: {np.degrees(ang):.2f} : Avaliando") + traj = self._simular_trajetoria_curta(p_atual, tipo, ang, velocidade, distancia_sim_m) + #print(f"{tipo.name} | angulo: {np.degrees(ang):.2f} : pontos simulacao: {len(traj)}") + custo, valido = self._avaliar_trajetoria_matriz_custo(tipo, ang, traj, p_ref, dados_matriz_custo, largura) + #print(f"{tipo.name} | angulo: {np.degrees(ang):.2f} : valido {valido}, custo: {custo:.4f}") + if valido: + erro_ideal = abs(ang - angulo_ideal_rad) + custo_ideal = peso_angulo_ideal * erro_ideal + return tipo, ang, custo + custo_ideal + return None + + for tipo in tipos: + for ang in angulos_rad: + tarefas.append(self.executor.submit(avaliar, tipo, ang)) + + try: + for fut in as_completed(tarefas, timeout=0.3): + try: + res = fut.result(timeout=0.05) + if res: + tipo, ang, custo = res + tipos_validos.append(tipo) + angulos_validos.append(ang) + chave = (tipo.value, round(float(np.degrees(ang)), 2)) + custos_candidatos[chave] = custo + except Exception as e: + mostrar_log(f"⚠️ Timeout ou erro em future: {e}") + except Exception as e: + mostrar_log(f"Uma tarefa estourou timeout durante o processo de filtrar candidatos validos na matriz de custo: {e}") + + return tipos_validos, angulos_validos, custos_candidatos + except Exception as e: + mostrar_log(f"❌ Erro ao filtrar candidatos por matriz: {e}") + return [], [], {} + + def _simular_trajetoria_curta(self, p_atual, tipo, angulo, velocidade, dist_min): + try: + traj = [] + distancia_m = velocidade * self.dt + if distancia_m <= 0.001: + return [] + + omega = self.gps_handler.calcular_omega(velocidade, angulo, tipo) + + x_temp, y_temp, theta_temp = p_atual[0], p_atual[1], p_atual[2] + + passos = math.ceil(dist_min / distancia_m) + + for _ in range(passos): + x_temp, y_temp, theta_temp = self._nova_posicao(x_temp, y_temp, theta_temp, omega, velocidade, tipo, angulo) + traj.append((x_temp, y_temp, theta_temp)) + + return traj + except Exception as e: + mostrar_log(f"❌ Erro na simulação curta: {e}") + return [] + + def _avaliar_trajetoria_matriz_custo(self, tipo, ang, trajetoria, p_ref, dados_matriz_custo, largura_equipamento): + try: + matriz_custo = dados_matriz_custo.get("Custo", None) + matriz_nav = dados_matriz_custo.get("Navegavel", None) + if (matriz_custo is None) or (np.size(matriz_custo) == 0) or not trajetoria: + return float("inf"), False + + altura = len(matriz_custo) + largura = len(matriz_custo[0]) + custo_total = 0.0 + celulas_atravessadas = 0 + + distancias_y = dados_matriz_custo.get("DistanciasRef", [])[::-1] + escalas_ref = dados_matriz_custo.get("EscalasX", [])[::-1] + + distancia_min_m = distancias_y[0] if len(distancias_y) > 0 else 0.6 + + for x, y, theta in trajetoria: + x_rel, y_rel = self.gps_handler.converter_para_referencial_robo(x, y, p_ref[0], p_ref[1], p_ref[2]) + if y_rel < distancia_min_m: + #print(f"{tipo.name} | angulo: {np.degrees(ang):.2f} : descartando, x: {x:.4f}, x_rel: {x_rel:.4f}, y: {y:.4f}, y_rel: {y_rel:.4f}, t: {theta:.4f}, d_min: {distancia_min:.4f}") + continue + + # Busca binária na profundidade + idx = bisect_right(distancias_y, y_rel) + if idx >= altura: + idx = altura - 1 + if idx < 0: + idx = 0 + #print(f"{tipo.name} | angulo: {np.degrees(ang):.2f} : x: {x:.4f}, x_rel: {x_rel:.4f}, y: {y:.4f}, y_rel: {y_rel:.4f}, t: {theta:.4f}, idx: {idx}, distancia: {(distancias_y[idx] if idx > -1 else 0):.4f}") + if idx < 0 or idx >= altura: + continue # fora da matriz + + idx = altura - idx + + escala_x = escalas_ref[idx] + largura_total = escala_x * largura + colunas_ocupadas = int(np.ceil(largura_equipamento / escala_x)) + colunas_por_lado = colunas_ocupadas // 2 + + # Se a matriz vai de -largura_total/2 (esquerda) até +largura_total/2 (direita) + j_central = int((x_rel + largura_total / 2) / escala_x) + + for j in range(j_central - colunas_por_lado, j_central + colunas_por_lado + 1): + if 0 <= j < largura: + celulas_atravessadas += 1 + custo_total += matriz_custo[idx][j] + navegavel = matriz_nav[idx][j] + if not navegavel: + return custo_total, False + custo_total = custo_total / max(1, celulas_atravessadas) + return custo_total, True + + except Exception as e: + mostrar_log(f"❌ Erro ao avaliar trajetoria na matriz: {e}") + return float("inf"), False + + def _uniformizar_angulos_candidatos(self, angulos_validos, angulo_ideal_rad, max_candidatos=9): + try: + if len(angulos_validos) <= max_candidatos: + return angulos_validos + + extremos = [] + rad_min = -np.radians(self.angulo_max_graus) + rad_max = np.radians(self.angulo_max_graus) + + if rad_min in angulos_validos: + extremos.append(rad_min) + if 0.0 in angulos_validos: + extremos.append(0.0) + if rad_max in angulos_validos: + extremos.append(rad_max) + if angulo_ideal_rad in angulos_validos and angulo_ideal_rad not in extremos: + extremos.append(angulo_ideal_rad) + + restantes = [a for a in angulos_validos if a not in extremos] + restantes.sort() + + qtd_restantes = max_candidatos - len(extremos) + if qtd_restantes <= 0: + return sorted(extremos[:max_candidatos]) + + # Distribuição uniforme usando linspace para evitar perdas + indices = np.linspace(0, len(restantes) - 1, qtd_restantes).astype(int) + selecionados = [restantes[i] for i in indices] + + #print(f"Candidatos uniformizados de {len(angulos_validos)} para {len(extremos + selecionados)}") + + return sorted(set(extremos + selecionados)) + + except Exception as e: + mostrar_log(f"❌ Erro ao uniformizar ângulos: {e}") + return angulos_validos + + def _selecionar_melhores(self, candidatos: list, N: int = 10) -> list: + try: + if not candidatos: + return [] + + # Ordena pelo custo total (menor primeiro) + candidatos_ordenados = sorted(candidatos, key=lambda c: c.get("custo", float('inf'))) + + # Retorna os N melhores + return candidatos_ordenados[:N] + except Exception as e: + mostrar_log(f"❌ Erro ao selecionar melhores candidatos: {e}") + return [] + + def _filtrar_por_margem_angular(self, candidatos, margem_graus=2.0): + """Mantém apenas o melhor candidato em cada margem angular.""" + selecionados = [] + angulos_escolhidos = [] + for c in candidatos: + # Pega o último ângulo do comando + if not c["comandos"]: + continue + _, angulo = c["comandos"][-1] + # Se não tem nada próximo, adiciona + if not any(abs(np.degrees(angulo) - np.degrees(a)) < margem_graus for a in angulos_escolhidos): + selecionados.append(c) + angulos_escolhidos.append(angulo) + return selecionados + + def _simular_passo(self, x, y, theta, tipo, angulo_testado, custos_candidatos, comando_anterior, contexto, visitados): + # Custo pode variar de 0 ~ 13, em situacoes mais extremas, por passo + try: + custo_total = 0.0 + x_sim, y_sim, theta_sim = x, y, theta + self.x_ant, self.y_ant = 0.0, 0.0 + chave = (tipo.value, round(float(np.degrees(angulo_testado)), 2)) + simulacoes = [] + comando_anterior_copy = deepcopy(comando_anterior) + pontos_visitados = deepcopy(visitados) + velocidade = contexto.get("Carro", {}).get("Velocidade", 0.0) + angulo_caminho = np.radians(contexto.get("Carro", {}).get("AnguloCaminho", 0.0)) + status_carro = StatusCarroMapa(contexto.get("Carro", {}).get("Status", StatusCarroMapa.Parado.value)) + dentro_corredor = contexto.get("Carro", {}).get("DentroCorredor", False) + #omega_bkp = self.gps_handler.calcular_omega_bkp(velocidade, angulo_testado, tipo) + omega = self.gps_handler.calcular_omega(velocidade, angulo_testado, tipo) + #print(f"Omega novo: {omega}, Omega antigo: {omega_bkp}, Velocidade: {velocidade}, Angulo: {angulo_testado}, Tipo: {tipo.name}") + custo_visual_worker = custos_candidatos.get(chave, 0.0) + + for passo in range(self.passos_horizonte_local): + try: + #old_theta = theta_sim + x_sim, y_sim, theta_sim = self._nova_posicao(x_sim, y_sim, theta_sim, omega, velocidade, tipo, angulo_testado) + + simulacoes.append((x_sim, y_sim, theta_sim)) + + idx_alvo_sim = self._corrigir_pontos_visitados(x_sim, y_sim, pontos_visitados) + ponto_alvo_sim = self.pontos_info[idx_alvo_sim]["xy"] + orient_sim = self.gps_handler.calcular_orientacao((x_sim, y_sim), ponto_alvo_sim) + orient_sim = (orient_sim + np.pi) % (2 * np.pi) + + erro_pos = np.linalg.norm([x_sim - ponto_alvo_sim[0], y_sim - ponto_alvo_sim[1]]) + peso_erro_orientacao_caminho = 0.3 if status_carro == StatusCarroMapa.CaminhandoRua and dentro_corredor else 0.7 + erro_ori_sim = self._erro_angular(orient_sim, theta_sim, angulo_caminho, peso_erro_orientacao_caminho) + + pesos = self._calcular_pesos_movimento(contexto, erro_ori_sim) + peso_erro_pos, peso_erro_ori, peso_suavidade, peso_fator_re, peso_movimento = pesos[0], pesos[1], pesos[2], pesos[3], pesos[5] + + vetor_alvo = np.array(ponto_alvo_sim) - np.array([x_sim, y_sim]) + vetor_alvo_norm = vetor_alvo / np.linalg.norm(vetor_alvo) + vetor_movel = np.array([np.cos(theta_sim), np.sin(theta_sim)]) + cos_angulo = -np.dot(vetor_movel, vetor_alvo_norm) + delta_angulo = abs(angulo_testado - comando_anterior_copy.get("angulo", 0)) + + custo_pos = erro_pos * peso_erro_pos + custo_ori = ((erro_ori_sim / np.pi) * erro_pos) * peso_erro_ori + custo_suavidade = ((delta_angulo / np.pi) * erro_pos) * peso_suavidade + custo_tipo_movimento = peso_movimento[tipo] + erro_re = (1.0 - cos_angulo) if cos_angulo > 0 else cos_angulo + custo_re = -erro_re * erro_pos * peso_fator_re + custo_mapa = (custo_pos + custo_ori + custo_suavidade + custo_tipo_movimento + custo_re + custo_visual_worker) + + custo_total += custo_mapa + + comando_anterior_copy["angulo"] = angulo_testado + comando_anterior_copy["tipo"] = tipo + + #if matriz_concluida: + # break + + #print(f"Passo: {passo}, Alvo: {idx_alvo_sim}, Distancia: {((passo + 1) * distancia_m):.2f} m, Theta: {theta_sim:.2f}°, DX: {(x - x_sim):.2f} m, DY: {(y - y_sim):.2f} m, y_sim: {y_sim:.4f}, x_sim: {x_sim:.4f}, Tipo: {tipo.name}, Angulo: {np.degrees(angulo_testado):.2f}, custo: {custo_total}") + #mostrar_log(f"Omega: {np.degrees(omega):.2f}°, Ori: {np.degrees(orient_sim):.2f}°, E Ori: {np.degrees(erro_ori_sim):.3f}°, E Pos: {erro_pos:.3f}, F Re: {erro_re:.3f}, d°: {np.degrees(delta_angulo):.2f}, CM: {custo_mapa:.4f}, CP: {custo_pos:.4}, CO: {custo_ori:.4f}, CT: {custo_tipo_movimento:.4f}, CS: {custo_suavidade:.4f}, CR: {custo_re:.4f}") + + except Exception as e: + mostrar_log(f"❌ Erro ao simular passo {passo}, {tipo.name}, {np.degrees(angulo_testado):.2f}: {e}") + + return custo_total, simulacoes, True + except Exception as e: + mostrar_log(f"Erro ao simular passo para {tipo.name} | angulo: {np.degrees(angulo_testado):.2f}: {e}") + return float('inf'), [(0, 0, 0)], False + + def _nova_posicao(self, x, y, theta, omega, velocidade, tipo, angulo_rad): + distancia_m = velocidade * self.dt + + theta_sim = theta + (omega * self.dt) + x_sim = x + (np.sin(theta_sim) * distancia_m) + y_sim = y + (np.cos(theta_sim) * distancia_m) + + return x_sim, y_sim, theta_sim + + def _nova_posicao_new(self, x, y, theta, omega, velocidade, tipo, angulo_rad): + dt = self.dt + v = velocidade + distancia = v * dt + + if tipo == TipoMovimentoDirecional.MovimentoDiagonal: + # crab: yaw não muda; desloca na direção (theta + steer) + move_heading = theta + angulo_rad + theta_sim = theta + dx = distancia * np.sin(move_heading) # Leste + dy = distancia * np.cos(move_heading) # Norte + x_sim = x + dx + y_sim = y + dy + return x_sim, y_sim, theta_sim + + # Demais modos: pode ter guinada + if abs(omega) < 1e-6: + # reta + theta_sim = theta + dx = distancia * np.cos(theta_sim) + dy = distancia * np.sin(theta_sim) + x_sim = x - dx + y_sim = y - dy + return x_sim, y_sim, theta_sim + else: + # arco (uniciclo, 0=N, +θ horário) + dtheta = omega * dt + x_sim = x - (v / omega) * (np.cos(theta + dtheta) - np.cos(theta)) + y_sim = y - (v / omega) * (-np.sin(theta + dtheta) + np.sin(theta)) + theta_sim = theta + dtheta + return x_sim, y_sim, theta_sim diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/__pycache__/costmap_fuser.cpython-311.pyc b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/visual_worker/processamento/__pycache__/costmap_fuser.cpython-311.pyc new file mode 100644 index 0000000000000000000000000000000000000000..dbb8278c6e0e635f6906fc6a51f0236234da37ba GIT binary patch literal 31781 zcmch=33MArnkI;oCn)d$36KOy0K8A}67NGibx@)X(z<0BVuBPY5|9$04me>;o~miu zqj?QitJaX4Uc(+$72K9*SoQ2FR*yX+m#b#9Z@Z^6IoVAt-f|viX1$);e!Gvg?s`+> zx37D@$i$UATynX4lF7)3$Upx0GvbdwuKy4GC?g|HhU@O%{jXP@i!#}NA%W;} z3eOudN=C`&Wf#P!{DNEvD=sJ%vO_XT@f{hZ{Gmc7dmsPebwNd`z9YM!rqpngs3f?_ zlzc)~o%|~TFg_K56CTl2Uor^3A+yT}{tJp!+0H+zkV^ZG{DP7)QW`u}R1vigPc@|# z@+qb=5lY^fE-{8Gp$vdZq4JR?l`>HU2&GX;pw1lD3^`ry1>5z}MVFm^Pw^`)Nct5i zre7(ELrDEfb)&JN{flYg`ak=}-&1^nSX`U^GonX#aPo`)Yqv%;{-Pu zF2VEV|0gG3%E&JPAtSTKK;Xw4@p7x9KENn`i1_zo?*K>e2$uv-xdSd*lQMS#C5(Cp zTv8MtqXKq9scIC4@31p)5%#TN28V zgmjWnZZt$G@Ta2GXca_Ew^(tOd;SAb|Nb9Xx%6%1($_3kPIG{u`Wp}t~+q`wk zIqyOyi__MrMVH$dP7%CL$F%6PIe;*k#5rs?#V8MLkG%9OkS`=J-L7z&^~(G;tIKV3 z+rvti{YJGWta91sr^9K&H@dFcuG?uNDZe7(n21wCoJ!)DiDMy7DV%V60^NjQ%B4ZH zDdGC+rJ=4%7wirS1$o3Vb;&j(Cb?pB*)I(sFevd{nRQ&c-PU?(*nVYk=F-VKC_~4k zi79&ay4!W>mXp3_r(KtB&bk&+Bx3N=b=o;)ce!i}C@JTqDN)8)NgdD)*YAW=tkzk_ ztlMh!^xdfNnbHZ`w_Z3nH*-dUk%kL6BMR z$-^q!?OB&6bA+ayCWq5Rn(NfHF4K5*YFIgMcZ3y=>tWUO;`}^aK+2cM{cy4%h+3M% zY5_W&I&yo;e%+0B7fxS5tDqr#=AARuD!Pe)E9YJAux8peKYs}L&N8Kx` z57XI_p4It3AN;eUoc=7YKg(vG-N?y((BU5n*w~yZE~kpmsbV!%zunLpwq)|u#<;Vg z(X)nzU=tkNoMu0-+0X8F{}$Jl5^3>t5!v-h|A0aEsUnxShO~ix<)@bHfiC5zU26D8 zr(!Gx&=_0mz;uMI8F^y&8t+$QEL|&MycnzQ$sIb~&r=6~NgQojQ ztZKn-bA*#H9=U^lnV<`&+*q_Z+_Up`dH_Ls5KeWH(5UHy(Bd&j+jpEwLW1jW;gCk2mU*C&G?qlq8l^@rHZLm_Q8+Hpp2NN;?@^F2@ChR?(4t9gC(y1sEu-}q7X zhXtIzhu8P8nLQzGu0Qv+mzQ297;pORK{KnbpAUfIqjb)K2GCuhWMNzRx=b8CkKc)5gkFCw5-TkLkn(B z)5>dF+1>7_I5|MXNfnCVmF^*v>|eLS`Lv}2?$7I5;C^OG8%|b!R+BxfQ2s)phJPxd zM~n-WUjTz3SQ5tVbbex!fap*FNVqVf$H<+Gd%v!~Lw_YnvmF)y*XVvT`z-f~%mG4ZsGChb8jj4Z=yVZbalggA#QL=G32nUqtMk+ZCEJZ`5I zs653|cHaK}I8wX*F&x6w$*CP7Z4R%sY{?WEZJ+Cn{>kNw!KR1S;FT2%r$4~!53rdB zKF=}uv;4!$W5Fygr2Bb8vdymPXLODdouRq@%`XqEx|PECy=TQF;Vw)!rmY9x?7`)MU;&7wayiEhP}^@}k(t)145Q zi?tdltzOLF=g?Bci%w^-5`#b0ca`yy0G-Lg*fTa~jdV&WlKHxtN!g<<%DpKsrH~aK zO7JDN#l>#qJd9!IXYV?uW^Jabg=!OeCb3b5yByXrDkXV5|~l``v^Ci5vw9&L_0!lW>q>aaO5 z#hkS{g!y4O1!BQ<-8N+x+wd3=3!Rxbaf~jY@cARYBr?G5_TS`kD)^j=^_-fuoEk2tj?bxsyvVYI3eCRbn}sFI zeXkx{8u1<2$ksim_1go_1&1GxaoHVwcE@^l|5|qcFBQK?~>I?j> z_&e=yeQ<}(G=*~VmX7|ti6k=4cx9k(NH6=jd>{vp&vNC&)u#>hDL*r34|OU(>r})4 z#cJ_)|N7MGzwW0qkmQR$|1)y^AN`RPyua%IqD8p=OaB*dJ|x$#qA{QKPmx82=tY(d z1j+WdXgGhc_y;%t;MVqGxX{9luPrhu#P{0G*KQ^Do|HUst?t!D$RxB&cdW(BWUtJXu-DL6= z<}G66j3(kQW|U0EHKEMBB}@@h?7{ajCB*fbD4Exce+y%{cZxA2_VY}kl*R})GbELI zUcHdyEtSg@GNx488|*#=r;?<&ke}sVK9&3(S>jBZu}GnYWm4QomCB@%Uc+1Fj;#Tr ze~HOb#+W#Ed7L7xA8&c$fSxgK&+YPUZv~TlF9j>C6>&-I$YshT^{OECv2%^olMcEn znaX=fOeK{Xsjt*1EK|-@^nmjFEs1;}`3l0#!o`#^l?0oVK558XC8bHq$4r&<%cY?h zEq9g{mu@cInpR>BKgC-up@y_fks6~rw)%>)j;)1Aae8Z>DRpeccU$UO@L=7y!cZ?s z9a~EVMu4z%?^1((#$Mm1$v`uLiegWUY`+h9wkW(7(0p6exu{nx1T`beD@+#YwFmKDJA@U8oOPmo2&Nx?$NU_kj(PDP{{!wO>8A+i25OXMj`pe8A3I2F#9FpQs zz;Kv3oPeR{8NUCp6qhvQJ;HR~1)qAmnIqobL~NLSCdz4PJMUrY!21LRv|}kiT>q(rZRvl-#dZP&T~vJ({!L{dOxEjG6V~$I|+dCWKn>+~*?9Dd{svT5!FjqlcCo?P1 z+7;nfk>V8e6WOGXCu`?SE|W)fFDHBFnA6^COqNeUcP92piPTYACi^=2bSZ_>viHt2 z_#^9Ul9WAEFOx%JrD=tgp9N-t(Fto)j>Mc9hZHk2NA(dYh&K5g)5Y{kF?pTn2d|@7 zy1|^LHOvGR9o;1{dRjqcpsev8@4kz7`H_*|%Z&40k)&MXQpPFGola$(j3Y81Q=&~@ zcf7C%B{W0dJG}>1dK=b0hGujTj_64IdP(}qXbY2+sWw`s1P2-2>?~htZqg7+%6ZQy z$!G7cPf7TClqO12Hb*vTh0E(=uG5ypbu@65i%OU~i91%(8Y2*-KEZ=7>LBqs%n}@V zBtG6B?tw!{Dc}&WAAI!E_0$Ci@b3 z5#vC~3<|Kr0&MRtu-ke|z>Wy8lF~qn97T&fgtv#qT$x@8wb!IMO5UB8z60%(5^8^s zQM4p|n-o8FWLd$COCXL(AtdjPOW#pPmoX-elv85ATPKCXOqQ%x?UU4Z_coY0YHT@O z*xQ_p^BwdI3cPOj*?0`KjXpu6PUdyLbmSMiOL08=pYNZUy?PRH)ONeCcZ3e8F}Si1vXO@f26pd zIw5;cM$ZGFS{YU}G=}A~&=H0Tsf$d=u&pR;Dt?vLL(NXPr<@Dd`#fFJ4Mh^U=(a zr+(*N2X^HX=Af@8LqlCout{a3SDm=84qR$LLBel?QX#c^S|M-4NA{)TUdTY*i;V7&xRFK*nggCa*qeJnZrfZx{;w1AbRv@}Y+_)bNHH z-+&l(BXHp19nMhA8>)AW3KbgtHh-&c?6(_w6WMY;C{J`ZbS12*Z?y>yZcf+F>-yQ< z?ue=ik`rAO(j)()fz*Kv*{2z41FgzW_1Odam7nfc!%q_pf*Jhd+UlRIR?swjVbzrL z`W?Ch-sro3BWbTOmj34e5mlrx)k;`+NKk6oHod@9OYX>)bkf@(Cl-OU$-G1vN)%O6WhnLdSMvI=lnu?(Unxur2@1Q@3vTNIm{BcgP0(kMElngq0!J#s z^4p?116f{;sxu{7g`}RgZ^~X`x-68Nivd2uRm!Z6P-$796RLA#Pvk!y|8P9e>2LA3 z{P_rbdg6aM{AY*RBbVa0?oyOfh%!@_=m=~=G@49pxH(-ruWM&_yQ6HEK&Sj^_JCITsa6f|C{1=nJo|=hQ0utrgqq4t+TO6cfLj8j$PITPl1OJn6n^eP zyy1y3BZL`wWC)C~Lr&g7=?y8Pmr&1&L=lIfnPN}Tqj*Wk2VD|;38F_a?$O8e)Uv+V zCkZ#^R5wxtPY$zzGTlIR>J&g^m2k3Obgp(Xl=5^i*CsVVueb#LY zr`TQiR+_Rcmf(GajL&}b*++Jl$zd~1+h%Xa3Z|MUOwqKRg7TOhwgN!l>@vYR%1+dr zJ-McZMZ}n*<pmO2}!y&TQCv-z$oy3N!K^#xhvxIczj2-4|cE{oZ$U^;#%IVN} zci*8W2!LwJK08kzA-Kqbb69=F>72LHeL`ZW0)|x(jIOX6HgYY^^id%R0ins1AgmTt z&V?QkYVGt4(d>i9FSHS1hc@f66FzpJVvx=J>~-)HO>FIy9^!}E;ev}M6Eq~fejQe2 zVbz=yu*pE=a?*fvfq12j!VA|8+D-2lU>C1bxi<*bYf-RkO8rWbzN1lQd zn#MrUc9R5&`C%bE z1rr>R6T=$vUdU-4?KO+O>Wr{xgzLhY@hFk^M8#+=u`7g*wF_;_4zuATnktK{620BQS)Z+-hC0dgvdLPZ9)1_I9yk^}x#1-3)OK zvM{*`CxZ;pGk*ofkBE_9gVBiK!vi*)VdWLrk|x7HKMU?aWF(9S>J?$quJB%DOK90% z5gHm1XJ~^&l7%y3LWr1z(f^2i7b+vtH;1)R!H60lyhgu6zJLf?`d#9Dm%t}cR&Xk; zTJ5)CbtbTj`!;fz6Y5?uhl!XljSq``yDO~17s15`x`>u96_!JMfNx>0s&K%rm3%P4 zkSfmp1!Mwlq_kZ@%310*NpFW~9AYRZL^NDMMAJVaB*nxqI_j|eW;jjgpM-`YbSQ|h zPCIGXyE(6iQ;3V9%B^r}tk)tW6N6d>h$JIB`VMi(OqqTWj*ATWB2&iyv5$iX4~i4i ze6-2k*Cad@&o2@f;z(4R%=f4ridg#K8 z_oh!!vWB&4fho|pmfy5=B$RIojIZT46JJT-k{Fj;9LVQ#Yl7#w+_t6B&Afu8L!rWw zrDLJYLN>D!_P8IEvyBJ2ngPCMV6AY#mmJDG$V%@}NrV4ouB4GKX^?&8ZCuYc-#AKDoqopXR$yW1)sCo#0C+e8)hau{zkn6}BuL+b|R?rv$P- zC?)2Jh6diyuykZ2x4_@@`nd<^wq!{eor2?&hpOAy>OtS-Kog%^&gPasHU8zfi|nP# z>|`waKO40mppKR3%Ane{0`x;)>}z+kZK-J^k{p|Xl#_B)4sNBp+ekNJ*$ZY=qM z;!$!?vqE!Kot&wQH+6BwZr<4K8`&tW2;BIfG-zANeXss;Jy+Vnmv%%GZmCi;$ST0^ zc7J1#nJmkqNJ{WvFDwuAW`+0LeY?wDJWsfQ&u$DI7(#D$Gmc|LRh~WZf z?%>TG#M-&4jm;kZez{-nH$PPRPY1HO>@q&PjLk0lWaNqYXNNyNyfhff)o;m?GqAz| z2W6};_q_zOu>6&#$3{-y#_QW&SNY_=rchBS7Ta!MPUH%VE4p{M4b+n?A!XkAJB>As)r;~Ee0;H=sK zzIK4cefkWWSst))nN=ZOq3=YnifuczR&$80ITR``@g4fDz{+nIzFGLNXt`+V=xYwyaep3D1C(ykZ8v@^ZLsVUfz;r<{p!W8e3UIO|XeG)NdNA zSYzvI58FKvs%TzmTCuIlSEg3l_=+z7p`9_Gn=Fq~gO!}AnKw222R2$dR`vg;=ueA& zR`PMl(=5JojB7c@w;W@Q^+6kFY}{yRTeEtxw1sC=WW8l+aN=-=D_ct2Ox#g}yv66@KL1EH4Iz`oFdF4ok% zaiD9p>xu2-UhcpV{=gB|)Cl@=jjXN`-{1Zx^TBzsuBLGofjnpqDg$Sh9h|Bleemc9s0LGxN+En8T-QC|7zeDD@m-p-e| z`$soQ%*!tZ+qjZuzNDEH-GH3bVBU{=Kg23S<9`P+o!zU=KT|(3|Abj#e4}KA0)OC9 z^ep8;^E(Cp1)$}1b?dsOHC+>@YvFY*tghv0&eCwGqidJ6bind(5i6;4!R)=$j%*ms zKd?X;9aw37ujiANRreG7Q|i-su6vU2p5$sK*)tc|OIFVE0&jVNGun8gZ3*p9m4T(P zsPh|*3`apszkTh^YY!Ke7dS&5Z>WR(FD_-vTGz`u*2+4>df|$@`Qq-S6Prf!(pbo7 zSsD!)>wqpLIRk4iQRg?=vW#ptdpgh_qyqi$;C@mgKG}^g%xr4&IIU%~ z*y2kM<(K-BHuEd|MQmj!+danRALH|nA3vjUejFP{ir0LSXiC z-qeA_CH4N7g7%g3tBd@B5w2vEFByHPg4k{fnM?gip~6bGuw_-lcAN~AHLm2Y%2&;+ z%9R4XtY^J!aII{RD;ws^hW*1||8}FM9wKp=usRB?s8w*@NeUP(aECi z)HME!x<9FVuiaM_G zU~sd#eWf&594rnDqP%{jc~`TdVC#EvubdK}qRW~3cwyZgOYfnInjg)+JG-LhDj@&b z{D=I9HY*w+2J4!F$A6jqi^8YRaRU?lz(nwPU??!eUcSO!n2X*1)KJyQRXtmE68Dqy z;?tk8m88g>kh85LtZp222*2ot3QYcrK;FZK<%TU~LUeP-N(I|_glj&^Hy@R}nY_T_ zzPP|mI`~OP;;YU29#(f6ECKABgC|5!VrE-PSyu7u88})brw9#wExVD;Zd{ptQnuDM z%C?O{07m#02bTEMU{W@x0^^M~wt1MV9pP(75Z){-@(qU!6|A9o)yQ_9`W(WNsC@ zoHLH|#&Om-j*J7X5AQ7B2@bsZlK&;Kg(zjFhE07T+EH$2$aG}I{nI-?x${Ivx+2yD z*-LJr!I6furIP24%2zM2x)}&Yctw{z`NFoCP<1n_8^P%oF_=*AU}7+moRqjPV=GOT zV+jneWmmD;RY5m~5uTuD^|>b#e|mBC;%2_tmqa?d;dhS(j`{BR?ySuDR2#;UfGS|; zjCH)R&X@W%H4-xR`bG%h1M-zJlI?&%^?>~8c~)mlj0ly2=!1kxbRiwRq9+kRV34hS zvVxLOX(if0sHhlYrjWrH$}is1rWp2Z$>1y<-pZEYfSs{TZSk*Vtr_Tfb@^|V`lkYY zY-z{J1Xs|>7j$yEE?(D#0U$O!PQBBDAuzQvv|7uxj&L=je9b6l8O4~-QWx}cmL8vW zqoHNx1lKUkHw^ny*!&tUvv#wn#($Zu?PhyVa78EiqLaR~%|a6fgOHo4n>odxJZo4U zXbWD52$#wZwq(ff_PbZ@zQY1Qa+V_zz+l0YK-hdq<66h~)-iI1$*GI1)v65>8k6=MXvuOJ9(bHG|NrSa{Y5$+cm!J8rLw-H_UTY3w+gr|H!7U znAO!_@SeVUlxsZ1Hy%RpDcbhj+VMH|_#8%$zQdp;ui$|d6ObJ6usW-h98okGY{u~p z9V@o?dc?uGZ_wv{{cy-^foJjcBfcY_1M0cpU~uvAkyZJ7$2oHsZ|)LbickRGG!*zd zmov~ZvT6Y8etdXUwN}@|*7bzSssMZE_2a(d7|#1sA6zD7T@zFU4?Ip?d2X$$i>>Mc z%ZfmonT9vC-_vdsnK3{(|EQiTYUGO=Lp3dg)YC!LqjT>RJ;F4jJa8qL`)Kx^hDQxs zscJKhqJZ;jd{fTf5QltS4!ZA>(m-=y_E9fa+{70*`O-H_%3|m^cZAYz~eB6gX(wc9@XJ! zr(ysQCrwsf7dYWZ=lV&`>e-(eKQ@MHJ27nuv<6y#>3Z779lF3Dx)3s!ZB^=YXm6sE zvG2FPsgY&oqZ(3sQR1a(Yo4| zU$(2^mu&eZ15kCrdCh8VI6D*o|+h(w!kA^(n z$G>=y7$}7(wxy5hE=cuVh<0EADRy4&n8hdTD#`ZLg=`waY*_V+eO%WGzUu_ndV;Gx$=9Cb z3@4W$_=FPM@U`R;8MXGqGo z=`E!&Dfdd4ly5;7j*+)(bjmtpzi3*=^O)Eq_XIJr28I>-=LMvReH$1VIkn;a4F z{)cb{h^E$#Yx{{Z(E69nL3WGR9%->x zGo)Cy)zY)4iPzGzr-|3nv!{vI(zB1&ecR-MmXYn5D%MtZ zq{{r7RIz&4kt*wJQpIXwN2+XCYv$k^NM9*jyPqmRWlH&ow&7IL{n0c6-^MUdS>(*B zc>f|z>CJIuLw&338oonhU#ul>Kg&i+7hNQ=!c0gfg@ff=iZ>q?qberfi?gU;gNpvxTOi39r>G<6 zm^}D~Imgm{P0j^k&fp7Ew44*4j6s_A8R{#>JJOWiLZ?GM-tZ$=oafV$1&c8R}%==uNwVuvssN%;ZKZN{!KPH2{#f ze6B2-GnM6m?3z+o<1wc1$h;*I*|V+ZPPB#u1+kRUvf2g}8$W$5)E=|WZMb6n^w;8w zwNfb*D2_=|$ydteDx#$#O|dQac!?)Sl4nej6_-8I6f+9SEw6BVWq;QmZIcXc! zF`KDG$tF#;XNs?(hcO0fqBJBvr6XCU=vkm((^taiq%;X}dAr2rO5+&)Gqkw49K>8Z zzA_T1O{6^Pmwv&s(9aZ8mgNjDbaRFE4{6_8Q>k%a|v3FmARlO1xrwxBKxt-tN3aCez3g4&;@&O-E>@F1#sctGdRK0_1{ zEir`7>r*&C+cu9h!Xbqg;e^HJ;TXVaqJ!+PnOqLrb=Os=+l15TaSS~kh=Zmpl)pSS zsMLZq+f8_InD=R@?z$Rc7{vNkCOjC7k4s6BQ%05UnkucSMx8pi-4ew7~>L zJ!q4`OB9u)s(K`bVDhL*X_!1JleyO<8dPAJdSZt7mix&KUNi@I5WOC@~6B{cp^zU)BgZzx_4LMBgQ;Ej@d-8F?%<4S_ zy#U~_+~Xo$ZPdUjwP(ieu;0E;_ifWr5;ZmxjH#kh~p{-j60BkWV+lmqRnGOq6P*4zUjH1M`J7Pqpq!LimBg{Ysm=&1+%B3(N#Rg` z;Q&=~(m!O0nMD3O;yi@|y>aq8AowC!K@#;$;t_O?;YlIt%S0&`y4l1&(NiWY^1{@> z=Eg5)2qK9r2MZ@R6J=d!;=+=pKCE&>B{-~sjxl~`#JLDd1$z5(?|X!T!^9~d4(XL? z(&N(qia4Z4rAg;Ulm3HlAWj`|>WR}s98v&b<#p$EdXxlBB$x(c321xXnuU6C+)V1E z0O?v>5X-BBw4fs-%~|pyg@%l$p=V0lNL(p-TTC3GG=(B0;~4SBOMXmzq=dy{>?Cl6 zU&MuLi>MBs8tXskZUBTc64k3+bU8^>A>=^bDpAN#^M!Dx|C(T`C6R)*F4R?tqCNyQ ztke_W4MeffD_-DlV1#x~!5+;#cpv~GuGXJ+E zCWS1L6CGA@wH?315>ra22MAObp+-fzC2{?oe7os)Uc%};{V8C#{pN$%M7{)4S5LCA z9IrX5NFA0#k#otjk(t`6FQ7dTx9uj_yo&)2@W#_MWC)t`?nAJ%ZXUS8LW!*F#)Zyf*L@jzv;mD4ry zx<;VU=CIllY!1Bf;`d(sLGz<7PT#=m8+@v70=sE2zn$@BM&Q~?D`)874INNC6{FaS zj@2^G(9awCp>7%hbF5IDp_?~!W1l)nrptfO9nqH6<@#>;+Wge=wP5SxZnlV=PS1*W zr~OIRlcpzXw$qOLY3rYLvu7{zXRokFr^F~Or=1r-fiffO{`k7ayrwY+$~jFHuOUB_ z5z=JdKfbOpt!Z#7BB!a~H5II;V(-{v>za}^O-Vq>Y07v_8LKHHuw(0*;x$dNpXM|c zUSnZ3mc23T%w`WhA>jb^z~rMdYuZ{?TN}#MJ=nLNX<5sJiW`?%&S#br z9ZB~i?|Nm&T4l#-7FXHLS9Y_N-K&!yUtaGWU+W!zdV}jd#rK|Kdry({n)OW6T0{qv z&n#s#OOgJ?AHDqU%PTjy%1$0yPL-Xzre-J4@sqaoN&DKQot>WHCa?07SJ}y{C`vHK zweMXEls#%#DdqCo_`EjMV_rcdvOX3$`a2@)Vvr-inx$JB5Kd+;S=ZLAX={QloOVC2 zg_crkMmMp-G;SD*{1ji@fFq;nAk7yybA}e)(Bd2L4MeT*jAYAObV7zg^2Rp+jfpHH zwo*3>iq{Kj)(UEZ?a*mxVhd`xf?mF$7h27+H*L~4Nl^WYy=z|?;R?F=f-Y%-Xk^l6 zZvJ|1`C4vyV0vX@b?C_?l#{vKqkQgBs8+{*@jU< zssYn$`K{}Qo;3r}h&;y`MtQ@iPwi6^mDUs#cP)1NUu4bQiEmMGP#qq{&&G)3wP4xf zdicbnuQ_c8uZ5bOI-~gWd^0fAbNP*YJ~ro+Sy04);~OBRjb3OMK*0>f3*9uCp&a|& z{~$;W#&vzon!W~VjQVC?-@LBxT+?@cV)?j+(~t1_k)^|-eDm8SZ$>JOU2`Z?@6QY51#?%DLYW2rv7io0WFb>o zsH84rED5?pxp{uuO6z8EWvHYgR8k3LkuAL}x8>I|P42$GAr{-c3goN+&I+%E=h za>fSU*sv{Ne*4&)$51^6&e+Two7asUYsL=F*u@*W){O&e#sSVa#2bf*>QQwrQH6lB zG`!Uz(;1dvLofa=(J(1nbbF#J=tB&y-e3)bxH-)buOYuWvh$AWMGOlz^knq<%6P@) zR@uL=Hl9e5{e@Z%|6e3&PUI^8B3nWHIyv#@ju#`a(VF&rp>m@y`}sWOf6PtK_CqF$cT=fB#wqlTKR)G?;2A$(#^UKR;T zody~7)%u4*O$U(YrlE-BtkZ|`@Li@B3%xLL^DBSr)Th$)58k#ga+}r7Cgrt~b zIEmko0ek(&^w!H3ci zuiu+)HKYF4bW4Ao1Pm=+DDUKdFuIeOR6aEU{Q^ zC#UV?wVmtQfi>;G6Z4Z(oc0i}J#_bQNTd716R(|kYjF7}r>W#Ml|f7(_TL?eqBy^- z{dx?CLh0Fm*!NoBTbAV-F1?&jFAu7@^!mHQe}5Q$_h*cT(lY-5a3f_cAy7ncNh>7xW8!yQd4-JtVF$1kBvSucQmxhG3 zE4l|YC@wz`p2Nbbu(UxIC17of)fEaoGhrtv*j=R)R~NC01v6{#%?o(@24RL~B*Ge0 zn%1ZCoq3?~U*t2ZxwINSt>*5~M*6;gy6BtmA9(#Dr#16hGna1R(=DvZva@7j{R3H} zk(LY&M!~7PJAeI>EaRfX_heoiIx#VMeBh*Y^tp+V)0a&cUaSHO4(3Y1Z?C#=guz0> zdYiDaA~=6cK7g!q;R95uY+9xN930%7s)|=tvC&&xAxoi6q}T>Yxu|gn0nQE*miq|h zLgNq$MZ`W&;$j1auTpx%0Z1QBAvJE9>yO}I8YP#9Wa)Q>zXVS38>j m{91hXH&T$Jx->a{IxOZG6!KEoKt!Dsxx5-<#T|}7^#2dVKh=}~ literal 0 HcmV?d00001