finalizado mpc com recursivo
This commit is contained in:
Diego Freitas
parent
1c08878dd0
commit
88c595c8a8
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -303,6 +303,7 @@ namespace AgroBase.Models.Modules
|
|||
if (Comandar)
|
||||
{
|
||||
Variaveis.OperacaoEmAndamento.SimulacaoAnguloControle = Angulo_SP;
|
||||
Console.WriteLine($"Angulo Atualizado para {Variaveis.OperacaoEmAndamento.SimulacaoAnguloControle}");
|
||||
}
|
||||
|
||||
int mx = Sentido_SP == Sentido.Antihorario ? -1 : 1;
|
||||
|
|
|
|||
|
|
@ -380,6 +380,7 @@ namespace AgroBase.Models.Modules
|
|||
RPM_SP = Convert.ToInt32(RPM_SP * FatorCompensacaoCurvas);
|
||||
|
||||
Variaveis.OperacaoEmAndamento.SimulacaoRpmControle = Variaveis.OperacaoEmAndamento.DispMvd.Dados.Modulos.Average(x => x.MovMotor.RPM_SP) / VariaveisEquipamento.RelacaoRPM;
|
||||
Console.WriteLine($"RPM Atualizado para {Variaveis.OperacaoEmAndamento.SimulacaoRpmControle}");
|
||||
|
||||
switch (DirecaoAtual)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -631,7 +631,7 @@ namespace AgroBase.Models
|
|||
|
||||
Variaveis.OperacaoEmAndamento.Sensoriamento.AtualizarDados();
|
||||
|
||||
Variaveis.OperacaoEmAndamento.AtualizaInformacoesControleOperacao();
|
||||
//Variaveis.OperacaoEmAndamento.AtualizaInformacoesControleOperacao();
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -356,9 +356,10 @@ namespace AgroBase.Services.Operadores
|
|||
distanciaMargem = p.LarguraCorredor * 0.8
|
||||
})
|
||||
.ToList(),
|
||||
horizonte = 4.0,
|
||||
horizonte = 2.5,
|
||||
angulo_max_graus = _Controle.Angulo_Max,
|
||||
distancia_entre_eixos = (VariaveisEquipamento.DistanciaEntreEixos / 100.0),
|
||||
largura_equipamento = (VariaveisEquipamento.LarguraEquipamentoMm / 1000.0),
|
||||
velocidade_min = FuncoesMatematicas.CalculaVelocidadeMsPercentual(_Controle.PercentualVelocidadeMin),
|
||||
velocidade_max = FuncoesMatematicas.CalculaVelocidadeMsPercentual(_Controle.PercentualVelocidadeMax)
|
||||
}
|
||||
|
|
|
|||
|
|
@ -95,6 +95,8 @@ namespace AgroBase.Services.Operadores
|
|||
_Controle.PercentualVelocidadeSP = msg.obj.comando.percentual_velocidade;
|
||||
_Controle.SimulacaoMPC = msg.obj.comando.simulacao.Select(x => new MPCSimulacaoModel() { latitude = x[0], longitude = x[1] }).ToList();
|
||||
|
||||
Variaveis.OperacaoEmAndamento.AtualizaInformacoesControleOperacao();
|
||||
|
||||
if (Variaveis.OperacaoEmAndamento.StatusAtual == Enums.StatusOperacao.Concluido && Variaveis.OperacaoEmAndamento.Iniciado)
|
||||
{
|
||||
Task.Run(async () =>
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -13,7 +13,14 @@
|
|||
"padded_top_topics_start_index": 0,
|
||||
"taxonomy_version": 0,
|
||||
"top_topics_and_observing_domains": [ ]
|
||||
}, {
|
||||
"calculation_time": "13395172669289149",
|
||||
"config_version": 0,
|
||||
"model_version": "0",
|
||||
"padded_top_topics_start_index": 0,
|
||||
"taxonomy_version": 0,
|
||||
"top_topics_and_observing_domains": [ ]
|
||||
} ],
|
||||
"hex_encoded_hmac_key": "40F346D3248C3AFDF2BEE1FE496DBD32F7CED6E5AE98B881ABC421AA7E7B5642",
|
||||
"next_scheduled_calculation_time": "13395172669287383"
|
||||
"next_scheduled_calculation_time": "13395777469290834"
|
||||
}
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
Before Width: | Height: | Size: 26 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 26 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 18 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 31 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 22 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 17 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 30 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 23 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 16 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 27 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 22 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 21 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 16 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 18 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 16 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 18 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 18 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 16 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 26 KiB |
|
Before Width: | Height: | Size: 22 KiB After Width: | Height: | Size: 22 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 17 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 30 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 23 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 26 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 31 KiB |
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -1,3 +1,3 @@
|
|||
2025/06/20-16:52:51.206 70f8 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Local Storage\leveldb/MANIFEST-000001
|
||||
2025/06/20-16:52:51.212 70f8 Recovering log #3
|
||||
2025/06/20-16:52:51.215 70f8 Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Local Storage\leveldb/000003.log
|
||||
2025/06/26-10:44:24.216 8088 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Local Storage\leveldb/MANIFEST-000001
|
||||
2025/06/26-10:44:24.223 8088 Recovering log #3
|
||||
2025/06/26-10:44:24.226 8088 Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Local Storage\leveldb/000003.log
|
||||
|
|
|
|||
|
|
@ -1,3 +1,3 @@
|
|||
2025/06/20-09:13:56.071 1514 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Local Storage\leveldb/MANIFEST-000001
|
||||
2025/06/20-09:13:56.078 1514 Recovering log #3
|
||||
2025/06/20-09:13:56.082 1514 Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Local Storage\leveldb/000003.log
|
||||
2025/06/26-10:22:26.943 6078 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Local Storage\leveldb/MANIFEST-000001
|
||||
2025/06/26-10:22:26.952 6078 Recovering log #3
|
||||
2025/06/26-10:22:26.956 6078 Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Local Storage\leveldb/000003.log
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
{"net":{"http_server_properties":{"servers":[{"alternative_service":[{"advertised_alpns":["h3"],"expiration":"13395009174487877","port":443,"protocol_str":"quic"}],"anonymization":["DAAAAAcAAABmaWxlOi8vAA==",false,0],"network_stats":{"srtt":79210},"server":"https://tile.openstreetmap.org","supports_spdy":true}],"supports_quic":{"address":"2804:d78:69a:2400:6466:b7fc:8b46:92c5","used_quic":true},"version":5},"network_qualities":{"CAASABiAgICA+P////8B":"4G","CAESABiAgICA+P////8B":"4G","CAISABiAgICA+P////8B":"4G"}}}
|
||||
{"net":{"http_server_properties":{"servers":[{"alternative_service":[{"advertised_alpns":["h3"],"expiration":"13395505488076776","port":443,"protocol_str":"quic"}],"anonymization":["DAAAAAcAAABmaWxlOi8vAA==",false,0],"network_stats":{"srtt":7496},"server":"https://tile.openstreetmap.org","supports_spdy":true}],"supports_quic":{"address":"2804:d78:6d4:dc00:395e:1fef:8c2d:dcbe","used_quic":true},"version":5},"network_qualities":{"CAASABiAgICA+P////8B":"4G","CAESABiAgICA+P////8B":"4G","CAISABiAgICA+P////8B":"4G","CAYSABiAgICA+P////8B":"Offline"}}}
|
||||
|
|
@ -1 +1 @@
|
|||
{"sts":[{"expiry":1781985173.395131,"host":"bWGAftl61rqoc0YzqPncsLvQQh/iC2Bdp3ejUeGC83w=","mode":"force-https","sts_include_subdomains":true,"sts_observed":1750449173.395134}],"version":2}
|
||||
{"sts":[{"expiry":1782481467.393117,"host":"bWGAftl61rqoc0YzqPncsLvQQh/iC2Bdp3ejUeGC83w=","mode":"force-https","sts_include_subdomains":true,"sts_observed":1750945467.393127}],"version":2}
|
||||
File diff suppressed because one or more lines are too long
|
|
@ -1,3 +1,3 @@
|
|||
2025/06/20-17:05:43.009 70f8 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Session Storage/MANIFEST-000001
|
||||
2025/06/20-17:05:43.011 70f8 Recovering log #3
|
||||
2025/06/20-17:05:43.019 70f8 Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Session Storage/000003.log
|
||||
2025/06/26-15:18:11.205 8088 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Session Storage/MANIFEST-000001
|
||||
2025/06/26-15:18:11.207 8088 Recovering log #3
|
||||
2025/06/26-15:18:11.210 8088 Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Session Storage/000003.log
|
||||
|
|
|
|||
|
|
@ -1,3 +1,3 @@
|
|||
2025/06/20-16:52:41.925 1514 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Session Storage/MANIFEST-000001
|
||||
2025/06/20-16:52:41.927 1514 Recovering log #3
|
||||
2025/06/20-16:52:41.930 1514 Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Session Storage/000003.log
|
||||
2025/06/26-10:44:15.822 6078 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Session Storage/MANIFEST-000001
|
||||
2025/06/26-10:44:15.823 6078 Recovering log #3
|
||||
2025/06/26-10:44:15.829 6078 Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Session Storage/000003.log
|
||||
|
|
|
|||
|
|
@ -1,3 +1,3 @@
|
|||
2025/06/20-16:52:51.127 5554 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Site Characteristics Database/MANIFEST-000001
|
||||
2025/06/20-16:52:51.129 5554 Recovering log #3
|
||||
2025/06/20-16:52:51.130 5554 Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Site Characteristics Database/000003.log
|
||||
2025/06/26-10:44:24.143 1508 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Site Characteristics Database/MANIFEST-000001
|
||||
2025/06/26-10:44:24.145 1508 Recovering log #3
|
||||
2025/06/26-10:44:24.146 1508 Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Site Characteristics Database/000003.log
|
||||
|
|
|
|||
|
|
@ -1,3 +1,3 @@
|
|||
2025/06/20-09:13:55.995 3d14 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Site Characteristics Database/MANIFEST-000001
|
||||
2025/06/20-09:13:55.997 3d14 Recovering log #3
|
||||
2025/06/20-09:13:55.997 3d14 Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Site Characteristics Database/000003.log
|
||||
2025/06/26-10:22:26.868 205c Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Site Characteristics Database/MANIFEST-000001
|
||||
2025/06/26-10:22:26.870 205c Recovering log #3
|
||||
2025/06/26-10:22:26.870 205c Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Site Characteristics Database/000003.log
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -1 +1 @@
|
|||
137.0.3296.83
|
||||
137.0.3296.93
|
||||
File diff suppressed because one or more lines are too long
Binary file not shown.
Binary file not shown.
File diff suppressed because one or more lines are too long
|
|
@ -17,7 +17,7 @@
|
|||
<meta name="viewport" content="width=device-width,
|
||||
initial-scale=1.0, maximum-scale=1.0, user-scalable=no" />
|
||||
<style>
|
||||
#map_78d55b161602f4f5843bc14d0066ecec {
|
||||
#map_8cbbfc19e2a4c0a6eea90701b0e88f16 {
|
||||
position: relative;
|
||||
width: 100.0%;
|
||||
height: 100.0%;
|
||||
|
|
@ -54,14 +54,14 @@
|
|||
<body>
|
||||
|
||||
|
||||
<div class="folium-map" id="map_78d55b161602f4f5843bc14d0066ecec" ></div>
|
||||
<div class="folium-map" id="map_8cbbfc19e2a4c0a6eea90701b0e88f16" ></div>
|
||||
|
||||
</body>
|
||||
<script>
|
||||
|
||||
|
||||
var map_78d55b161602f4f5843bc14d0066ecec = L.map(
|
||||
"map_78d55b161602f4f5843bc14d0066ecec",
|
||||
var map_8cbbfc19e2a4c0a6eea90701b0e88f16 = L.map(
|
||||
"map_8cbbfc19e2a4c0a6eea90701b0e88f16",
|
||||
{
|
||||
center: [-22.172636164916668, -47.395186322185666],
|
||||
crs: L.CRS.EPSG3857,
|
||||
|
|
@ -78,7 +78,7 @@
|
|||
|
||||
|
||||
|
||||
var tile_layer_cb1bbbce40090f2c71fdcb5fd4ad4670 = L.tileLayer(
|
||||
var tile_layer_9013db21f6eca3782c27866d5a1afa86 = L.tileLayer(
|
||||
"https://tile.openstreetmap.org/{z}/{x}/{y}.png",
|
||||
{
|
||||
"minZoom": 0,
|
||||
|
|
@ -95,7 +95,7 @@
|
|||
);
|
||||
|
||||
|
||||
tile_layer_cb1bbbce40090f2c71fdcb5fd4ad4670.addTo(map_78d55b161602f4f5843bc14d0066ecec);
|
||||
tile_layer_9013db21f6eca3782c27866d5a1afa86.addTo(map_8cbbfc19e2a4c0a6eea90701b0e88f16);
|
||||
|
||||
|
||||
|
||||
|
|
@ -111,7 +111,7 @@
|
|||
}*/
|
||||
});
|
||||
}
|
||||
function geo_json_13fe334f8dc8f0340d67aca253f89711_onEachFeature(feature, layer) {
|
||||
function geo_json_9504ddffaf9242962edc59fd8f534970_onEachFeature(feature, layer) {
|
||||
|
||||
layer.on({
|
||||
|
||||
|
|
@ -148,23 +148,23 @@
|
|||
}*/
|
||||
});
|
||||
};
|
||||
var geo_json_13fe334f8dc8f0340d67aca253f89711 = L.geoJson(null, {
|
||||
onEachFeature: geo_json_13fe334f8dc8f0340d67aca253f89711_onEachFeature,
|
||||
var geo_json_9504ddffaf9242962edc59fd8f534970 = L.geoJson(null, {
|
||||
onEachFeature: geo_json_9504ddffaf9242962edc59fd8f534970_onEachFeature,
|
||||
|
||||
...{
|
||||
}
|
||||
});
|
||||
|
||||
function geo_json_13fe334f8dc8f0340d67aca253f89711_add (data) {
|
||||
geo_json_13fe334f8dc8f0340d67aca253f89711
|
||||
function geo_json_9504ddffaf9242962edc59fd8f534970_add (data) {
|
||||
geo_json_9504ddffaf9242962edc59fd8f534970
|
||||
.addData(data);
|
||||
}
|
||||
geo_json_13fe334f8dc8f0340d67aca253f89711_add({"features": [{"geometry": {"coordinates": [[-47.395205344, -22.172559531333334], [-47.395212610166666, -22.172614638833334], [-47.395219157, -22.172656417833334], [-47.395223544833335, -22.1726892105], [-47.39522414098443, -22.17269369161165], [-47.395225326538004, -22.172702654611555]], "id": null, "type": "LineString"}, "id": 0, "properties": {"Dist1": 14.558011415731592, "Dist2": 0.0, "Id": "1", "Length": 14.558011415731592, "Name": "CidadeJardimTerreno2"}, "type": "Feature"}, {"geometry": {"coordinates": [[-47.39521090233333, -22.172708800833334], [-47.395206943666665, -22.172679811], [-47.395202420666664, -22.1726491015], [-47.395198865666664, -22.172615782833333], [-47.395193255833334, -22.172577132833332], [-47.395192610024395, -22.17257265769541], [-47.395191325698136, -22.172563706509337]], "id": null, "type": "LineString"}, "id": 1, "properties": {"Dist1": 14.558011415731592, "Dist2": 0.0, "Id": "2", "Length": 14.771318274761821, "Name": "CidadeJardimTerreno2"}, "type": "Feature"}, {"geometry": {"coordinates": [[-47.3951762925, -22.172561603], [-47.395180824166665, -22.172591686166665], [-47.395185745333336, -22.172623080166666], [-47.395190433, -22.172656367166667], [-47.395195199, -22.172693641833334], [-47.39519576904436, -22.172698125891035], [-47.39519690267159, -22.172707094716973]], "id": null, "type": "LineString"}, "id": 2, "properties": {"Dist1": 14.558011415731592, "Dist2": 0.0, "Id": "3", "Length": 14.827915524386164, "Name": "CidadeJardimTerreno2"}, "type": "Feature"}, {"geometry": {"coordinates": [[-47.39518293216667, -22.1727127985], [-47.39517819266667, -22.172680514833335], [-47.3951732595, -22.172646752833334], [-47.39516880516667, -22.1726149155], [-47.39516381233334, -22.172581167166665], [-47.39516315429932, -22.172576693573966], [-47.39516184565584, -22.172567745443878]], "id": null, "type": "LineString"}, "id": 3, "properties": {"Dist1": 14.558011415731592, "Dist2": 0.0, "Id": "4", "Length": 14.785159385903514, "Name": "CidadeJardimTerreno2"}, "type": "Feature"}, {"geometry": {"coordinates": [[-47.395147317833334, -22.172566031], [-47.395151503166666, -22.172595452333333], [-47.3951561855, -22.172628011166665], [-47.39516159866667, -22.172663757333332], [-47.39516672716667, -22.172697770833334], [-47.395167397572706, -22.172702242832194], [-47.39516873082615, -22.17271118781009]], "id": null, "type": "LineString"}, "id": 4, "properties": {"Dist1": 14.558011415731592, "Dist2": 0.0, "Id": "5", "Length": 14.80117692191233, "Name": "CidadeJardimTerreno2"}, "type": "Feature"}], "type": "FeatureCollection"});
|
||||
geo_json_13fe334f8dc8f0340d67aca253f89711.setStyle(function(feature) {return feature.properties.style;});
|
||||
geo_json_9504ddffaf9242962edc59fd8f534970_add({"features": [{"geometry": {"coordinates": [[-47.395205344, -22.172559531333334], [-47.395212610166666, -22.172614638833334], [-47.395219157, -22.172656417833334], [-47.395223544833335, -22.1726892105], [-47.39522414098443, -22.17269369161165], [-47.395225326538004, -22.172702654611555]], "id": null, "type": "LineString"}, "id": 0, "properties": {"Dist1": 14.558011415731592, "Dist2": 0.0, "Id": "1", "Length": 14.558011415731592, "Name": "CidadeJardimTerreno2"}, "type": "Feature"}, {"geometry": {"coordinates": [[-47.39521090233333, -22.172708800833334], [-47.395206943666665, -22.172679811], [-47.395202420666664, -22.1726491015], [-47.395198865666664, -22.172615782833333], [-47.395193255833334, -22.172577132833332], [-47.395192610024395, -22.17257265769541], [-47.395191325698136, -22.172563706509337]], "id": null, "type": "LineString"}, "id": 1, "properties": {"Dist1": 14.558011415731592, "Dist2": 0.0, "Id": "2", "Length": 14.771318274761821, "Name": "CidadeJardimTerreno2"}, "type": "Feature"}, {"geometry": {"coordinates": [[-47.3951762925, -22.172561603], [-47.395180824166665, -22.172591686166665], [-47.395185745333336, -22.172623080166666], [-47.395190433, -22.172656367166667], [-47.395195199, -22.172693641833334], [-47.39519576904436, -22.172698125891035], [-47.39519690267159, -22.172707094716973]], "id": null, "type": "LineString"}, "id": 2, "properties": {"Dist1": 14.558011415731592, "Dist2": 0.0, "Id": "3", "Length": 14.827915524386164, "Name": "CidadeJardimTerreno2"}, "type": "Feature"}, {"geometry": {"coordinates": [[-47.39518293216667, -22.1727127985], [-47.39517819266667, -22.172680514833335], [-47.3951732595, -22.172646752833334], [-47.39516880516667, -22.1726149155], [-47.39516381233334, -22.172581167166665], [-47.39516315429932, -22.172576693573966], [-47.39516184565584, -22.172567745443878]], "id": null, "type": "LineString"}, "id": 3, "properties": {"Dist1": 14.558011415731592, "Dist2": 0.0, "Id": "4", "Length": 14.785159385903514, "Name": "CidadeJardimTerreno2"}, "type": "Feature"}, {"geometry": {"coordinates": [[-47.395147317833334, -22.172566031], [-47.395151503166666, -22.172595452333333], [-47.3951561855, -22.172628011166665], [-47.39516159866667, -22.172663757333332], [-47.39516672716667, -22.172697770833334], [-47.395167397572706, -22.172702242832194], [-47.39516873082615, -22.17271118781009]], "id": null, "type": "LineString"}, "id": 4, "properties": {"Dist1": 14.558011415731592, "Dist2": 0.0, "Id": "5", "Length": 14.80117692191233, "Name": "CidadeJardimTerreno2"}, "type": "Feature"}], "type": "FeatureCollection"});
|
||||
geo_json_9504ddffaf9242962edc59fd8f534970.setStyle(function(feature) {return feature.properties.style;});
|
||||
|
||||
|
||||
|
||||
geo_json_13fe334f8dc8f0340d67aca253f89711.addTo(map_78d55b161602f4f5843bc14d0066ecec);
|
||||
geo_json_9504ddffaf9242962edc59fd8f534970.addTo(map_8cbbfc19e2a4c0a6eea90701b0e88f16);
|
||||
|
||||
</script>
|
||||
|
||||
|
|
@ -185,7 +185,7 @@
|
|||
}
|
||||
trajeto_json_add({"features": []});
|
||||
|
||||
trajeto_json.addTo(map_78d55b161602f4f5843bc14d0066ecec);
|
||||
trajeto_json.addTo(map_8cbbfc19e2a4c0a6eea90701b0e88f16);
|
||||
|
||||
function adicionarGeometria(novaGeometria) {
|
||||
trajeto_json.addData(novaGeometria);
|
||||
|
|
@ -243,7 +243,7 @@
|
|||
}
|
||||
trajeto_dinamico_json_add({"features": []});
|
||||
|
||||
trajeto_dinamico_json.addTo(map_78d55b161602f4f5843bc14d0066ecec);
|
||||
trajeto_dinamico_json.addTo(map_8cbbfc19e2a4c0a6eea90701b0e88f16);
|
||||
|
||||
function adicionarGeometriaDinamica(novaGeometria) {
|
||||
trajeto_dinamico_json.addData(novaGeometria);
|
||||
|
|
@ -296,9 +296,9 @@
|
|||
|
||||
var marcadorEquipamento = L.marker([0, 0], {
|
||||
icon: customIcon
|
||||
}).addTo(map_78d55b161602f4f5843bc14d0066ecec);
|
||||
}).addTo(map_8cbbfc19e2a4c0a6eea90701b0e88f16);
|
||||
|
||||
var marcadorBase = L.marker([0, 0], {}).addTo(map_78d55b161602f4f5843bc14d0066ecec);
|
||||
var marcadorBase = L.marker([0, 0], {}).addTo(map_8cbbfc19e2a4c0a6eea90701b0e88f16);
|
||||
var icon = L.AwesomeMarkers.icon(
|
||||
{"extraClasses": "fa-rotate-0", "icon": "info-sign", "iconColor": "white", "markerColor": "red", "prefix": "glyphicon"}
|
||||
);
|
||||
|
|
@ -380,7 +380,7 @@
|
|||
}
|
||||
|
||||
if (foco) {
|
||||
map_78d55b161602f4f5843bc14d0066ecec.setView(novaPosicao, map_78d55b161602f4f5843bc14d0066ecec.getZoom());
|
||||
map_8cbbfc19e2a4c0a6eea90701b0e88f16.setView(novaPosicao, map_8cbbfc19e2a4c0a6eea90701b0e88f16.getZoom());
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -397,7 +397,7 @@
|
|||
function atualizarSelecaoRuas(selecionadas) {
|
||||
selecionadas = JSON.parse(selecionadas);
|
||||
RuasSelecionadas = Array.isArray(selecionadas) ? [...selecionadas] : [];
|
||||
geo_json_13fe334f8dc8f0340d67aca253f89711.eachLayer(function (layer) {
|
||||
geo_json_9504ddffaf9242962edc59fd8f534970.eachLayer(function (layer) {
|
||||
if (RuasSelecionadas.includes(parseInt(layer.feature.id))) {
|
||||
layer.setStyle({ color: 'blue' });
|
||||
} else {
|
||||
|
|
|
|||
|
|
@ -0,0 +1,245 @@
|
|||
import numpy as np
|
||||
import depthai as dai
|
||||
import time
|
||||
|
||||
from health_worker.modulos.imu import IMUCamera
|
||||
from shared.enums import StatusModulo, T_Code
|
||||
|
||||
class CameraOak:
|
||||
def __init__(self, mostrar_log, dev_info):
|
||||
self.mostrar_log = mostrar_log
|
||||
self.dev_info = dev_info
|
||||
self.mx_id = dev_info.getMxId()
|
||||
self.versao = dev_info.name
|
||||
self.dispositivo = T_Code.Vzo
|
||||
self.modelo = "Desconhecido"
|
||||
self.ultima_saude = None
|
||||
self.timestamp_ultimo_frame_rgb = None
|
||||
self.timestamp_ultimo_frame_depth = None
|
||||
self.tem_depth = False
|
||||
self.tem_imu = False
|
||||
self.rodando = False
|
||||
|
||||
# Fase 1: detectar sensores sem pipeline
|
||||
try:
|
||||
device_tmp = dai.Device(dev_info)
|
||||
sensores = device_tmp.getConnectedCameras()
|
||||
device_tmp.close()
|
||||
|
||||
if dai.CameraBoardSocket.LEFT in sensores and dai.CameraBoardSocket.RIGHT in sensores:
|
||||
self.modelo = "OAK-D Lite"
|
||||
self.dispositivo = T_Code.Snr
|
||||
self.tem_depth = True
|
||||
elif dai.CameraBoardSocket.CAM_A in sensores:
|
||||
self.modelo = "OAK-1 Lite W"
|
||||
self.dispositivo = T_Code.Cam
|
||||
self.tem_depth = False
|
||||
else:
|
||||
self.mostrar_log(f"[WARN] Sensores desconhecidos: {sensores}")
|
||||
except Exception as e:
|
||||
self.mostrar_log(f"[ERRO] Falha ao detectar sensores: {e}")
|
||||
|
||||
# Fase 2: criar pipeline e instanciar normalmente
|
||||
self.pipeline = self._criar_pipeline()
|
||||
self.device = dai.Device(self.pipeline, self.dev_info)
|
||||
self.q_video = self.device.getOutputQueue(name="video", maxSize=1, blocking=False)
|
||||
|
||||
if self.tem_depth:
|
||||
self.q_depth = self.device.getOutputQueue(name="depth", maxSize=1, blocking=False)
|
||||
self.parametros = {
|
||||
"rgb_width": 1920,
|
||||
"rgb_height": 1080,
|
||||
"depth_width": 640,
|
||||
"depth_height": 400,
|
||||
"fx": 440.0,
|
||||
"fy": 440.0,
|
||||
"baseline": 0.075,
|
||||
"fov_h": 1,
|
||||
"fov_v": 1,
|
||||
"qtd_setores_solo": 25,
|
||||
"distancia_maxima": 3000,
|
||||
"percentual_altura_solo": 30,
|
||||
}
|
||||
self._set_calib()
|
||||
|
||||
if "imu" in self.device.getOutputQueueNames():
|
||||
self.q_imu = self.device.getOutputQueue(name="imu", maxSize=50, blocking=False)
|
||||
self.imu = IMUCamera(self.q_imu)
|
||||
self.tem_imu = True
|
||||
else:
|
||||
self.tem_imu = False
|
||||
|
||||
def _criar_pipeline(self):
|
||||
pipeline = dai.Pipeline()
|
||||
|
||||
# RGB
|
||||
cam = pipeline.create(dai.node.ColorCamera)
|
||||
cam.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1080_P)
|
||||
cam.setInterleaved(False)
|
||||
cam.setBoardSocket(dai.CameraBoardSocket.CAM_A)
|
||||
xout = pipeline.create(dai.node.XLinkOut)
|
||||
xout.setStreamName("video")
|
||||
cam.video.link(xout.input)
|
||||
self.mostrar_log(f"Pipeline rgb criado")
|
||||
|
||||
# DEPTH (somente se sensores presentes)
|
||||
if getattr(self, "tem_depth", False):
|
||||
try:
|
||||
mono_left = pipeline.create(dai.node.MonoCamera)
|
||||
mono_right = pipeline.create(dai.node.MonoCamera)
|
||||
stereo = pipeline.create(dai.node.StereoDepth)
|
||||
|
||||
mono_left.setResolution(dai.MonoCameraProperties.SensorResolution.THE_480_P)
|
||||
mono_right.setResolution(dai.MonoCameraProperties.SensorResolution.THE_480_P)
|
||||
mono_left.setBoardSocket(dai.CameraBoardSocket.LEFT)
|
||||
mono_right.setBoardSocket(dai.CameraBoardSocket.RIGHT)
|
||||
|
||||
stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.ROBOTICS)
|
||||
stereo.setDepthAlign(dai.CameraBoardSocket.RGB)
|
||||
|
||||
mono_left.out.link(stereo.left)
|
||||
mono_right.out.link(stereo.right)
|
||||
|
||||
xout_depth = pipeline.create(dai.node.XLinkOut)
|
||||
xout_depth.setStreamName("depth")
|
||||
stereo.depth.link(xout_depth.input)
|
||||
self.mostrar_log(f"Pipeline depth criado")
|
||||
except Exception as e:
|
||||
self.mostrar_log(f"[WARN] Falha ao montar pipeline depth: {e}")
|
||||
|
||||
# IMU (somente se presente)
|
||||
if getattr(self, "tem_depth", False):
|
||||
try:
|
||||
imu = pipeline.create(dai.node.IMU)
|
||||
imu.enableIMUSensor(dai.IMUSensor.ACCELEROMETER_RAW, 60)
|
||||
imu.enableIMUSensor(dai.IMUSensor.GYROSCOPE_RAW, 60)
|
||||
imu.setBatchReportThreshold(1)
|
||||
imu.setMaxBatchReports(20)
|
||||
xoutImu = pipeline.create(dai.node.XLinkOut)
|
||||
xoutImu.setStreamName("imu")
|
||||
imu.out.link(xoutImu.input)
|
||||
self.mostrar_log(f"Pipeline imu criado")
|
||||
except Exception as e:
|
||||
self.mostrar_log(f"[WARN] Falha ao montar pipeline imu: {e}")
|
||||
|
||||
return pipeline
|
||||
|
||||
def _set_calib(self):
|
||||
calib = self.device.readCalibration()
|
||||
|
||||
# Obtem matriz intrínseca da câmera LEFT (com resolução padrão 640x400)
|
||||
intrinsics = calib.getCameraIntrinsics(dai.CameraBoardSocket.LEFT, self.parametros["depth_width"], self.parametros["depth_height"])
|
||||
self.parametros["fx"] = intrinsics[0][0] # fx
|
||||
self.parametros["fy"] = intrinsics[1][1] # fy
|
||||
self.parametros["baseline"] = calib.getBaselineDistance() / 100.0 # de cm → m
|
||||
self.parametros["fov_h"] = 2 * np.arctan(self.parametros["depth_width"] / (2 * self.parametros["fx"]))
|
||||
self.parametros["fov_v"] = 2 * np.arctan(self.parametros["depth_height"] / (2 * self.parametros["fy"]))
|
||||
self.parametros["fov_d"] = np.radians(1.0)
|
||||
|
||||
#self.mostrar_log(f"[CALIB] {self.parametros}")
|
||||
|
||||
def requisitar_frame_rgb(self):
|
||||
#print("Requisitando frame")
|
||||
try:
|
||||
start = time.time()
|
||||
frame = self.q_video.get().getCvFrame()
|
||||
dur = time.time() - start
|
||||
self.timestamp_ultimo_frame_rgb = time.time()
|
||||
|
||||
resultado = {
|
||||
"erro": None,
|
||||
"duracao": dur,
|
||||
"frame_valido": frame is not None and frame.shape[0] > 0
|
||||
}
|
||||
|
||||
return frame, resultado
|
||||
|
||||
except Exception as e:
|
||||
return None, {
|
||||
"erro": str(e),
|
||||
"duracao": 0,
|
||||
"frame_valido": False
|
||||
}
|
||||
|
||||
def requisitar_frame_depth(self):
|
||||
try:
|
||||
start = time.time()
|
||||
frame = self.q_depth.get().getFrame()
|
||||
dur = time.time() - start
|
||||
self.timestamp_ultimo_frame_depth = time.time()
|
||||
|
||||
return frame, {
|
||||
"erro": None,
|
||||
"duracao": dur,
|
||||
"frame_valido": frame is not None and frame.shape[0] > 0
|
||||
}
|
||||
|
||||
except Exception as e:
|
||||
return None, {
|
||||
"erro": str(e),
|
||||
"duracao": 0,
|
||||
"frame_valido": False
|
||||
}
|
||||
|
||||
def requisitar_imu_status(self):
|
||||
if getattr(self, "tem_imu", False):
|
||||
return self.imu.get_imu_status()
|
||||
else:
|
||||
return None
|
||||
|
||||
def atualizar_saude(self):
|
||||
#print("Atualizando saude")
|
||||
frame, resultado = self.requisitar_frame_rgb()
|
||||
|
||||
saude = 50
|
||||
motivos = []
|
||||
|
||||
if resultado["erro"]:
|
||||
motivos.append(resultado["erro"])
|
||||
saude = 0
|
||||
elif not resultado["frame_valido"]:
|
||||
motivos.append("Frame inválido ou vazio")
|
||||
saude = 0
|
||||
else:
|
||||
saude += 50
|
||||
if resultado["duracao"] > 1.0:
|
||||
saude -= 20
|
||||
motivos.append(f"Tempo elevado para captura: {resultado['duracao']:.2f}s")
|
||||
|
||||
status = StatusModulo.OPERANTE
|
||||
if saude <= 0:
|
||||
status = StatusModulo.FALHA
|
||||
elif saude < 80:
|
||||
status = StatusModulo.ALERTA
|
||||
|
||||
from shared.contexto_global import ContextoGlobal
|
||||
ContextoGlobal.set_item_dado(self.dispositivo, "saude", "status", status)
|
||||
ContextoGlobal.set_item_dado(self.dispositivo, "saude", "saude", saude)
|
||||
ContextoGlobal.set_item_dado(self.dispositivo, "saude", "motivos", motivos)
|
||||
ContextoGlobal.set_item_dado(self.dispositivo, "saude", "saude_individual", [])
|
||||
|
||||
agora = time.time()
|
||||
timeout = 2.0
|
||||
|
||||
ts_depth = self.timestamp_ultimo_frame_depth or 0
|
||||
ts_rgb = self.timestamp_ultimo_frame_rgb or 0
|
||||
|
||||
self.rodando = (
|
||||
(agora - ts_depth) <= timeout
|
||||
or (agora - ts_rgb) <= timeout
|
||||
)
|
||||
|
||||
self.ultima_saude = {
|
||||
"status": status,
|
||||
"saude": max(0, min(100, saude)),
|
||||
"motivos": motivos,
|
||||
"saude_individual": []
|
||||
}
|
||||
|
||||
def to_dict(self):
|
||||
return {
|
||||
"dispositivo": self.dispositivo.value,
|
||||
"mx_id": self.mx_id,
|
||||
"versao": self.versao,
|
||||
"modelo": self.modelo
|
||||
}
|
||||
|
|
@ -0,0 +1,17 @@
|
|||
import time
|
||||
|
||||
from camera_worker.manager import CameraManager
|
||||
|
||||
module_id = "camera"
|
||||
topico_tx = f"operador/{module_id}/tx"
|
||||
topico_rx = f"operador/{module_id}/rx"
|
||||
debug = True
|
||||
|
||||
def mostrar_log(mensagem):
|
||||
if debug:
|
||||
print(f"{time.time()} - [{module_id}] {mensagem}")
|
||||
|
||||
manager = CameraManager(mostrar_log)
|
||||
|
||||
def get_camera_manager():
|
||||
return manager
|
||||
|
|
@ -0,0 +1,72 @@
|
|||
import sys
|
||||
import os
|
||||
import time
|
||||
import json
|
||||
|
||||
def main():
|
||||
sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), "..")))
|
||||
print("🔎 Caminho sys.path:", os.path.abspath(os.path.join(os.path.dirname(__file__), "..")), "CameraWorker")
|
||||
|
||||
from shared.mqtt_handler import iniciar_mqtt, registrar_callback, enviar_mensagem_mqtt, enviar_mensagem_script_carregado
|
||||
from shared.mensagem_worker import WorkerFilaMensagens
|
||||
from shared.enums import T_Code, TipoComando
|
||||
from camera_worker.config import mostrar_log, module_id, topico_tx, topico_rx, get_camera_manager
|
||||
|
||||
|
||||
# 🔹 Callback que o MQTT chama
|
||||
def ao_receber_comando(topico, payload):
|
||||
fila_mensagens.adicionar(topico, payload)
|
||||
|
||||
# 🔹 Função de processamento dos comandos recebidos
|
||||
def processar_comando(topico, message):
|
||||
try:
|
||||
# Aqui você decodifica o comando e processa normalmente
|
||||
mensagem = json.loads(message.decode())
|
||||
dispositivo = T_Code(mensagem.get("dispositivo"))
|
||||
tipo_comando = TipoComando(mensagem.get("comando"))
|
||||
payload = mensagem.get("payload", {})
|
||||
|
||||
#mostrar_log(f"📨 Mensagem recebida no tópico {topico}. Comando: {tipo_comando.name}, dispositivo {dispositivo.name}, payload: {payload}")
|
||||
#mostrar_log(f"Comando recebido: {dispositivo}")
|
||||
|
||||
resposta = {}
|
||||
|
||||
if (resposta != {}):
|
||||
resposta["cmd"] = tipo_comando
|
||||
enviar_mensagem_mqtt(topico=topico_tx, dispositivo=dispositivo,objeto=resposta)
|
||||
|
||||
except Exception as e:
|
||||
mostrar_log(f"❌ Erro ao processar comando MQTT: {e}")
|
||||
|
||||
# 🔹 Inicia a fila e os workers
|
||||
fila_mensagens = WorkerFilaMensagens(id=module_id, processador_callback=processar_comando)
|
||||
|
||||
# 🔹 Loop principal do Manager
|
||||
def loop_ativo():
|
||||
manager = get_camera_manager()
|
||||
while True:
|
||||
try:
|
||||
manager.atualizar_cameras()
|
||||
|
||||
except KeyboardInterrupt:
|
||||
mostrar_log("🛑 Encerrando Camera Worker...")
|
||||
break
|
||||
except Exception as e:
|
||||
mostrar_log(f"❌ Erro no loop do Manager: {e}")
|
||||
|
||||
time.sleep(5)
|
||||
|
||||
# 🔹 Execução principal
|
||||
def inicializar():
|
||||
iniciar_mqtt(module_id)
|
||||
|
||||
mostrar_log("🚀 Iniciando Camera Worker...")
|
||||
registrar_callback(topico=topico_rx, callback=ao_receber_comando)
|
||||
enviar_mensagem_script_carregado(topico_tx)
|
||||
loop_ativo()
|
||||
|
||||
inicializar()
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
||||
|
|
@ -0,0 +1,83 @@
|
|||
import time
|
||||
import depthai as dai
|
||||
from .camera_oak import CameraOak
|
||||
from multiprocessing import Process, Queue
|
||||
|
||||
class CameraManager:
|
||||
def __init__(self, mostrar_log):
|
||||
self.cameras = []
|
||||
self.mostrar_log = mostrar_log
|
||||
self._ultimo_scan = 0
|
||||
|
||||
def atualizar_cameras(self):
|
||||
if time.time() - self._ultimo_scan < 10:
|
||||
return
|
||||
self._ultimo_scan = time.time()
|
||||
|
||||
try:
|
||||
dispositivos = dai.Device.getAllAvailableDevices()
|
||||
dispositivos_mapeados = [d.getMxId() for d in dispositivos]
|
||||
except Exception as e:
|
||||
dispositivos_mapeados = []
|
||||
|
||||
if len(dispositivos_mapeados) == 0:
|
||||
self.mostrar_log("⚠️ Nenhum dispositivo retornado.")
|
||||
dispositivos_serializados = []
|
||||
else:
|
||||
dispositivos_serializados = dispositivos_mapeados
|
||||
|
||||
novas_cameras = []
|
||||
|
||||
for mx_id in dispositivos_serializados:
|
||||
existente = next((c for c in self.cameras if c.mx_id == mx_id), None)
|
||||
|
||||
if existente:
|
||||
existente.atualizar_saude()
|
||||
novas_cameras.append(existente)
|
||||
else:
|
||||
if not self._is_camera_em_uso(mx_id):
|
||||
print("em uso: False")
|
||||
try:
|
||||
dev_info = dai.DeviceInfo(mx_id)
|
||||
nova = CameraOak(self.mostrar_log, dev_info)
|
||||
nova.atualizar_saude()
|
||||
novas_cameras.append(nova)
|
||||
except Exception as e:
|
||||
self.mostrar_log(f"[ERRO] Falha ao iniciar camera {dev_info}: {e}")
|
||||
else:
|
||||
print("em uso: True")
|
||||
|
||||
if len(novas_cameras) > 0:
|
||||
self.cameras = novas_cameras
|
||||
|
||||
for cam in self.cameras:
|
||||
cam.atualizar_saude()
|
||||
|
||||
if len(self.cameras) > 0:
|
||||
a = 1
|
||||
# enviar mensagem mqtt para tentar atualizar o camera manager agora que existe alguma camera mapeada
|
||||
|
||||
def _is_camera_em_uso(self, mx_id):
|
||||
for c in self.cameras:
|
||||
if c.mx_id == mx_id and c.rodando:
|
||||
return True
|
||||
return False
|
||||
|
||||
def get_cameras(self, mx_id = None):
|
||||
if mx_id == None:
|
||||
return self.cameras
|
||||
else:
|
||||
camera = next((c for c in self.cameras if c.mx_id == mx_id), None)
|
||||
return camera
|
||||
|
||||
def get_cameras_dict(self):
|
||||
return [cam.to_dict() for cam in self.cameras]
|
||||
|
||||
def get_camera_frame(self, mx_id):
|
||||
cam = next((c for c in self.cameras if c.mx_id == mx_id), None)
|
||||
if cam != None:
|
||||
frame, res = cam.requisitar_frame_rgb()
|
||||
|
||||
return frame, cam.timestamp_ultimo_frame_rgb, res
|
||||
else:
|
||||
return None, None, {}
|
||||
|
|
@ -3,129 +3,134 @@ import os
|
|||
import time
|
||||
import json
|
||||
|
||||
sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), "..")))
|
||||
print("🔎 Caminho sys.path:", os.path.abspath(os.path.join(os.path.dirname(__file__), "..")))
|
||||
def main():
|
||||
sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), "..")))
|
||||
print("🔎 Caminho sys.path:", os.path.abspath(os.path.join(os.path.dirname(__file__), "..")), "HealthWorker")
|
||||
|
||||
from shared.mqtt_handler import registrar_callback, enviar_mensagem_mqtt, enviar_mensagem_script_carregado
|
||||
from shared.mensagem_worker import WorkerFilaMensagens
|
||||
from shared.enums import T_Code, TipoComando
|
||||
from shared.contexto_global import ContextoGlobal
|
||||
from health_worker.modulos.gps import ModuloGPS
|
||||
from health_worker.modulos.direcional import ModuloDirecional
|
||||
from health_worker.modulos.movimentacao import ModuloMovimentacao
|
||||
from health_worker.modulos.sensoriamento import ModuloSensoriamento
|
||||
from health_worker.config import mostrar_log, module_id, topico_tx, topico_rx, get_camera_manager
|
||||
from shared.mqtt_handler import iniciar_mqtt, registrar_callback, enviar_mensagem_mqtt, enviar_mensagem_script_carregado
|
||||
from shared.mensagem_worker import WorkerFilaMensagens
|
||||
from shared.enums import T_Code, TipoComando
|
||||
from shared.contexto_global import ContextoGlobal
|
||||
from health_worker.modulos.gps import ModuloGPS
|
||||
from health_worker.modulos.direcional import ModuloDirecional
|
||||
from health_worker.modulos.movimentacao import ModuloMovimentacao
|
||||
from health_worker.modulos.sensoriamento import ModuloSensoriamento
|
||||
from health_worker.config import mostrar_log, module_id, topico_tx, topico_rx, get_camera_manager
|
||||
|
||||
|
||||
|
||||
modulos = {
|
||||
T_Code.Gps: ModuloGPS(),
|
||||
T_Code.Dir: ModuloDirecional(),
|
||||
T_Code.Mov: ModuloMovimentacao(),
|
||||
T_Code.Sen: ModuloSensoriamento(),
|
||||
}
|
||||
modulos = {
|
||||
T_Code.Gps: ModuloGPS(),
|
||||
T_Code.Dir: ModuloDirecional(),
|
||||
T_Code.Mov: ModuloMovimentacao(),
|
||||
T_Code.Sen: ModuloSensoriamento(),
|
||||
}
|
||||
|
||||
# 🔹 Callback que o MQTT chama
|
||||
def ao_receber_comando(topico, payload):
|
||||
fila_mensagens.adicionar(topico, payload)
|
||||
# 🔹 Callback que o MQTT chama
|
||||
def ao_receber_comando(topico, payload):
|
||||
fila_mensagens.adicionar(topico, payload)
|
||||
|
||||
# 🔹 Função de processamento dos comandos recebidos
|
||||
def processar_comando(topico, message):
|
||||
try:
|
||||
# Aqui você decodifica o comando e processa normalmente
|
||||
mensagem = json.loads(message.decode())
|
||||
dispositivo = T_Code(mensagem.get("dispositivo"))
|
||||
tipo_comando = TipoComando(mensagem.get("comando"))
|
||||
payload = mensagem.get("payload", {})
|
||||
|
||||
#mostrar_log(f"📨 Mensagem recebida no tópico {topico}. Comando: {tipo_comando.name}, dispositivo {dispositivo.name}, payload: {payload}")
|
||||
#mostrar_log(f"Comando recebido: {dispositivo}")
|
||||
|
||||
resposta = {}
|
||||
|
||||
if tipo_comando in (TipoComando.GetDadosModulo, TipoComando.SetDadosModulo):
|
||||
if dispositivo in modulos:
|
||||
modulo = modulos[dispositivo]
|
||||
if tipo_comando == TipoComando.SetDadosModulo:
|
||||
modulo.atualizar_status(payload)
|
||||
elif tipo_comando == TipoComando.GetDadosModulo:
|
||||
resposta = modulo.verificar_status()
|
||||
elif tipo_comando == TipoComando.SetDadosContexto:
|
||||
ContextoGlobal.atualizar_contexto(payload)
|
||||
#mostrar_log("Dados de Contexto atualizados com sucesso!")
|
||||
for chave, dados_modulo in payload.items():
|
||||
if chave not in T_Code.__members__:
|
||||
#mostrar_log(f"🔎 Ignorando chave '{chave}' (não é módulo do T_Code)")
|
||||
continue
|
||||
try:
|
||||
t_code = T_Code[chave]
|
||||
if t_code in modulos:
|
||||
modulos[t_code].atualizar_status(dados_modulo)
|
||||
except Exception as e:
|
||||
mostrar_log(f"❌ Erro ao atualizar módulo {chave}: {e}")
|
||||
elif tipo_comando == TipoComando.GetDadosContexto:
|
||||
resposta = ContextoGlobal.get_tudo()
|
||||
elif tipo_comando == TipoComando.SetDadosOperacao:
|
||||
ContextoGlobal.atualizar_operacao(payload)
|
||||
mostrar_log("Dados da Operacao atualizados com sucesso!")
|
||||
elif tipo_comando == TipoComando.GetListaCameras:
|
||||
manager = get_camera_manager()
|
||||
cameras = manager.get_cameras_dict()
|
||||
resposta = {
|
||||
"cameras": {
|
||||
"timestamp": time.time(),
|
||||
"itens": cameras
|
||||
}
|
||||
}
|
||||
elif tipo_comando == TipoComando.GetCameraFrame:
|
||||
camera_id = payload.get("dados", {}).get("mx_id", "")
|
||||
manager = get_camera_manager()
|
||||
frame, timestamp, res = manager.get_camera_frame(camera_id)
|
||||
base64_img = None
|
||||
if frame is not None:
|
||||
from shared.utils import encode_image_base64
|
||||
base64_img = encode_image_base64(frame)
|
||||
resposta = {
|
||||
"frame": {
|
||||
"timestamp": timestamp,
|
||||
"frame": base64_img
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if (resposta != {}):
|
||||
resposta["cmd"] = tipo_comando
|
||||
enviar_mensagem_mqtt(topico=topico_tx, dispositivo=dispositivo,objeto=resposta)
|
||||
|
||||
except Exception as e:
|
||||
mostrar_log(f"❌ Erro ao processar comando MQTT: {e}")
|
||||
|
||||
|
||||
|
||||
|
||||
# 🔹 Inicia a fila e os workers
|
||||
fila_mensagens = WorkerFilaMensagens(id=module_id, processador_callback=processar_comando)
|
||||
|
||||
# 🔹 Loop principal do Manager
|
||||
def loop_ativo():
|
||||
while True:
|
||||
# 🔹 Função de processamento dos comandos recebidos
|
||||
def processar_comando(topico, message):
|
||||
try:
|
||||
manager = get_camera_manager()
|
||||
manager.atualizar_cameras()
|
||||
#cameras = manager.get_cameras()
|
||||
#mostrar_log([f"Camera: {cam.modelo}, ID: {cam.mx_id}, Saude: {cam.ultima_saude}" for cam in cameras])
|
||||
# Aqui você decodifica o comando e processa normalmente
|
||||
mensagem = json.loads(message.decode())
|
||||
dispositivo = T_Code(mensagem.get("dispositivo"))
|
||||
tipo_comando = TipoComando(mensagem.get("comando"))
|
||||
payload = mensagem.get("payload", {})
|
||||
|
||||
#mostrar_log(f"📨 Mensagem recebida no tópico {topico}. Comando: {tipo_comando.name}, dispositivo {dispositivo.name}, payload: {payload}")
|
||||
#mostrar_log(f"Comando recebido: {dispositivo} {tipo_comando}")
|
||||
|
||||
resposta = {}
|
||||
|
||||
if tipo_comando in (TipoComando.GetDadosModulo, TipoComando.SetDadosModulo):
|
||||
if dispositivo in modulos:
|
||||
modulo = modulos[dispositivo]
|
||||
if tipo_comando == TipoComando.SetDadosModulo:
|
||||
modulo.atualizar_status(payload)
|
||||
elif tipo_comando == TipoComando.GetDadosModulo:
|
||||
resposta = modulo.verificar_status()
|
||||
elif tipo_comando == TipoComando.SetDadosContexto:
|
||||
ContextoGlobal.atualizar_contexto(payload)
|
||||
#mostrar_log("Dados de Contexto atualizados com sucesso!")
|
||||
for chave, dados_modulo in payload.items():
|
||||
if chave not in T_Code.__members__:
|
||||
#mostrar_log(f"🔎 Ignorando chave '{chave}' (não é módulo do T_Code)")
|
||||
continue
|
||||
try:
|
||||
t_code = T_Code[chave]
|
||||
if t_code in modulos:
|
||||
modulos[t_code].atualizar_status(dados_modulo)
|
||||
except Exception as e:
|
||||
mostrar_log(f"❌ Erro ao atualizar módulo {chave}: {e}")
|
||||
elif tipo_comando == TipoComando.GetDadosContexto:
|
||||
resposta = ContextoGlobal.get_tudo()
|
||||
elif tipo_comando == TipoComando.SetDadosOperacao:
|
||||
ContextoGlobal.atualizar_operacao(payload)
|
||||
mostrar_log("Dados da Operacao atualizados com sucesso!")
|
||||
elif tipo_comando == TipoComando.GetListaCameras:
|
||||
manager = get_camera_manager()
|
||||
cameras = manager.get_cameras_dict()
|
||||
resposta = {
|
||||
"cameras": {
|
||||
"timestamp": time.time(),
|
||||
"itens": cameras
|
||||
}
|
||||
}
|
||||
elif tipo_comando == TipoComando.GetCameraFrame:
|
||||
camera_id = payload.get("dados", {}).get("mx_id", "")
|
||||
manager = get_camera_manager()
|
||||
frame, timestamp, res = manager.get_camera_frame(camera_id)
|
||||
base64_img = None
|
||||
if frame is not None:
|
||||
from shared.utils import encode_image_base64
|
||||
base64_img = encode_image_base64(frame)
|
||||
resposta = {
|
||||
"frame": {
|
||||
"timestamp": timestamp,
|
||||
"frame": base64_img
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if (resposta != {}):
|
||||
resposta["cmd"] = tipo_comando
|
||||
enviar_mensagem_mqtt(topico=topico_tx, dispositivo=dispositivo,objeto=resposta)
|
||||
|
||||
except KeyboardInterrupt:
|
||||
mostrar_log("🛑 Encerrando Health Worker...")
|
||||
break
|
||||
except Exception as e:
|
||||
mostrar_log(f"❌ Erro no loop do Manager: {e}")
|
||||
mostrar_log(f"❌ Erro ao processar comando MQTT: {e}")
|
||||
|
||||
time.sleep(5)
|
||||
# 🔹 Inicia a fila e os workers
|
||||
fila_mensagens = WorkerFilaMensagens(id=module_id, processador_callback=processar_comando)
|
||||
|
||||
# 🔹 Loop principal do Manager
|
||||
def loop_ativo():
|
||||
manager = get_camera_manager()
|
||||
while True:
|
||||
try:
|
||||
manager.atualizar_cameras()
|
||||
#cameras = manager.get_cameras()
|
||||
#mostrar_log([f"Camera: {cam.modelo}, ID: {cam.mx_id}, Saude: {cam.ultima_saude}" for cam in cameras])
|
||||
|
||||
except KeyboardInterrupt:
|
||||
mostrar_log("🛑 Encerrando Health Worker...")
|
||||
break
|
||||
except Exception as e:
|
||||
mostrar_log(f"❌ Erro no loop do Health: {e}")
|
||||
|
||||
time.sleep(5)
|
||||
|
||||
# 🔹 Execução principal
|
||||
def inicializar():
|
||||
iniciar_mqtt(module_id)
|
||||
mostrar_log("🚀 Iniciando Health Worker...")
|
||||
registrar_callback(topico=topico_rx, callback=ao_receber_comando)
|
||||
enviar_mensagem_script_carregado(topico_tx)
|
||||
loop_ativo()
|
||||
|
||||
inicializar()
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
||||
# 🔹 Execução principal
|
||||
def inicializar():
|
||||
mostrar_log("🚀 Iniciando Health Worker...")
|
||||
registrar_callback(topico=topico_rx, callback=ao_receber_comando)
|
||||
enviar_mensagem_script_carregado(topico_tx)
|
||||
loop_ativo()
|
||||
|
|
|
|||
|
|
@ -1,31 +1,57 @@
|
|||
import time
|
||||
import depthai as dai
|
||||
from shared.enums import T_Code, VisualWorkerCommandType
|
||||
from .camera_oak import CameraOak
|
||||
from multiprocessing import Process, Queue
|
||||
|
||||
class CameraManager:
|
||||
def __init__(self, mostrar_log):
|
||||
self.cameras = []
|
||||
self.mostrar_log = mostrar_log
|
||||
self._ultimo_scan = 0
|
||||
|
||||
def atualizar_cameras(self):
|
||||
dispositivos = dai.Device.getAllAvailableDevices()
|
||||
if time.time() - self._ultimo_scan < 10:
|
||||
return
|
||||
self._ultimo_scan = time.time()
|
||||
|
||||
try:
|
||||
q = Queue()
|
||||
p = Process(target=_escanear_dispositivos, args=(q,))
|
||||
p.start()
|
||||
p.join(timeout=5.0) # tempo limite de segurança
|
||||
if q.empty():
|
||||
self.mostrar_log("⚠️ Timeout ao escanear câmeras. Nenhum dispositivo retornado.")
|
||||
dispositivos_serializados = []
|
||||
else:
|
||||
dispositivos_serializados = q.get()
|
||||
except Exception as e:
|
||||
try:
|
||||
dispositivos = dai.Device.getAllAvailableDevices()
|
||||
dispositivos_serializados = [d.getMxId() for d in dispositivos]
|
||||
except Exception as e:
|
||||
dispositivos_serializados = []
|
||||
|
||||
novas_cameras = []
|
||||
|
||||
for dev_info in dispositivos:
|
||||
mx_id = dev_info.getMxId()
|
||||
for mx_id in dispositivos_serializados:
|
||||
existente = next((c for c in self.cameras if c.mx_id == mx_id), None)
|
||||
|
||||
if existente:
|
||||
#mostrar_log(f"Camera existente {existente.modelo}: {mx_id}")
|
||||
existente.atualizar_saude()
|
||||
novas_cameras.append(existente)
|
||||
else:
|
||||
#mostrar_log(f"Nova camera: {mx_id}")
|
||||
try:
|
||||
nova = CameraOak(self.mostrar_log, dev_info)
|
||||
nova.atualizar_saude()
|
||||
novas_cameras.append(nova)
|
||||
except Exception as e:
|
||||
self.mostrar_log(f"[ERRO] Falha ao iniciar camera {dev_info}: {e}")
|
||||
if not self._is_camera_em_uso(mx_id):
|
||||
print("em uso: False")
|
||||
try:
|
||||
dev_info = dai.DeviceInfo(mx_id)
|
||||
nova = CameraOak(self.mostrar_log, dev_info)
|
||||
nova.atualizar_saude()
|
||||
novas_cameras.append(nova)
|
||||
except Exception as e:
|
||||
self.mostrar_log(f"[ERRO] Falha ao iniciar camera {dev_info}: {e}")
|
||||
else:
|
||||
print("em uso: True")
|
||||
|
||||
if len(novas_cameras) > 0:
|
||||
self.cameras = novas_cameras
|
||||
|
|
@ -34,8 +60,14 @@ class CameraManager:
|
|||
cam.atualizar_saude()
|
||||
|
||||
if len(self.cameras) > 0:
|
||||
from visual_worker.config import tentar_atualizar_camera_manager
|
||||
tentar_atualizar_camera_manager()
|
||||
from shared.mqtt_handler import enviar_mensagem_visual_worker
|
||||
enviar_mensagem_visual_worker(VisualWorkerCommandType.ATUALIZAR_CAMERA_MANAGER.value, {})
|
||||
|
||||
def _is_camera_em_uso(self, mx_id):
|
||||
for c in self.cameras:
|
||||
if c.mx_id == mx_id and c.rodando:
|
||||
return True
|
||||
return False
|
||||
|
||||
def get_cameras(self, mx_id = None):
|
||||
if mx_id == None:
|
||||
|
|
@ -55,3 +87,14 @@ class CameraManager:
|
|||
return frame, cam.timestamp_ultimo_frame_rgb, res
|
||||
else:
|
||||
return None, None, {}
|
||||
|
||||
|
||||
def _escanear_dispositivos(q: Queue):
|
||||
import depthai as dai
|
||||
try:
|
||||
dispositivos = dai.Device.getAllAvailableDevices()
|
||||
dispositivos_mapeados = [d.getMxId() for d in dispositivos]
|
||||
q.put(dispositivos_mapeados)
|
||||
except Exception as e:
|
||||
q.put([])
|
||||
|
||||
|
|
|
|||
|
|
@ -3,7 +3,6 @@ import depthai as dai
|
|||
import time
|
||||
|
||||
from health_worker.modulos.imu import IMUCamera
|
||||
from shared.contexto_global import ContextoGlobal
|
||||
from shared.enums import StatusModulo, T_Code
|
||||
|
||||
class CameraOak:
|
||||
|
|
@ -19,6 +18,7 @@ class CameraOak:
|
|||
self.timestamp_ultimo_frame_depth = None
|
||||
self.tem_depth = False
|
||||
self.tem_imu = False
|
||||
self.rodando = False
|
||||
|
||||
# Fase 1: detectar sensores sem pipeline
|
||||
try:
|
||||
|
|
@ -151,6 +151,7 @@ class CameraOak:
|
|||
"duracao": dur,
|
||||
"frame_valido": frame is not None and frame.shape[0] > 0
|
||||
}
|
||||
|
||||
return frame, resultado
|
||||
|
||||
except Exception as e:
|
||||
|
|
@ -211,11 +212,23 @@ class CameraOak:
|
|||
elif saude < 80:
|
||||
status = StatusModulo.ALERTA
|
||||
|
||||
from shared.contexto_global import ContextoGlobal
|
||||
ContextoGlobal.set_item_dado(self.dispositivo, "saude", "status", status)
|
||||
ContextoGlobal.set_item_dado(self.dispositivo, "saude", "saude", saude)
|
||||
ContextoGlobal.set_item_dado(self.dispositivo, "saude", "motivos", motivos)
|
||||
ContextoGlobal.set_item_dado(self.dispositivo, "saude", "saude_individual", [])
|
||||
|
||||
agora = time.time()
|
||||
timeout = 2.0
|
||||
|
||||
ts_depth = self.timestamp_ultimo_frame_depth or 0
|
||||
ts_rgb = self.timestamp_ultimo_frame_rgb or 0
|
||||
|
||||
self.rodando = (
|
||||
(agora - ts_depth) <= timeout
|
||||
or (agora - ts_rgb) <= timeout
|
||||
)
|
||||
|
||||
self.ultima_saude = {
|
||||
"status": status,
|
||||
"saude": max(0, min(100, saude)),
|
||||
|
|
|
|||
|
|
@ -1,15 +1,18 @@
|
|||
import threading
|
||||
import time
|
||||
from shared.mqtt_handler import iniciar_mqtt
|
||||
from manager_worker.main import inicializar as iniciar_manager_worker
|
||||
from health_worker.main import inicializar as iniciar_health_worker
|
||||
from visual_worker.main import inicializar as iniciar_visual_worker
|
||||
from manager_worker.main import main as iniciar_manager_worker
|
||||
from health_worker.main import main as iniciar_health_worker
|
||||
from visual_worker.main import main as iniciar_visual_worker
|
||||
|
||||
if __name__ == "__main__":
|
||||
iniciar_mqtt()
|
||||
def main():
|
||||
iniciar_mqtt("ia_central")
|
||||
threading.Thread(target=iniciar_manager_worker, daemon=True).start()
|
||||
threading.Thread(target=iniciar_health_worker, daemon=True).start()
|
||||
threading.Thread(target=iniciar_visual_worker, daemon=True).start()
|
||||
|
||||
while True:
|
||||
time.sleep(1)
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
|
|
|||
|
|
@ -0,0 +1,32 @@
|
|||
import time
|
||||
from multiprocessing import Process
|
||||
from manager_worker.main import main as iniciar_manager_worker
|
||||
from health_worker.main import main as iniciar_health_worker
|
||||
from visual_worker.main import main as iniciar_visual_worker
|
||||
from camera_worker.main import main as iniciar_camera_worker
|
||||
|
||||
def main():
|
||||
processos = [
|
||||
#Process(target=iniciar_manager_worker, name="ManagerWorker"),
|
||||
Process(target=iniciar_health_worker, name="HealthWorker"),
|
||||
Process(target=iniciar_visual_worker, name="VisualWorker"),
|
||||
#Process(target=iniciar_camera_worker, name="CameraWorker"),
|
||||
]
|
||||
|
||||
for p in processos:
|
||||
p.daemon = True
|
||||
p.start()
|
||||
print(f"✅ Processo '{p.name}' iniciado com PID {p.pid}")
|
||||
|
||||
try:
|
||||
while True:
|
||||
time.sleep(1)
|
||||
except KeyboardInterrupt:
|
||||
print("⛔ Encerrando processos...")
|
||||
for p in processos:
|
||||
p.terminate()
|
||||
p.join()
|
||||
print("✅ Todos os processos encerrados.")
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
|
@ -1,3 +1,4 @@
|
|||
import time
|
||||
from shared.mqtt_handler import enviar_mensagem_mqtt
|
||||
from shared.contexto_global import ContextoGlobal
|
||||
from shared.enums import StatusOperacao, T_Code
|
||||
|
|
@ -15,6 +16,8 @@ class ManagerWorker:
|
|||
self.processador = None
|
||||
|
||||
def executar(self):
|
||||
t0 = time.time()
|
||||
|
||||
resposta = {}
|
||||
self.definir_processador()
|
||||
|
||||
|
|
@ -37,6 +40,10 @@ class ManagerWorker:
|
|||
if resposta != {}:
|
||||
self.enviar_comando(resposta)
|
||||
|
||||
t1 = time.time()
|
||||
latencia = t1 - t0
|
||||
return latencia
|
||||
|
||||
def definir_processador(self):
|
||||
status = ContextoGlobal.get_operacao("status")
|
||||
if status == StatusOperacao.NaoIniciado:
|
||||
|
|
|
|||
|
|
@ -3,49 +3,63 @@ import os
|
|||
import time
|
||||
import json
|
||||
|
||||
sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), "..")))
|
||||
print("🔎 Caminho sys.path:", os.path.abspath(os.path.join(os.path.dirname(__file__), "..")))
|
||||
def main():
|
||||
|
||||
from shared.contexto_global import ContextoGlobal
|
||||
from shared.mqtt_handler import registrar_callback, enviar_mensagem_script_carregado
|
||||
from shared.mensagem_worker import WorkerFilaMensagens
|
||||
from manager_worker.gerenciador import ManagerWorker
|
||||
from manager_worker.config import mostrar_log, module_id, topico_tx, topico_rx
|
||||
sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), "..")))
|
||||
print("🔎 Caminho sys.path:", os.path.abspath(os.path.join(os.path.dirname(__file__), "..")), "ManagerWorker")
|
||||
|
||||
manager = ManagerWorker()
|
||||
from shared.contexto_global import ContextoGlobal
|
||||
from shared.mqtt_handler import iniciar_mqtt, registrar_callback, enviar_mensagem_script_carregado
|
||||
from shared.mensagem_worker import WorkerFilaMensagens
|
||||
from manager_worker.gerenciador import ManagerWorker
|
||||
from manager_worker.config import mostrar_log, module_id, topico_tx, topico_rx
|
||||
|
||||
# 🔹 Callback para mensagens recebidas via MQTT (caso queira receber algo direto)
|
||||
def ao_receber_comando(topico, payload):
|
||||
fila_mensagens.adicionar(topico, payload)
|
||||
manager = ManagerWorker()
|
||||
|
||||
# 🔹 Processamento das mensagens recebidas (se necessário)
|
||||
def processar_comando(topico, message):
|
||||
try:
|
||||
mensagem = json.loads(message.decode())
|
||||
# Você pode tratar mensagens diretas aqui, mas o manager geralmente só envia
|
||||
mostrar_log(f"Mensagem recebida: {mensagem}")
|
||||
except Exception as e:
|
||||
mostrar_log(f"❌ Erro ao processar comando: {e}")
|
||||
frequencia_loop = 5 # hz
|
||||
|
||||
# 🔹 Inicializa fila de mensagens
|
||||
fila_mensagens = WorkerFilaMensagens(id=module_id, processador_callback=processar_comando)
|
||||
# 🔹 Callback para mensagens recebidas via MQTT (caso queira receber algo direto)
|
||||
def ao_receber_comando(topico, payload):
|
||||
fila_mensagens.adicionar(topico, payload)
|
||||
|
||||
# 🔹 Loop principal do Manager
|
||||
def loop_ativo():
|
||||
while True:
|
||||
# 🔹 Processamento das mensagens recebidas (se necessário)
|
||||
def processar_comando(topico, message):
|
||||
try:
|
||||
manager.executar()
|
||||
except KeyboardInterrupt:
|
||||
mostrar_log("🛑 Encerrando Manager Worker...")
|
||||
break
|
||||
mensagem = json.loads(message.decode())
|
||||
# Você pode tratar mensagens diretas aqui, mas o manager geralmente só envia
|
||||
mostrar_log(f"Mensagem recebida: {mensagem}")
|
||||
except Exception as e:
|
||||
mostrar_log(f"❌ Erro no loop do Manager: {e}")
|
||||
mostrar_log(f"❌ Erro ao processar comando: {e}")
|
||||
|
||||
time.sleep(0.2) # 5 Hz
|
||||
# 🔹 Inicializa fila de mensagens
|
||||
fila_mensagens = WorkerFilaMensagens(id=module_id, processador_callback=processar_comando)
|
||||
|
||||
# 🔹 Loop principal do Manager
|
||||
def loop_ativo():
|
||||
while True:
|
||||
latencia = 0.0
|
||||
try:
|
||||
latencia = manager.executar()
|
||||
except KeyboardInterrupt:
|
||||
mostrar_log("🛑 Encerrando Manager Worker...")
|
||||
break
|
||||
except Exception as e:
|
||||
mostrar_log(f"❌ Erro no loop do Manager: {e}")
|
||||
|
||||
delay_corrigido = max(0, (1.0 / frequencia_loop) - latencia)
|
||||
|
||||
time.sleep(delay_corrigido)
|
||||
|
||||
def inicializar():
|
||||
#iniciar_mqtt(module_id)
|
||||
mostrar_log("🚀 Iniciando Manager Worker...")
|
||||
registrar_callback(topico=topico_rx, callback=ao_receber_comando)
|
||||
enviar_mensagem_script_carregado(topico_tx)
|
||||
ContextoGlobal.iniciar_contexto_base()
|
||||
loop_ativo()
|
||||
|
||||
inicializar()
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
||||
def inicializar():
|
||||
mostrar_log("🚀 Iniciando Manager Worker...")
|
||||
registrar_callback(topico=topico_rx, callback=ao_receber_comando)
|
||||
enviar_mensagem_script_carregado(topico_tx)
|
||||
ContextoGlobal.iniciar_contexto_base()
|
||||
loop_ativo()
|
||||
|
|
|
|||
|
|
@ -1,7 +1,6 @@
|
|||
import time
|
||||
|
||||
import numpy as np
|
||||
from shared.enums import StatusCarroMapa, StatusModulo, StatusOperacao, T_Code, TipoMovimentoDirecional, VisualWorkerTipoAnalise
|
||||
from shared.enums import StatusCarroMapa, StatusModulo, StatusOperacao, T_Code, TipoMovimentoDirecional
|
||||
from shared.contexto_global import ContextoGlobal
|
||||
from manager_worker.config import mostrar_log
|
||||
from visual_worker.config import get_camera_manager
|
||||
|
|
@ -11,148 +10,137 @@ def definir_comando():
|
|||
"""
|
||||
Define o comando de direção com base em regras mockadas + MPC
|
||||
"""
|
||||
gps = ContextoGlobal.get_item(T_Code.Gps, "dados") or {}
|
||||
visual_worker_operante = (ContextoGlobal.get_item(T_Code.Snr, "saude", "status") or StatusModulo.DESCONECTADO) == StatusModulo.OPERANTE
|
||||
contexto = {
|
||||
"Operacao": {
|
||||
"Status": ContextoGlobal.get_operacao("status"),
|
||||
"Finalizando": ContextoGlobal.get_operacao("finalizando")
|
||||
},
|
||||
"GPS": {
|
||||
"Latitude": gps.get("lat", 0),
|
||||
"Longitude": gps.get("lon", 0),
|
||||
"AnguloCarro": gps.get("theta", 0),
|
||||
},
|
||||
"Carro": {
|
||||
"Velocidade": ContextoGlobal.get_item(T_Code.Gerais, "dados", "velocidade_ms") or 0.0,
|
||||
"Status": ContextoGlobal.get_item(T_Code.Trajetoria, "dados", "status") or StatusCarroMapa.Parado.value,
|
||||
"ManobrandoEntreRuas": ContextoGlobal.get_item(T_Code.Trajetoria, "dados", "manobrando") or False,
|
||||
"DentroCorredor": (ContextoGlobal.get_item(T_Code.Trajetoria, "dados", "CorredorAtual") or {}).get("dentro", False),
|
||||
"DistanciaEsquerda": (ContextoGlobal.get_item(T_Code.Trajetoria, "dados", "CorredorAtual") or {}).get("dist_esq", 0.0),
|
||||
"DistanciaDireita": (ContextoGlobal.get_item(T_Code.Trajetoria, "dados", "CorredorAtual") or {}).get("dist_dir", 0.0),
|
||||
"IdxProximoPonto": (ContextoGlobal.get_item(T_Code.Trajetoria, "dados", "idx_proximo_ponto") or 0),
|
||||
"AnguloCaminho": (ContextoGlobal.get_item(T_Code.Trajetoria, "dados", "angulo") or 0),
|
||||
},
|
||||
"VisualWorker": {
|
||||
"Operante": visual_worker_operante,
|
||||
"Camera": {
|
||||
"FovH": get_camera_manager().camera.parametros["fov_h"] or 0.7 if get_camera_manager().camera is not None else 0.7,
|
||||
try:
|
||||
gps = ContextoGlobal.get_item(T_Code.Gps, "dados") or {}
|
||||
visual_worker_operante = (ContextoGlobal.get_item(T_Code.Snr, "saude", "status") or StatusModulo.DESCONECTADO) == StatusModulo.OPERANTE
|
||||
contexto = {
|
||||
"Operacao": {
|
||||
"Status": ContextoGlobal.get_operacao("status"),
|
||||
"Finalizando": ContextoGlobal.get_operacao("finalizando")
|
||||
},
|
||||
"GPS": {
|
||||
"Latitude": gps.get("lat", 0),
|
||||
"Longitude": gps.get("lon", 0),
|
||||
"AnguloCarro": gps.get("theta", 0),
|
||||
},
|
||||
"Carro": {
|
||||
"Velocidade": ContextoGlobal.get_item(T_Code.Gerais, "dados", "velocidade_ms") or 0.0,
|
||||
"Status": ContextoGlobal.get_item(T_Code.Trajetoria, "dados", "status") or StatusCarroMapa.Parado.value,
|
||||
"ManobrandoEntreRuas": ContextoGlobal.get_item(T_Code.Trajetoria, "dados", "manobrando") or False,
|
||||
"DentroCorredor": (ContextoGlobal.get_item(T_Code.Trajetoria, "dados", "CorredorAtual") or {}).get("dentro", False),
|
||||
"DistanciaEsquerda": (ContextoGlobal.get_item(T_Code.Trajetoria, "dados", "CorredorAtual") or {}).get("dist_esq", 0.0),
|
||||
"DistanciaDireita": (ContextoGlobal.get_item(T_Code.Trajetoria, "dados", "CorredorAtual") or {}).get("dist_dir", 0.0),
|
||||
"IdxProximoPonto": (ContextoGlobal.get_item(T_Code.Trajetoria, "dados", "idx_proximo_ponto") or 0),
|
||||
"AnguloCaminho": (ContextoGlobal.get_item(T_Code.Trajetoria, "dados", "angulo") or 0),
|
||||
},
|
||||
"MatrizCusto": [],
|
||||
},
|
||||
"RegrasAtivas": {
|
||||
"matriz_custo": visual_worker_operante and True,
|
||||
"VisualWorker": {
|
||||
"Operante": visual_worker_operante,
|
||||
"Camera": {
|
||||
"FovH": get_camera_manager().camera.parametros["fov_h"] or 0.7 if get_camera_manager().camera is not None else 0.7,
|
||||
},
|
||||
"MatrizCusto": [[]],
|
||||
},
|
||||
"RegrasAtivas": {
|
||||
"matriz_custo": visual_worker_operante and True,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
definido, comando = regras_taticas(contexto)
|
||||
definido, comando = regras_taticas(contexto)
|
||||
|
||||
if (definido):
|
||||
return comando["angulo"], comando["tipo"], comando["simulacao"]
|
||||
else:
|
||||
contexto["VisualWorker"]["MatrizCusto"] = comando.get("analise", {}).get("matriz_custo", [])
|
||||
|
||||
# DADOS MOCKADOS PARA DEBUG DE OBSTACULOS
|
||||
#obsts = []
|
||||
#for i in range(1):
|
||||
# try:
|
||||
# cx = float(input(f"🔧 Obstáculo {i+1} — centro X (0.0 a 1.0): "))
|
||||
# dist = float(input(f"🔧 Obstáculo {i+1} — distância (m): "))
|
||||
# except ValueError:
|
||||
# continue
|
||||
# obsts.append({
|
||||
# "bbox": [0, 0, 10, 10],
|
||||
# "tipo": "obstaculo",
|
||||
# "distancia_m": dist,
|
||||
# "variacao": 10,
|
||||
# "largura_m": 0.3,
|
||||
# "altura_m": 0.3,
|
||||
# "centro_percentual": [np.clip(cx, 0.0, 1.0), 0.75]
|
||||
# })
|
||||
#contexto["VisualWorker"]["Obstaculos"] = obsts
|
||||
#contexto["Carro"]["Velocidade"] = 0.5
|
||||
#ContextoGlobal.set_controle_dado("angulo_sp", 0)
|
||||
#ContextoGlobal.set_controle_dado("tipo_movimento_direcional", TipoMovimentoDirecional.RodasDianteiras.value)
|
||||
# FIM DADOS MOCKADOS
|
||||
|
||||
return definir_comando_mpc(contexto)
|
||||
if (definido):
|
||||
return comando["angulo"], comando["tipo"], comando["simulacao"]
|
||||
else:
|
||||
contexto["VisualWorker"]["MatrizCusto"] = comando.get("analise", {}).get("matriz_custo", [[]])
|
||||
return definir_comando_mpc(contexto)
|
||||
except Exception as e:
|
||||
mostrar_log(f"❌ Erro ao definir comando MPC: {e}")
|
||||
|
||||
def comando_direcional_parado(analise={}):
|
||||
angulo, tipo = comando_parado()
|
||||
return {
|
||||
"angulo": angulo,
|
||||
"tipo": tipo,
|
||||
"simulacao": [],
|
||||
"analise": analise
|
||||
}
|
||||
try:
|
||||
angulo, tipo = comando_parado()
|
||||
return {
|
||||
"angulo": angulo,
|
||||
"tipo": tipo,
|
||||
"simulacao": [],
|
||||
"analise": analise
|
||||
}
|
||||
except Exception as e:
|
||||
mostrar_log(f"❌ Erro ao montar comando direcional parado: {e}")
|
||||
|
||||
def regras_taticas(contexto):
|
||||
if contexto["Operacao"]["Status"] != StatusOperacao.EmAndamento or contexto["Operacao"]["Finalizando"]:
|
||||
mostrar_log("🟥 Direcional parado: operação não está em andamento ou está finalizando.")
|
||||
return True, comando_direcional_parado()
|
||||
|
||||
if not contexto.get("VisualWorker", {}).get("Operante", False):
|
||||
#mostrar_log("⚠️ Sensores principais inativos. Rodando MPC no modo básico.")
|
||||
return False, comando_direcional_parado()
|
||||
|
||||
camera_manager = get_camera_manager()
|
||||
imu = camera_manager.get_imu_data()
|
||||
if imu is not None and False:
|
||||
if abs(imu.get("pitch", 0)) > 15 or abs(imu.get("roll", 0)) > 10:
|
||||
mostrar_log("🟥 Inclinação perigosa detectada. Parando movimentação.")
|
||||
try:
|
||||
if contexto["Operacao"]["Status"] != StatusOperacao.EmAndamento or contexto["Operacao"]["Finalizando"]:
|
||||
mostrar_log("🟥 Direcional parado: operação não está em andamento ou está finalizando.")
|
||||
return True, comando_direcional_parado()
|
||||
|
||||
velocidade_media = contexto["Carro"]["Velocidade"]
|
||||
if velocidade_media < 0.01:
|
||||
mostrar_log("⚠️ Possível travamento detectado (velocidade quase zero).")
|
||||
# Pode evoluir para uma lógica de recuperação no futuro
|
||||
if not contexto.get("VisualWorker", {}).get("Operante", False):
|
||||
#mostrar_log("⚠️ Sensores principais inativos. Rodando MPC no modo básico.")
|
||||
return False, comando_direcional_parado()
|
||||
|
||||
# 🔸 Pega os dados do contexto
|
||||
distancia_max = camera_manager.camera.parametros["distancia_maxima"] / 1000.0
|
||||
camera_manager = get_camera_manager()
|
||||
|
||||
analise = camera_manager.realizar_analise(
|
||||
[VisualWorkerTipoAnalise.MATRIZ_CONFIANCA, VisualWorkerTipoAnalise.ANOMALIA, VisualWorkerTipoAnalise.SOLO, VisualWorkerTipoAnalise.RADAR_2D],
|
||||
velocidade_media,
|
||||
distancia_max
|
||||
)
|
||||
obstaculos = analise.get("anomalias", {}).get("deteccoes", [])
|
||||
if obstaculos:
|
||||
for obst in obstaculos:
|
||||
if obst.get("distancia_m", 99) < 0.4:
|
||||
mostrar_log("🟥 Obstáculo crítico detectado a menos de 40cm. Parando.")
|
||||
if camera_manager.camera is None:
|
||||
return False, comando_direcional_parado()
|
||||
|
||||
imu = camera_manager.get_imu_data()
|
||||
if imu is not None and False:
|
||||
if abs(imu.get("pitch", 0)) > 15 or abs(imu.get("roll", 0)) > 10:
|
||||
mostrar_log("🟥 Inclinação perigosa detectada. Parando movimentação.")
|
||||
return True, comando_direcional_parado()
|
||||
|
||||
return False, comando_direcional_parado(analise)
|
||||
|
||||
velocidade_media = contexto["Carro"]["Velocidade"]
|
||||
if velocidade_media < 0.01:
|
||||
mostrar_log("⚠️ Possível travamento detectado (velocidade quase zero).")
|
||||
# Pode evoluir para uma lógica de recuperação no futuro
|
||||
|
||||
# 🔸 Pega os dados do contexto
|
||||
analise = {}
|
||||
if contexto.get("RegrasAtivas", {}).get("matriz_custo", False):
|
||||
analise = camera_manager.get_analise_mais_recente()
|
||||
obstaculos = analise.get("anomalias", {}).get("deteccoes", [])
|
||||
if obstaculos:
|
||||
for obst in obstaculos:
|
||||
if obst.get("distancia_m", 99) < 0.4:
|
||||
mostrar_log("🟥 Obstáculo crítico detectado a menos de 40cm. Parando.")
|
||||
return True, comando_direcional_parado()
|
||||
|
||||
return False, comando_direcional_parado(analise)
|
||||
|
||||
except Exception as e:
|
||||
mostrar_log(f"❌ Erro ao processar regras taticas do direcional: {e}")
|
||||
|
||||
def definir_comando_mpc(contexto):
|
||||
mpc = get_mpc()
|
||||
if mpc is None:
|
||||
mostrar_log("⚠️ MPC ainda não inicializado.")
|
||||
return
|
||||
try:
|
||||
mpc = get_mpc()
|
||||
if mpc is None:
|
||||
mostrar_log("⚠️ MPC ainda não inicializado.")
|
||||
return
|
||||
|
||||
comando_anterior = {
|
||||
"angulo": ContextoGlobal.get_controle("angulo_sp") or 0.0,
|
||||
"tipo": ContextoGlobal.get_controle("tipo_movimento_direcional") or TipoMovimentoDirecional.RodasDianteiras.value
|
||||
}
|
||||
comando_anterior = {
|
||||
"angulo": ContextoGlobal.get_controle("angulo_sp") or 0.0,
|
||||
"tipo": ContextoGlobal.get_controle("tipo_movimento_direcional") or TipoMovimentoDirecional.RodasDianteiras
|
||||
}
|
||||
|
||||
# 🔸 Requisição ao controlador MPC
|
||||
t0 = time.time()
|
||||
comando = mpc.compute(contexto, comando_anterior)
|
||||
#comando = mpc.compute_horizonte(contexto, comando_anterior)
|
||||
t1 = time.time()
|
||||
EPSILON = 1e-3 # 1 milissegundo, pra evitar divisão por zero
|
||||
if (comando != None):
|
||||
# 🔸 Extrai os dados de controle
|
||||
angulo_sp = comando.get("angulo", 0.0)
|
||||
tipo_movimento = comando.get("tipo", TipoMovimentoDirecional.RodasDianteiras)
|
||||
ContextoGlobal.set_controle_dado("angulo_sp", angulo_sp)
|
||||
ContextoGlobal.set_controle_dado("tipo_movimento_direcional", tipo_movimento)
|
||||
mostrar_log(f"🧭 Direcional MPC | Ângulo: {angulo_sp:.2f}° | Movimento: {tipo_movimento.name} | {(t1 - t0):.3f}s ({1 / max(t1 - t0, EPSILON):.3f} Hz)")
|
||||
else:
|
||||
mostrar_log(f"🧭 Direcional MPC | Erro ao definir comando")
|
||||
# 🔸 Requisição ao controlador MPC
|
||||
t0 = time.time()
|
||||
comando = mpc.compute_receding(contexto, comando_anterior)
|
||||
#comando = mpc.compute_horizonte(contexto, comando_anterior)
|
||||
t1 = time.time()
|
||||
EPSILON = 1e-3 # 1 milissegundo, pra evitar divisão por zero
|
||||
if (comando != None):
|
||||
# 🔸 Extrai os dados de controle
|
||||
angulo_sp = comando.get("angulo", 0.0)
|
||||
tipo_movimento = comando.get("tipo", TipoMovimentoDirecional.RodasDianteiras)
|
||||
ContextoGlobal.set_controle_dado("angulo_sp", angulo_sp)
|
||||
ContextoGlobal.set_controle_dado("tipo_movimento_direcional", tipo_movimento)
|
||||
mostrar_log(f"🧭 Direcional MPC | Ângulo: {angulo_sp:.2f}° | Movimento: {tipo_movimento.name} | {(t1 - t0):.3f}s ({1 / max(t1 - t0, EPSILON):.3f} Hz)")
|
||||
else:
|
||||
mostrar_log(f"🧭 Direcional MPC | Erro ao definir comando")
|
||||
|
||||
#input("⏸️ Pressione Enter para continuar...")
|
||||
#input("⏸️ Pressione Enter para continuar...")
|
||||
|
||||
return ContextoGlobal.get_controle("angulo_sp"), ContextoGlobal.get_controle("tipo_movimento_direcional"), comando.get("simulacao", [])
|
||||
return ContextoGlobal.get_controle("angulo_sp"), ContextoGlobal.get_controle("tipo_movimento_direcional"), comando.get("simulacao", [])
|
||||
except Exception as e:
|
||||
mostrar_log(f"❌ Erro ao definir comando MPC: {e}")
|
||||
|
||||
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -1,6 +1,6 @@
|
|||
import time
|
||||
|
||||
from shared.enums import T_Code, ModoOperacao, StatusOperacao, StatusModulo, TiposControladorDirecional
|
||||
from shared.enums import T_Code, ModoOperacao, StatusOperacao, StatusModulo, TiposControladorDirecional, VisualWorkerCommandType
|
||||
from manager_worker.modulos.mpc import inicializar as iniciar_mpc
|
||||
|
||||
class ContextoGlobal:
|
||||
|
|
@ -40,30 +40,32 @@ class ContextoGlobal:
|
|||
|
||||
@classmethod
|
||||
def atualizar_contexto(cls, dados_contexto: dict):
|
||||
timestamp = time.time()
|
||||
try:
|
||||
timestamp = time.time()
|
||||
|
||||
for modulo, dados in dados_contexto.items():
|
||||
if modulo not in cls._dados:
|
||||
cls._dados[modulo] = {}
|
||||
for modulo, dados in dados_contexto.items():
|
||||
if modulo not in cls._dados:
|
||||
cls._dados[modulo] = {}
|
||||
|
||||
if isinstance(dados, list):
|
||||
for item in dados:
|
||||
endereco = item.get("endereco")
|
||||
if endereco is not None:
|
||||
if endereco not in cls._dados[modulo]:
|
||||
cls._dados[modulo][endereco] = {}
|
||||
cls._dados[modulo][endereco]["dados"] = item
|
||||
cls._dados[modulo][endereco]["timestamp"] = timestamp
|
||||
|
||||
if isinstance(dados, list):
|
||||
for item in dados:
|
||||
endereco = item.get("endereco")
|
||||
if endereco is not None:
|
||||
if endereco not in cls._dados[modulo]:
|
||||
cls._dados[modulo][endereco] = {}
|
||||
elif isinstance(dados, dict):
|
||||
if "dados" not in cls._dados[modulo]:
|
||||
cls._dados[modulo]["dados"] = {}
|
||||
cls._dados[modulo]["dados"].update(dados)
|
||||
cls._dados[modulo]["timestamp"] = timestamp
|
||||
|
||||
cls._dados[modulo][endereco]["dados"] = item
|
||||
cls._dados[modulo][endereco]["timestamp"] = timestamp
|
||||
|
||||
elif isinstance(dados, dict):
|
||||
if "dados" not in cls._dados[modulo]:
|
||||
cls._dados[modulo]["dados"] = {}
|
||||
|
||||
cls._dados[modulo]["dados"].update(dados)
|
||||
cls._dados[modulo]["timestamp"] = timestamp
|
||||
|
||||
cls._atualizar_dados_operacao()
|
||||
cls._atualizar_dados_operacao()
|
||||
except Exception as e:
|
||||
print(f"{time.time()} - [CONTEXTO_GLOBAL] ❌ Erro ao atualizar contexto: {e}")
|
||||
|
||||
|
||||
@classmethod
|
||||
def get_item(cls, modulo: T_Code, chave: str, dado: str = None, endereco=None):
|
||||
|
|
@ -111,6 +113,7 @@ class ContextoGlobal:
|
|||
cls._atualizar_operacao_liberada()
|
||||
cls._atualiza_status_operacao()
|
||||
cls._atualizar_visual_worker()
|
||||
cls._atualizar_pontos_mapa()
|
||||
|
||||
@classmethod
|
||||
def _atualizar_operacao_liberada(cls):
|
||||
|
|
@ -272,9 +275,33 @@ class ContextoGlobal:
|
|||
|
||||
@classmethod
|
||||
def _atualizar_visual_worker(cls):
|
||||
from visual_worker.config import atualiar_camera_manager
|
||||
mx_id = cls.get_operacao(T_Code.Snr.name).get("mx_id")
|
||||
atualiar_camera_manager(mx_id=mx_id)
|
||||
mx_id = (cls.get_operacao(T_Code.Snr.name) or {}).get("mx_id")
|
||||
if mx_id is not None:
|
||||
from shared.mqtt_handler import enviar_mensagem_visual_worker
|
||||
enviar_mensagem_visual_worker(VisualWorkerCommandType.INICIAR_CAMERA_MANAGER.value, {"mx_id": mx_id})
|
||||
|
||||
@classmethod
|
||||
def _atualizar_pontos_mapa(cls):
|
||||
pontos_info = []
|
||||
p_ref = (0, 0)
|
||||
dados_dir = cls.get_operacao(T_Code.Dir.name) or {}
|
||||
if (dados_dir.get("tipo_controle", TiposControladorDirecional.Manual) == TiposControladorDirecional.MPC):
|
||||
dados_mpc = dados_dir.get("mpc", {})
|
||||
pontos = dados_mpc.get("pontos", [])
|
||||
if pontos is not None and len(pontos) > 0:
|
||||
p_ref = pontos[0]["lat"], pontos[0]["lon"]
|
||||
from shared.gps_handler import GPSHandler
|
||||
handler = GPSHandler(p_ref[0], p_ref[1])
|
||||
for p in pontos:
|
||||
x, y = handler.converter_latlon_para_xz(p["lat"], p["lon"])
|
||||
ponto = {
|
||||
"xy": (x, y),
|
||||
"tipo": p.get("tipo", 3),
|
||||
"distanciaMargem": p.get("distanciaMargem", 0.7)
|
||||
}
|
||||
pontos_info.append(ponto)
|
||||
cls.set_operacao_dado("pontos_mapa", pontos_info)
|
||||
cls.set_operacao_dado("ponto_mapa_ref", p_ref)
|
||||
|
||||
|
||||
_sonar = {}
|
||||
|
|
|
|||
|
|
@ -137,6 +137,9 @@ class TipoPontoRua(IntEnum):
|
|||
# Visual Worker
|
||||
class VisualWorkerCommandType(IntEnum):
|
||||
SALVAR_FRAMES = 0
|
||||
ATUALIZAR_CAMERA_MANAGER = 1
|
||||
INICIAR_CAMERA_MANAGER = 2
|
||||
GET_ULTIMA_ANALISE = 3
|
||||
|
||||
class VisualWorkerTipoDeteccao(IntEnum):
|
||||
SEGURO = 1
|
||||
|
|
|
|||
|
|
@ -0,0 +1,29 @@
|
|||
import numpy as np
|
||||
|
||||
class GPSHandler:
|
||||
def __init__(self, lat0, lon0, raio_terra=6371000):
|
||||
self.lat0 = lat0
|
||||
self.lon0 = lon0
|
||||
self.raio_terra = raio_terra
|
||||
|
||||
def converter_latlon_para_xz(self, lat, lon):
|
||||
dlat = np.radians(lat - self.lat0)
|
||||
dlon = np.radians(lon - self.lon0)
|
||||
x = self.raio_terra * dlon * np.cos(np.radians((lat + self.lat0) / 2))
|
||||
z = self.raio_terra * dlat
|
||||
return x, z
|
||||
|
||||
def encontrar_indice_mais_proximo(self, pontos_xy, x_robo, z_robo):
|
||||
dists = [
|
||||
np.hypot(p["xy"][0] - x_robo, p["xy"][1] - z_robo)
|
||||
for p in pontos_xy
|
||||
]
|
||||
return int(np.argmin(dists))
|
||||
|
||||
def converter_para_referencial_robo(self, x_ponto, z_ponto, x_robo, z_robo, theta_robo_rad):
|
||||
dx = x_ponto - x_robo
|
||||
dz = z_ponto - z_robo
|
||||
# 🔁 Rotação padrão no sentido horário
|
||||
x_rel = dx * np.cos(theta_robo_rad) - dz * np.sin(theta_robo_rad)
|
||||
z_rel = dz * np.cos(theta_robo_rad) - dx * np.sin(theta_robo_rad)
|
||||
return x_rel, z_rel
|
||||
|
|
@ -2,6 +2,8 @@ import paho.mqtt.client as mqtt
|
|||
import json
|
||||
import time
|
||||
|
||||
from shared.enums import T_Code, VisualWorkerCommandType
|
||||
|
||||
mqtt_client = None
|
||||
debug = True
|
||||
|
||||
|
|
@ -11,11 +13,11 @@ def mostrar_log(mensagem):
|
|||
if debug:
|
||||
print(f"{time.time()} - [MQTT] {mensagem}")
|
||||
|
||||
def iniciar_mqtt(broker="localhost", porta=1883):
|
||||
def iniciar_mqtt(client_id, broker="localhost", porta=1883):
|
||||
global mqtt_client, mqtt_topic_tx, mqtt_topic_rx
|
||||
|
||||
mqtt_client = mqtt.Client(
|
||||
client_id="ia_central",
|
||||
client_id=f"{client_id}_worker",
|
||||
protocol=mqtt.MQTTv311,
|
||||
transport="tcp"
|
||||
# A partir da 2.0 o padrão já é API V5, então não precisa do callback_api_version
|
||||
|
|
@ -56,6 +58,27 @@ def enviar_mensagem_mqtt(topico, dispositivo, objeto):
|
|||
}
|
||||
mqtt_client.publish(topico, json.dumps(mensagem))
|
||||
|
||||
def enviar_mensagem_mqtt_entre_modulos(topico: str, dispositivo: T_Code, comando: int, payload):
|
||||
#mostrar_log(f"Enviando dados MQTT para o dispositivo {dispositivo}...")
|
||||
mensagem = {
|
||||
"momento": time.time(),
|
||||
"dispositivo": dispositivo,
|
||||
"comando": comando,
|
||||
"payload": payload
|
||||
}
|
||||
mqtt_client.publish(topico, json.dumps(mensagem))
|
||||
|
||||
def enviar_mensagem_visual_worker(comando: VisualWorkerCommandType, payload):
|
||||
#mostrar_log(f"Enviando dados MQTT para o dispositivo {dispositivo}...")
|
||||
mensagem = {
|
||||
"momento": time.time(),
|
||||
"dispositivo": T_Code.Snr,
|
||||
"comando": comando,
|
||||
"payload": payload
|
||||
}
|
||||
from visual_worker.config import topico_rx
|
||||
mqtt_client.publish(topico_rx, json.dumps(mensagem))
|
||||
|
||||
def registrar_callback(topico, callback):
|
||||
_inscritos[topico] = callback
|
||||
if mqtt_client:
|
||||
|
|
|
|||
|
|
@ -1,7 +1,11 @@
|
|||
import base64
|
||||
import math
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
from manager_worker.config import mostrar_log
|
||||
|
||||
|
||||
|
||||
def penalizar_frequencia(freq, freq_base, freq_min, penalidade_max):
|
||||
if freq >= freq_base:
|
||||
|
|
@ -27,3 +31,34 @@ def decode_image_base64(base64_str):
|
|||
img_array = np.frombuffer(img_data, dtype=np.uint8)
|
||||
frame_decodificado = cv2.imdecode(img_array, cv2.IMREAD_COLOR)
|
||||
return frame_decodificado
|
||||
|
||||
def analisar_linhas_por_profundidade(matriz, campo):
|
||||
try:
|
||||
linhas_info = {}
|
||||
|
||||
if matriz is None or len(matriz) == 0 or len(matriz[0]) == 0:
|
||||
return linhas_info
|
||||
|
||||
from visual_worker.config import get_camera_manager
|
||||
camera_manager = get_camera_manager()
|
||||
fov_h = camera_manager.camera.parametros["fov_h"]
|
||||
|
||||
for i, linha in enumerate(matriz):
|
||||
profundidades = [cel.get(campo, 0.0) for cel in linha if cel.get(campo, 0.0) > 0]
|
||||
if profundidades:
|
||||
distancia = float(np.median(profundidades))
|
||||
altura_grid = len(matriz)
|
||||
largura_grid = len(matriz[0])
|
||||
altura_linha = altura_grid / largura_grid
|
||||
largura_linha = 2 * distancia * math.tan(fov_h / 2)
|
||||
linhas_info[i] = {
|
||||
"idx": i,
|
||||
"distancia": distancia,
|
||||
"altura": altura_linha,
|
||||
"largura": 2 * distancia * math.tan(fov_h / 2),
|
||||
"escala_x": largura_linha / largura_grid
|
||||
}
|
||||
|
||||
return linhas_info
|
||||
except Exception as e:
|
||||
mostrar_log(f"❌ Erro ao analisar linhas por profundidade: {e}")
|
||||
|
|
@ -1,35 +1,44 @@
|
|||
import datetime
|
||||
import math
|
||||
import os
|
||||
import threading
|
||||
import time
|
||||
from statistics import median
|
||||
|
||||
import cv2
|
||||
import cupy as cp
|
||||
import numpy as np
|
||||
from health_worker.config import manager as health_manager
|
||||
from health_worker.config import get_camera_manager
|
||||
from visual_worker.utils import calcular_threshold_anomalias, calcular_referencia, obter_regiao_solo, calcular_profundidades_setores, gerar_heatmap
|
||||
from visual_worker.processamento.analise_solo import AnaliseSoloManager
|
||||
from visual_worker.processamento.analise_anomalias import AnaliseAnomaliasManager
|
||||
from visual_worker.processamento.radar_top_down import Radar2DManager
|
||||
from visual_worker.processamento.segmentacao_semantica import ClassesSegmentacao, SegmentacaoManager
|
||||
from shared.enums import VisualWorkerTipoAnalise
|
||||
from shared.utils import decode_image_base64
|
||||
from shared.enums import T_Code, VisualWorkerTipoAnalise
|
||||
from shared.utils import analisar_linhas_por_profundidade, decode_image_base64
|
||||
from shared.gps_handler import GPSHandler
|
||||
|
||||
class CameraManager:
|
||||
_iniciando = False # variável de classe (compartilhada por instâncias)
|
||||
|
||||
def __init__(self, mostrar_log, mx_id=None):
|
||||
if CameraManager._iniciando:
|
||||
return
|
||||
|
||||
CameraManager._iniciando = True
|
||||
self.mostrar_log = mostrar_log
|
||||
self.mx_id = None
|
||||
self.camera = None
|
||||
self.operante = False
|
||||
|
||||
if mx_id == None:
|
||||
self.mostrar_log(f"❌ Camera não definida.")
|
||||
#self.mostrar_log(f"❌ Camera não definida.")
|
||||
CameraManager._iniciando = False
|
||||
return
|
||||
|
||||
self.mx_id = mx_id
|
||||
self.camera = health_manager.get_cameras(mx_id)
|
||||
self.camera = get_camera_manager().get_cameras(mx_id)
|
||||
if self.camera is None:
|
||||
self.mostrar_log(f"❌ Camera com ID {mx_id} não encontrada no manager.")
|
||||
CameraManager._iniciando = False
|
||||
else:
|
||||
self.mostrar_log(f"📷 Camera visual selecionada: {self.camera.modelo} - {self.camera.mx_id}")
|
||||
self.grid_ref = None
|
||||
|
|
@ -41,6 +50,11 @@ class CameraManager:
|
|||
self.solo_manager = AnaliseSoloManager()
|
||||
self.segmentacao_manager = SegmentacaoManager("C:/AgroBaseModels/Ruas/segformer_cana_fixed.onnx")
|
||||
self.radar_manager = Radar2DManager()
|
||||
self.operante = True
|
||||
self._ultima_analise = None
|
||||
self._timestamp_analise = None
|
||||
self.iniciar_loop_analise_continua(4.0)
|
||||
CameraManager._iniciando = False
|
||||
|
||||
def get_rgb_frame(self):
|
||||
if self.camera is None:
|
||||
|
|
@ -75,7 +89,34 @@ class CameraManager:
|
|||
return None, None, None
|
||||
|
||||
def get_imu_data(self):
|
||||
return self.camera.requisitar_imu_status()
|
||||
if self.camera is not None:
|
||||
return self.camera.requisitar_imu_status()
|
||||
return None
|
||||
|
||||
def iniciar_loop_analise_continua(self, freq):
|
||||
def loop():
|
||||
while self.operante:
|
||||
t_ini = time.time()
|
||||
resultado = self.realizar_analise(
|
||||
tipos_analise=[
|
||||
VisualWorkerTipoAnalise.MATRIZ_CONFIANCA,
|
||||
VisualWorkerTipoAnalise.ANOMALIA,
|
||||
VisualWorkerTipoAnalise.SOLO,
|
||||
VisualWorkerTipoAnalise.RADAR_2D
|
||||
]
|
||||
)
|
||||
self._ultima_analise = resultado
|
||||
self._timestamp_analise = time.time()
|
||||
time.sleep(max(0, (1.0 / freq) - (self._timestamp_analise - t_ini)))
|
||||
threading.Thread(target=loop, daemon=True).start()
|
||||
|
||||
def get_analise_mais_recente(self, max_idade_segundos=1.0):
|
||||
if self._ultima_analise is None:
|
||||
return {}
|
||||
t = time.time() - self._timestamp_analise
|
||||
if t > max_idade_segundos:
|
||||
self.mostrar_log(f"⚠️ Análise visual desatualizada a {t} segundos. Limite de {max_idade_segundos} segundos.")
|
||||
return self._ultima_analise
|
||||
|
||||
def _calibrar(self, n_frames=50):
|
||||
self.calibrado = False
|
||||
|
|
@ -144,7 +185,7 @@ class CameraManager:
|
|||
|
||||
return grid_referencia
|
||||
|
||||
def realizar_analise(self, tipos_analise, velocidade_ms: float, distancia_max_m: float, largura_min: float = 0.15, altura_min: float = 0.15):
|
||||
def realizar_analise(self, tipos_analise: list, largura_min: float = 0.15, altura_min: float = 0.15):
|
||||
t0 = time.time()
|
||||
resultado = {
|
||||
"timestamp": t0,
|
||||
|
|
@ -154,6 +195,7 @@ class CameraManager:
|
|||
"segmentacao": {},
|
||||
"matriz_confianca": []
|
||||
}
|
||||
|
||||
if not self.calibrado:
|
||||
if not self._calibrar():
|
||||
self.mostrar_log("❌ Não é possível analisar — câmera ainda não calibrada.")
|
||||
|
|
@ -166,9 +208,10 @@ class CameraManager:
|
|||
|
||||
parametros_camera = self.camera.parametros
|
||||
fov_h = parametros_camera["fov_h"]
|
||||
distancia_max_m = parametros_camera["distancia_maxima"] / 1000.0
|
||||
|
||||
# 🔸 Calcula threshold com base na velocidade
|
||||
limiar_delta = 0.15 # calcular_threshold_anomalias(velocidade_ms)
|
||||
limiar_delta = calcular_threshold_anomalias()
|
||||
limiar_conf = 0.4
|
||||
t2 = time.time()
|
||||
|
||||
|
|
@ -195,7 +238,7 @@ class CameraManager:
|
|||
t4 = time.time()
|
||||
|
||||
linha_topo = grid_conf[0]
|
||||
lista_profundidades = [cel.get("prof_median", np.nan) for cel in linha_topo]
|
||||
lista_profundidades = [cel.get("prof_ref", np.nan) for cel in linha_topo]
|
||||
dist_max = np.nanmedian(lista_profundidades)
|
||||
|
||||
if VisualWorkerTipoAnalise.ANOMALIA in tipos_analise:
|
||||
|
|
@ -216,20 +259,22 @@ class CameraManager:
|
|||
resultado["radar_2d"] = analise_radar
|
||||
t7 = time.time()
|
||||
|
||||
matriz_custo = self._gerar_matriz_custo(
|
||||
grid_conf,
|
||||
analise_anomalias["deteccoes"],
|
||||
analise_segmentacao["corredor_perfil"],
|
||||
analise_radar["analise"]["corredor_perfil"],
|
||||
analise_solo["perfil"],
|
||||
linhas_solo,
|
||||
dist_max,
|
||||
0.85,
|
||||
fov_h
|
||||
)
|
||||
#matriz_custo = self._gerar_matriz_custo(
|
||||
# grid_conf,
|
||||
# analise_anomalias["deteccoes"],
|
||||
# analise_radar["analise"]["bboxes_sombra"],
|
||||
# analise_segmentacao["corredor_perfil"],
|
||||
# analise_radar["analise"]["corredor_perfil"],
|
||||
# analise_solo["perfil"],
|
||||
# linhas_solo,
|
||||
# 0.85
|
||||
#)
|
||||
#self._mostrar_debug_matriz_custo(rgb_frame, matriz_custo, exibir_debug=True)
|
||||
|
||||
matriz_custo = self._gerar_matriz_custo_fundida(grid_conf, 0.85)
|
||||
resultado["matriz_custo"] = matriz_custo
|
||||
t8 = time.time()
|
||||
self._mostrar_debug_matriz_custo(rgb_frame, matriz_custo, exibir_debug=True)
|
||||
self._mostrar_debug_matriz_custo_fundida(rgb_frame, matriz_custo, exibir_debug=True)
|
||||
|
||||
#if VisualWorkerTipoAnalise.MATRIZ_CONFIANCA in tipos_analise:
|
||||
# # Antes de chamar o método:
|
||||
|
|
@ -252,33 +297,33 @@ class CameraManager:
|
|||
|
||||
|
||||
EPSILON = 1e-3 # 1 milissegundo, pra evitar divisão por zero
|
||||
self.mostrar_log(f"Tempo get_depth_frame: {(t1 - t0):.3f}s ({1 / max(t1 - t0, EPSILON):.3f} Hz)")
|
||||
self.mostrar_log(f"Tempo calculo_limiar: {(t2 - t1):.3f}s ({1 / max(t2 - t1, EPSILON):.3f} Hz)")
|
||||
#self.mostrar_log(f"Tempo get_depth_frame: {(t1 - t0):.3f}s ({1 / max(t1 - t0, EPSILON):.3f} Hz)")
|
||||
#self.mostrar_log(f"Tempo calculo_limiar: {(t2 - t1):.3f}s ({1 / max(t2 - t1, EPSILON):.3f} Hz)")
|
||||
self.mostrar_log(f"Tempo segmentacao_semantica: {(t3 - t2):.3f}s ({1 / max(t3 - t2, EPSILON):.3f} Hz)")
|
||||
self.mostrar_log(f"Tempo matriz_confianca: {(t4 - t3):.3f}s ({1 / max(t4 - t3, EPSILON):.3f} Hz)")
|
||||
self.mostrar_log(f"Tempo analise_anomalias: {(t5 - t4):.3f}s ({1 / max(t5 - t4, EPSILON):.3f} Hz)")
|
||||
self.mostrar_log(f"Tempo analise_solo: {(t6 - t5):.3f}s ({1 / max(t6 - t5, EPSILON):.3f} Hz)")
|
||||
self.mostrar_log(f"Tempo radar_2d: {(t7 - t6):.3f}s ({1 / max(t7 - t6, EPSILON):.3f} Hz)")
|
||||
#self.mostrar_log(f"Tempo analise_anomalias: {(t5 - t4):.3f}s ({1 / max(t5 - t4, EPSILON):.3f} Hz)")
|
||||
#self.mostrar_log(f"Tempo analise_solo: {(t6 - t5):.3f}s ({1 / max(t6 - t5, EPSILON):.3f} Hz)")
|
||||
#self.mostrar_log(f"Tempo radar_2d: {(t7 - t6):.3f}s ({1 / max(t7 - t6, EPSILON):.3f} Hz)")
|
||||
self.mostrar_log(f"Tempo matriz_custo: {(t8 - t7):.3f}s ({1 / max(t8 - t7, EPSILON):.3f} Hz)")
|
||||
self.mostrar_log(f"Tempo analise_total: {(t8 - t0):.3f}s ({1 / max(t8 - t0, EPSILON):.3f} Hz)")
|
||||
|
||||
|
||||
self._mostrar_debug_integrado(
|
||||
rgb_frame,
|
||||
grid_conf,
|
||||
analise_solo["perfil"],
|
||||
linhas_solo,
|
||||
analise_solo["inclinacao"],
|
||||
analise_anomalias["deteccoes"],
|
||||
analise_radar["analise"]["corredor_perfil"],
|
||||
analise_segmentacao["corredor_perfil"],
|
||||
fov_h,
|
||||
analise_radar["analise"]["bboxes_sombra"],
|
||||
analise_radar["analise"]["grid_info"]["grid_x"],
|
||||
analise_radar["analise"]["grid_info"]["grid_z"],
|
||||
segmentacao_vis,
|
||||
True
|
||||
)
|
||||
#self._mostrar_debug_integrado(
|
||||
# rgb_frame,
|
||||
# grid_conf,
|
||||
# analise_solo["perfil"],
|
||||
# linhas_solo,
|
||||
# analise_solo["inclinacao"],
|
||||
# analise_anomalias["deteccoes"],
|
||||
# analise_radar["analise"]["corredor_perfil"],
|
||||
# analise_segmentacao["corredor_perfil"],
|
||||
# fov_h,
|
||||
# analise_radar["analise"]["bboxes_sombra"],
|
||||
# analise_radar["analise"]["grid_info"]["grid_x"],
|
||||
# analise_radar["analise"]["grid_info"]["grid_z"],
|
||||
# segmentacao_vis,
|
||||
# True
|
||||
#)
|
||||
|
||||
|
||||
return resultado
|
||||
|
|
@ -305,11 +350,7 @@ class CameraManager:
|
|||
cv2.imwrite(caminho_heatmap, heatmap_frame)
|
||||
frames_salvos.append(caminho_heatmap)
|
||||
|
||||
analise = self.realizar_analise(
|
||||
[VisualWorkerTipoAnalise.RADAR_2D, VisualWorkerTipoAnalise.SEGMENTACAO_SEMANTICA],
|
||||
0.0,
|
||||
self.camera.parametros["distancia_maxima"] / 1000.0
|
||||
)
|
||||
analise = self.get_analise_mais_recente()
|
||||
|
||||
for res in analise.keys():
|
||||
if (res == "timestamp"):
|
||||
|
|
@ -654,14 +695,14 @@ class CameraManager:
|
|||
z2 = perfil_solo[j]
|
||||
if z1 <= 0 or z2 <= 0:
|
||||
continue
|
||||
y1 = self._converter_profundidade_para_y(z1, grid_conf, h_step)
|
||||
y2 = self._converter_profundidade_para_y(z2, grid_conf, h_step)
|
||||
y1 = self._converter_profundidade_para_y(z1, grid_conf, h_step) or 0
|
||||
y2 = self._converter_profundidade_para_y(z2, grid_conf, h_step) or 0
|
||||
x1 = (j - 1) * w_step
|
||||
x2 = j * w_step
|
||||
cv2.line(overlay, (x1, y1), (x2, y2), (0, 0, 0), 2) # linha preta do perfil
|
||||
# Linha de referência da média do solo
|
||||
media_z = np.nanmean(perfil_solo)
|
||||
y_ref = self._converter_profundidade_para_y(media_z, grid_conf, h_step)
|
||||
y_ref = self._converter_profundidade_para_y(media_z, grid_conf, h_step) or 0
|
||||
cv2.line(overlay, (0, y_ref), (largura, y_ref), (100, 100, 100), 1)
|
||||
cv2.putText(overlay, f"Media: {media_z:.2f}m", (10, y_ref - 5), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (100, 100, 100), 1)
|
||||
# Linha superior da faixa de análise (por qtd de linhas da grid_conf)
|
||||
|
|
@ -752,23 +793,20 @@ class CameraManager:
|
|||
mais_proximo = min(perfil_corredor, key=lambda item: abs(item["distancia_m"] - profundidade_alvo))
|
||||
return mais_proximo
|
||||
|
||||
def _gerar_matriz_custo(self, matriz_conf, bboxes_anomalias, perfil_corredor_seg, perfil_corredor_prof, perfil_solo, linha_max_solo, altura_m, largura_robo_m, fov_h_rad):
|
||||
def _gerar_matriz_custo(self, matriz_conf, bboxes_anomalias, bboxes_sombra, perfil_corredor_seg, perfil_corredor_prof, perfil_solo, linha_max_solo, largura_robo_m):
|
||||
matriz_custo = None
|
||||
try:
|
||||
altura_grid = len(matriz_conf)
|
||||
largura_grid = len(matriz_conf[0])
|
||||
escala_z = altura_m / altura_grid
|
||||
centro = largura_grid // 2
|
||||
|
||||
# Calcula largura real visível em cada linha usando FOV horizontal
|
||||
larguras_linha = [2 * (i * escala_z) * math.tan(fov_h_rad / 2) for i in range(altura_grid)]
|
||||
linhas_info = analisar_linhas_por_profundidade(matriz_conf, "prof_ref")
|
||||
|
||||
matriz_custo = [
|
||||
[
|
||||
{
|
||||
"custo": 0.0,
|
||||
"escala_x": larguras_linha[i] / largura_grid,
|
||||
"distancia_m": 0.0
|
||||
"escala_x": linhas_info[i]["escala_x"],
|
||||
"distancia_m": linhas_info[i]["distancia"]
|
||||
}
|
||||
for j in range(largura_grid)
|
||||
]
|
||||
|
|
@ -798,33 +836,25 @@ class CameraManager:
|
|||
matriz_custo[i][j] = {
|
||||
"custo": custo,
|
||||
"distancia_m": cel.get("prof_ref", 0.0),
|
||||
"escala_x": larguras_linha[i] / largura_grid
|
||||
"escala_x": linhas_info[i]["escala_x"]
|
||||
}
|
||||
|
||||
# Estreitamento do corredor (segmentação + profundidade)
|
||||
if perfil_corredor_seg is not None and perfil_corredor_prof is not None:
|
||||
if False and perfil_corredor_seg is not None and perfil_corredor_prof is not None:
|
||||
for i in range(altura_grid):
|
||||
perfil_seg = self._encontrar_item_perfil_corredor(perfil_corredor_seg, linhas_info[i]["distancia"])
|
||||
perfil_prof = self._encontrar_item_perfil_corredor(perfil_corredor_prof, linhas_info[i]["distancia"])
|
||||
if perfil_seg is None or perfil_prof is None:
|
||||
continue
|
||||
largura_seg = perfil_seg["largura_m"]
|
||||
largura_prof = perfil_prof["largura_m"]
|
||||
largura_fundida = (0.7 * largura_seg) + (0.3 * largura_prof)
|
||||
margem = 0.1
|
||||
largura_esperada = largura_robo_m + margem
|
||||
folga = largura_fundida - largura_esperada
|
||||
custo = -folga * 5.0
|
||||
print(f"Linha {i}, Largura Prof: {largura_prof}, Largura Seg: {largura_seg}, Largura Fundida: {largura_fundida}, Folga: {folga}, Custo: {custo}")
|
||||
for j in range(largura_grid):
|
||||
cel = matriz_conf[i][j]
|
||||
prof_ref = cel.get("prof_ref")
|
||||
if prof_ref is None:
|
||||
continue
|
||||
|
||||
perfil_seg = self._encontrar_item_perfil_corredor(perfil_corredor_seg, prof_ref)
|
||||
perfil_prof = self._encontrar_item_perfil_corredor(perfil_corredor_prof, prof_ref)
|
||||
|
||||
if perfil_seg is None or perfil_prof is None:
|
||||
continue
|
||||
|
||||
largura_seg = perfil_seg["largura_m"]
|
||||
largura_prof = perfil_prof["largura_m"]
|
||||
largura_fundida = (0.7 * largura_seg) + (0.3 * largura_prof)
|
||||
|
||||
margem = 0.1
|
||||
largura_esperada = largura_robo_m + margem
|
||||
folga = largura_fundida - largura_esperada
|
||||
custo = -folga * 5.0
|
||||
|
||||
matriz_custo[i][j]["custo"] += custo
|
||||
|
||||
# Custo por variacao no perfil do solo
|
||||
|
|
@ -846,37 +876,35 @@ class CameraManager:
|
|||
for i in range(altura_grid - 1, altura_grid - linhas_para_aplicar - 1, -1):
|
||||
matriz_custo[i][j]["custo"] += penalidade
|
||||
|
||||
# Custo por anomalias (bboxes)
|
||||
if bboxes_anomalias:
|
||||
if bboxes_anomalias or bboxes_sombra:
|
||||
multiplicador_base = 1.2
|
||||
custo_minimo = 1.0
|
||||
custo_maximo_extra = 5.0
|
||||
for bbox in bboxes_anomalias:
|
||||
|
||||
bboxes = bboxes_anomalias + bboxes_sombra
|
||||
|
||||
for bbox in bboxes:
|
||||
if "bbox" not in bbox:
|
||||
continue # pula bboxes inválidas
|
||||
continue
|
||||
x0_pct, y0_pct, x1_pct, y1_pct = bbox["bbox"]
|
||||
tipo = bbox.get("tipo", "obstaculo")
|
||||
# Tamanho da bbox (percentual da imagem)
|
||||
w_pct = abs(x1_pct - x0_pct)
|
||||
h_pct = abs(y1_pct - y0_pct)
|
||||
# Distância do centro da bbox (já tá em metros)
|
||||
dist_m = bbox.get("distancia_m", 0.0)
|
||||
# Converter as dimensões de percentual para metros em cada linha
|
||||
cel_ini_z = int(dist_m / escala_z)
|
||||
cel_fim_z = int((dist_m + h_pct) / escala_z)
|
||||
for i in range(cel_ini_z, cel_fim_z + 1):
|
||||
if 0 <= i < altura_grid:
|
||||
escala_x_i = matriz_custo[i][largura_grid // 2]["escala_x"]
|
||||
cel_ini_x = int(x0_pct * larguras_linha[i] / escala_x_i)
|
||||
cel_fim_x = int(x1_pct * larguras_linha[i] / escala_x_i)
|
||||
for j in range(cel_ini_x, cel_fim_x + 1):
|
||||
if 0 <= j < largura_grid:
|
||||
custo_atual = matriz_custo[i][j]["custo"]
|
||||
if custo_atual < custo_minimo:
|
||||
matriz_custo[i][j]["custo"] += custo_minimo
|
||||
else:
|
||||
penalidade = min(custo_atual * (multiplicador_base - 1), custo_maximo_extra)
|
||||
matriz_custo[i][j]["custo"] += penalidade
|
||||
cel_ini_i = int(y0_pct * altura_grid)
|
||||
cel_fim_i = int(y1_pct * altura_grid)
|
||||
cel_ini_j = int(x0_pct * largura_grid)
|
||||
cel_fim_j = int(x1_pct * largura_grid)
|
||||
for i in range(cel_ini_i, cel_fim_i + 1):
|
||||
if not (0 <= i < altura_grid):
|
||||
continue
|
||||
for j in range(cel_ini_j, cel_fim_j + 1):
|
||||
if not (0 <= j < largura_grid):
|
||||
continue
|
||||
custo_atual = matriz_custo[i][j]["custo"]
|
||||
if custo_atual < custo_minimo:
|
||||
matriz_custo[i][j]["custo"] += custo_minimo
|
||||
else:
|
||||
penalidade = min(custo_atual * (multiplicador_base - 1), custo_maximo_extra)
|
||||
matriz_custo[i][j]["custo"] += penalidade
|
||||
|
||||
|
||||
except Exception as e:
|
||||
self.mostrar_log(f"❌ Erro ao gerar matriz de custo: {e} | i={i} j={j}")
|
||||
|
|
@ -934,4 +962,211 @@ class CameraManager:
|
|||
cv2.imshow("Matriz de Custo (Debug)", img_debug)
|
||||
cv2.waitKey(1)
|
||||
|
||||
|
||||
|
||||
def _gerar_matriz_custo_fundida(self, matriz_conf, largura_robo_m):
|
||||
altura_grid = len(matriz_conf)
|
||||
largura_grid = len(matriz_conf[0])
|
||||
matriz_custo = []
|
||||
linhas_info = analisar_linhas_por_profundidade(matriz_conf, "prof_ref")
|
||||
for i in range(altura_grid):
|
||||
linha_custo = []
|
||||
for j in range(largura_grid):
|
||||
cel = matriz_conf[i][j]
|
||||
escala_x = linhas_info[i]["escala_x"]
|
||||
prof_ref = cel.get("prof_ref", 0.0)
|
||||
pos_pct = j / largura_grid
|
||||
# 🟢 Custo Baseado em Segmentação
|
||||
peso_seg = 2.5
|
||||
_is = cel.get("indice_seg_chao", 1.0)
|
||||
custo_segmentacao = (1.0 - _is) * peso_seg
|
||||
# 🟡 Custo por Confiabilidade
|
||||
peso_conf = 2.0
|
||||
_ic = cel.get("indice_confiabilidade", 1.0)
|
||||
custo_confianca = (1.0 - _ic) * peso_conf
|
||||
# 🔵 Custo por delta de profundidade (suavidade)
|
||||
peso_delta = 1.0
|
||||
_delta = abs(cel.get("prof_delta", 0.0))
|
||||
custo_delta = _delta * peso_delta
|
||||
# 🟣 Penalidade leve nas bordas (ajuda na centralização)
|
||||
peso_borda = 1.0
|
||||
penalidade_borda = peso_borda * (1 - (1 - 2 * abs(pos_pct - 0.5)))**2
|
||||
# 🧠 Bonificação por estar no caminho do mapa (adicionamos depois)
|
||||
bonificacao_mapa = 0.0
|
||||
custo_total = (
|
||||
custo_segmentacao +
|
||||
custo_confianca +
|
||||
custo_delta +
|
||||
penalidade_borda +
|
||||
bonificacao_mapa # negativa quando for aplicada
|
||||
)
|
||||
linha_custo.append({
|
||||
"linha": i,
|
||||
"coluna": j,
|
||||
"custo": custo_total,
|
||||
"distancia_m": prof_ref,
|
||||
"escala_x": escala_x,
|
||||
"indice_seg_chao": _is,
|
||||
"indice_confiabilidade": _ic,
|
||||
"prof_delta": _delta,
|
||||
"em_caminho": False
|
||||
})
|
||||
matriz_custo.append(linha_custo)
|
||||
|
||||
# 🔵 Após montar, adiciona bonificação do caminho do mapa (se houver)
|
||||
from shared.contexto_global import ContextoGlobal
|
||||
pontos_xy = ContextoGlobal.get_operacao("pontos_mapa") or []
|
||||
#print(f"Pontos do mapa: {len(pontos_xy)}")
|
||||
if pontos_xy:
|
||||
gps = ContextoGlobal.get_item(T_Code.Gps, "dados") or {}
|
||||
lat_atual = gps.get("lat", 0)
|
||||
lon_atual = gps.get("lon", 0)
|
||||
theta_robo = np.radians(gps.get("theta", 0.0))
|
||||
|
||||
# Converte usando mesmo ponto de referência do mapa
|
||||
p_ref = ContextoGlobal.get_operacao("ponto_mapa_ref") or (0, 0)
|
||||
gps_handler = GPSHandler(p_ref[0], p_ref[1])
|
||||
x_robo, z_robo = gps_handler.converter_latlon_para_xz(lat_atual, lon_atual)
|
||||
|
||||
idx_proximo = ContextoGlobal.get_item(T_Code.Trajetoria, "dados", "idx_proximo_ponto") or 0
|
||||
pontos_plotar = []
|
||||
distancia_max = 3.0 # linhas_info[0]["distancia"]
|
||||
|
||||
#print(f"idx_proximo: {idx_proximo}, theta: {round(np.degrees(theta_robo), 2)}")
|
||||
|
||||
for i in range(idx_proximo, len(pontos_xy)):
|
||||
p = pontos_xy[i]
|
||||
x_mapa, z_mapa = p["xy"]
|
||||
|
||||
x_rel, z_rel = gps_handler.converter_para_referencial_robo(x_mapa, z_mapa, x_robo, z_robo, theta_robo)
|
||||
#print(f"Ponto: {i}, x_rel: {x_rel}, z_rel: {z_rel}, x_robo: {x_robo}, z_robo: {z_robo}")
|
||||
if z_rel <= 0:
|
||||
continue # está atrás do robô, ignora
|
||||
|
||||
dist = np.sqrt(x_rel**2 + z_rel**2)
|
||||
#print(f"Distancia: {dist}")
|
||||
if dist <= distancia_max:
|
||||
# Substitui ponto original por ponto no referencial do robô
|
||||
p_relativo = {
|
||||
"xy": (x_rel, z_rel),
|
||||
"tipo": p.get("tipo", 3),
|
||||
"distanciaMargem": p.get("distanciaMargem", 0.7)
|
||||
}
|
||||
pontos_plotar.append(p_relativo)
|
||||
else:
|
||||
break # parou de estar no alcance da câmera
|
||||
|
||||
#print(f"Pontos no radar: {len(pontos_plotar)}")
|
||||
matriz_custo = self._aplicar_bonus_caminho(matriz_custo, pontos_plotar, largura_robo_m)
|
||||
|
||||
return matriz_custo
|
||||
|
||||
def _aplicar_bonus_caminho(self, matriz_custo, caminho_xy, largura_robo_m=0.85, margem_segurança_m=0.1):
|
||||
altura_grid = len(matriz_custo)
|
||||
largura_grid = len(matriz_custo[0])
|
||||
faixa_total = largura_robo_m + margem_segurança_m # largura total da faixa bonificada
|
||||
for ponto in caminho_xy:
|
||||
x_m, z_m = ponto["xy"][0], ponto["xy"][1]
|
||||
#print(f"x_m: {round(x_m,2)}, z_m: {round(z_m,2)}")
|
||||
for i in range(altura_grid):
|
||||
for j in range(largura_grid):
|
||||
cel = matriz_custo[i][j]
|
||||
escala_x = cel["escala_x"]
|
||||
centro_j = (largura_grid - 1) / 2 # funciona para par e ímpar
|
||||
x_cel = (j - centro_j) * escala_x
|
||||
z_cel = cel["distancia_m"]
|
||||
dist_lateral = abs(x_cel - x_m)
|
||||
dist_frontal = abs(z_cel - z_m)
|
||||
# 🔹 Bonifica as células dentro da largura do robô + margem
|
||||
if dist_frontal < 0.5 and dist_lateral <= faixa_total / 2:
|
||||
cel["custo"] -= 1.5 # bônus
|
||||
cel["em_caminho"] = True
|
||||
return matriz_custo
|
||||
|
||||
def _mostrar_debug_matriz_custo_fundida(self, rgb_frame: np.ndarray, matriz_custo: list, exibir_debug: bool = False):
|
||||
if not exibir_debug:
|
||||
return
|
||||
|
||||
img_debug = cv2.resize(rgb_frame.copy(), (1600, 900))
|
||||
overlay = img_debug.copy()
|
||||
|
||||
altura, largura, _ = img_debug.shape
|
||||
grid_h = len(matriz_custo)
|
||||
grid_w = len(matriz_custo[0])
|
||||
h_step = altura // grid_h
|
||||
w_step = largura // grid_w
|
||||
|
||||
todos_custos = [cel["custo"] for linha in matriz_custo for cel in linha]
|
||||
custo_min = min(todos_custos)
|
||||
custo_max = max(todos_custos)
|
||||
delta_custo = custo_max - custo_min if custo_max != custo_min else 1.0
|
||||
|
||||
mostrar_texto = grid_h <= 20 and grid_w <= 20
|
||||
|
||||
for i in range(grid_h):
|
||||
for j in range(grid_w):
|
||||
cel = matriz_custo[i][j]
|
||||
y0, y1 = i * h_step, (i + 1) * h_step
|
||||
x0, x1 = j * w_step, (j + 1) * w_step
|
||||
|
||||
custo = cel["custo"]
|
||||
norm = (custo - custo_min) / delta_custo
|
||||
r = int(255 * norm)
|
||||
g = int(255 * (1.0 - norm))
|
||||
cor = (0, g, r)
|
||||
|
||||
cv2.rectangle(overlay, (x0, y0), (x1, y1), cor, -1)
|
||||
|
||||
if cel.get("em_caminho", False):
|
||||
cv2.rectangle(overlay, (x0, y0), (x1, y1), (255, 100, 0), 1) # contorno azul
|
||||
|
||||
if mostrar_texto:
|
||||
pos_x, pos_y = x0 + 2, y0 + 12
|
||||
cv2.putText(overlay, f"{custo:.2f}", (pos_x, pos_y), cv2.FONT_HERSHEY_SIMPLEX, 0.35, (255, 255, 255), 1)
|
||||
|
||||
# Aplica overlay com transparência
|
||||
alpha = 0.4
|
||||
cv2.addWeighted(overlay, alpha, img_debug, 1 - alpha, 0, img_debug)
|
||||
|
||||
# Linha central do robô
|
||||
cv2.line(img_debug, (largura // 2, 0), (largura // 2, altura), (150, 150, 150), 1)
|
||||
|
||||
# Se tiver ponto do caminho, desenha seta direcional
|
||||
try:
|
||||
from shared.contexto_global import ContextoGlobal
|
||||
pontos_xy = ContextoGlobal.get_operacao("pontos_mapa") or []
|
||||
gps = ContextoGlobal.get_item(T_Code.Gps, "dados") or {}
|
||||
if pontos_xy and gps:
|
||||
lat, lon = gps.get("lat", 0), gps.get("lon", 0)
|
||||
theta_robo = np.radians(gps.get("theta", 0.0))
|
||||
from shared.gps_handler import GPSHandler
|
||||
p_ref = ContextoGlobal.get_operacao("ponto_mapa_ref") or (0, 0)
|
||||
handler = GPSHandler(p_ref[0], p_ref[1])
|
||||
x_robo, z_robo = handler.converter_latlon_para_xz(lat, lon)
|
||||
|
||||
# Pega o próximo ponto (idx já considerado pelo seu sistema)
|
||||
idx_proximo = ContextoGlobal.get_item(T_Code.Trajetoria, "dados", "idx_proximo_ponto") or 0
|
||||
#idx_proximo -= 1
|
||||
if idx_proximo < len(pontos_xy):
|
||||
p = pontos_xy[idx_proximo]["xy"]
|
||||
dx, dz = p[0] - x_robo, p[1] - z_robo
|
||||
x_rel, z_rel = handler.converter_para_referencial_robo(p[0], p[1], x_robo, z_robo, theta_robo)
|
||||
|
||||
angulo = np.arctan2(x_rel, z_rel) # com x e z invertidos, pois Z vai para cima
|
||||
|
||||
# Posição da seta no topo da imagem
|
||||
centro_x = largura // 2
|
||||
centro_y = 50
|
||||
raio = 50 # tamanho da seta
|
||||
destino_x = int(centro_x + raio * np.sin(angulo))
|
||||
destino_y = int(centro_y - raio * np.cos(angulo))
|
||||
|
||||
cv2.arrowedLine(img_debug, (centro_x, centro_y), (destino_x, destino_y), (255, 0, 255), 2, tipLength=0.3)
|
||||
cv2.circle(img_debug, (centro_x, centro_y), 4, (255, 0, 255), -1)
|
||||
|
||||
pos_x, pos_y = centro_x - 20, centro_y + 12
|
||||
cv2.putText(img_debug, f"{idx_proximo}, {np.degrees(angulo):.2f}, {z_rel:.2f} m", (pos_x, pos_y), cv2.FONT_HERSHEY_SIMPLEX, 0.35, (255, 0, 255), 1)
|
||||
except Exception as e:
|
||||
print(f"❌ Erro ao desenhar seta de direção: {e}")
|
||||
|
||||
cv2.imshow("Matriz de Custo (Debug)", img_debug)
|
||||
cv2.waitKey(1)
|
||||
|
|
|
|||
|
|
@ -17,17 +17,20 @@ manager = CameraManager(mostrar_log)
|
|||
def get_camera_manager():
|
||||
return manager
|
||||
|
||||
def atualiar_camera_manager(mx_id):
|
||||
def iniciar_camera_manager(mx_id):
|
||||
global manager, ultima_atualizacao
|
||||
if mx_id is None:
|
||||
return
|
||||
agora = time.time()
|
||||
if ((agora - ultima_atualizacao) > 5 and (manager is None or manager.camera is None or manager.mx_id != mx_id)):
|
||||
ultima_atualizacao = agora
|
||||
mostrar_log("Atualizando Camera Manager...")
|
||||
mostrar_log("Iniciando Camera Manager...")
|
||||
manager = CameraManager(mostrar_log=mostrar_log, mx_id=mx_id)
|
||||
if manager.camera is not None:
|
||||
mostrar_log(f"Camera manager atualizado, com MX_ID: {mx_id}")
|
||||
mostrar_log(f"Camera manager iniciado, com MX_ID: {mx_id}")
|
||||
|
||||
def tentar_atualizar_camera_manager():
|
||||
def atualizar_camera_manager():
|
||||
global manager
|
||||
mostrar_log("Atualizando camera manager...")
|
||||
if manager.mx_id is not None and manager.camera is None:
|
||||
atualiar_camera_manager(manager.mx_id)
|
||||
iniciar_camera_manager(manager.mx_id)
|
||||
|
|
|
|||
|
|
@ -2,83 +2,88 @@ import sys
|
|||
import os
|
||||
import time
|
||||
import json
|
||||
import cv2
|
||||
|
||||
sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), "..")))
|
||||
print("🔎 Caminho sys.path:", os.path.abspath(os.path.join(os.path.dirname(__file__), "..")))
|
||||
def main():
|
||||
sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), "..")))
|
||||
print("🔎 Caminho sys.path:", os.path.abspath(os.path.join(os.path.dirname(__file__), "..")), "VisualWorker")
|
||||
|
||||
from shared.mqtt_handler import registrar_callback, enviar_mensagem_mqtt, enviar_mensagem_script_carregado
|
||||
from shared.mensagem_worker import WorkerFilaMensagens
|
||||
from shared.enums import T_Code, VisualWorkerCommandType
|
||||
from visual_worker.config import mostrar_log, module_id, topico_tx, topico_rx, get_camera_manager
|
||||
from shared.mqtt_handler import iniciar_mqtt, registrar_callback, enviar_mensagem_mqtt, enviar_mensagem_script_carregado
|
||||
from shared.mensagem_worker import WorkerFilaMensagens
|
||||
from shared.enums import T_Code, VisualWorkerCommandType
|
||||
from visual_worker.config import mostrar_log, module_id, topico_tx, topico_rx, get_camera_manager, iniciar_camera_manager, atualizar_camera_manager
|
||||
|
||||
# 🔹 Callback que o MQTT chama
|
||||
def ao_receber_comando(topico, payload):
|
||||
fila_mensagens.adicionar(topico, payload)
|
||||
|
||||
# 🔹 Função de processamento dos comandos recebidos
|
||||
def processar_comando(topico, message):
|
||||
try:
|
||||
# Aqui você decodifica o comando e processa normalmente
|
||||
mensagem = json.loads(message.decode())
|
||||
dispositivo = T_Code(mensagem.get("dispositivo"))
|
||||
codigo = int(mensagem.get("comando"))
|
||||
payload = mensagem.get("payload", {})
|
||||
|
||||
# 🔹 Callback que o MQTT chama
|
||||
def ao_receber_comando(topico, payload):
|
||||
fila_mensagens.adicionar(topico, payload)
|
||||
#mostrar_log(f"📨 Mensagem recebida no tópico {topico}. Comando recebido: {dispositivo}, payload: {payload}")
|
||||
#mostrar_log(f"Comando recebido: {dispositivo}")
|
||||
|
||||
# 🔹 Função de processamento dos comandos recebidos
|
||||
def processar_comando(topico, message):
|
||||
try:
|
||||
# Aqui você decodifica o comando e processa normalmente
|
||||
mensagem = json.loads(message.decode())
|
||||
dispositivo = T_Code(mensagem.get("dispositivo"))
|
||||
codigo = int(mensagem.get("comando"))
|
||||
payload = mensagem.get("payload", {})
|
||||
resposta = {}
|
||||
|
||||
#mostrar_log(f"📨 Mensagem recebida no tópico {topico}. Comando recebido: {dispositivo}, payload: {payload}")
|
||||
#mostrar_log(f"Comando recebido: {dispositivo}")
|
||||
|
||||
resposta = {}
|
||||
|
||||
if codigo == VisualWorkerCommandType.SALVAR_FRAMES:
|
||||
manager = get_camera_manager()
|
||||
frames = manager.salvar_frames()
|
||||
resposta = {
|
||||
"save_frames": {
|
||||
"timestamp": time.time(),
|
||||
"frames": frames
|
||||
}
|
||||
}
|
||||
|
||||
if (resposta != {}):
|
||||
resposta["cmd"] = codigo
|
||||
enviar_mensagem_mqtt(topico=topico_tx, dispositivo=codigo, objeto=resposta)
|
||||
|
||||
except Exception as e:
|
||||
print("❌ Erro ao processar comando MQTT:", e)
|
||||
|
||||
|
||||
# 🔹 Inicia a fila e os workers
|
||||
fila_mensagens = WorkerFilaMensagens(id=module_id, processador_callback=processar_comando)
|
||||
|
||||
# 🔹 Execução principal
|
||||
def inicializar():
|
||||
mostrar_log("🚀 Iniciando Visual Worker...")
|
||||
|
||||
registrar_callback(topico=topico_rx, callback=ao_receber_comando)
|
||||
|
||||
enviar_mensagem_script_carregado(topico_tx)
|
||||
|
||||
tmp = 0
|
||||
try:
|
||||
while True:
|
||||
time.sleep(1)
|
||||
if (False and tmp == 10):
|
||||
if codigo == VisualWorkerCommandType.SALVAR_FRAMES:
|
||||
manager = get_camera_manager()
|
||||
segmentacao, log = manager.segmentacao_manager.segmentar(None)
|
||||
# Pega o frame colorido da segmentação
|
||||
frame_base64 = segmentacao["frame"]["frame"]
|
||||
frames = manager.salvar_frames()
|
||||
resposta = {
|
||||
"save_frames": {
|
||||
"timestamp": time.time(),
|
||||
"frames": frames
|
||||
}
|
||||
}
|
||||
elif codigo == VisualWorkerCommandType.ATUALIZAR_CAMERA_MANAGER:
|
||||
atualizar_camera_manager()
|
||||
elif codigo == VisualWorkerCommandType.INICIAR_CAMERA_MANAGER:
|
||||
mx_id = payload.get("mx_id")
|
||||
iniciar_camera_manager(mx_id)
|
||||
elif codigo == VisualWorkerCommandType.GET_ULTIMA_ANALISE:
|
||||
manager = get_camera_manager()
|
||||
resposta = manager.get_analise_mais_recente()
|
||||
|
||||
# Decodifica do base64 pra imagem
|
||||
from shared.utils import decode_image_base64
|
||||
mask_color = decode_image_base64(frame_base64)
|
||||
if (resposta != {}):
|
||||
resposta["cmd"] = codigo
|
||||
enviar_mensagem_mqtt(topico=topico_tx, dispositivo=codigo, objeto=resposta)
|
||||
|
||||
# Mostra a imagem
|
||||
cv2.imshow("Segmentação Semântica", mask_color)
|
||||
cv2.waitKey(0)
|
||||
cv2.destroyAllWindows()
|
||||
tmp += 1
|
||||
except KeyboardInterrupt:
|
||||
mostrar_log("Encerrando Visual Worker...")
|
||||
except Exception as e:
|
||||
print("❌ Erro ao processar comando MQTT:", e)
|
||||
|
||||
|
||||
# 🔹 Inicia a fila e os workers
|
||||
fila_mensagens = WorkerFilaMensagens(id=module_id, processador_callback=processar_comando)
|
||||
|
||||
|
||||
# 🔹 Loop principal do Manager
|
||||
def loop_ativo():
|
||||
while True:
|
||||
try:
|
||||
a = 1
|
||||
except KeyboardInterrupt:
|
||||
mostrar_log("🛑 Encerrando Visual Worker...")
|
||||
break
|
||||
except Exception as e:
|
||||
mostrar_log(f"❌ Erro no loop do Visual: {e}")
|
||||
|
||||
time.sleep(1)
|
||||
|
||||
|
||||
# 🔹 Execução principal
|
||||
def inicializar():
|
||||
iniciar_mqtt(module_id)
|
||||
mostrar_log("🚀 Iniciando Visual Worker...")
|
||||
registrar_callback(topico=topico_rx, callback=ao_receber_comando)
|
||||
enviar_mensagem_script_carregado(topico_tx)
|
||||
loop_ativo()
|
||||
|
||||
inicializar()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
|
|
|||
|
|
@ -11,8 +11,6 @@ class Radar2DManager:
|
|||
self.exibir_debug = False
|
||||
|
||||
def analisar_radar_2d(self, depth_frame, fov_h, distancia_max_m):
|
||||
depth_frame = cp.asarray(depth_frame)
|
||||
|
||||
distancia_min = 0.5
|
||||
passo = 0.25
|
||||
cell_size = 0.05
|
||||
|
|
@ -101,7 +99,10 @@ class Radar2DManager:
|
|||
range=[[-distancia_max, distancia_max], [0, distancia_max]]
|
||||
)
|
||||
|
||||
max_count = cp.max(hist) if cp.max(hist) > 0 else 1
|
||||
if hist.size == 0 or cp.sum(hist) == 0:
|
||||
max_count = 1
|
||||
else:
|
||||
max_count = cp.max(hist)
|
||||
|
||||
ix, iz = cp.nonzero(hist)
|
||||
counts = hist[ix, iz]
|
||||
|
|
|
|||
|
|
@ -23,7 +23,7 @@ class SegmentacaoManager:
|
|||
self.classes = None
|
||||
self.color_map = None
|
||||
self.carregado = False
|
||||
self.use_mock = True
|
||||
self.use_mock = False
|
||||
self.img_mock = "C:\\ZendionInc\\agrobot_base\\Treinamento\\models\\ruas\\dataset\\images\\81.jpeg"
|
||||
self._carregar_modelo()
|
||||
|
||||
|
|
|
|||
|
|
@ -3,6 +3,7 @@ import numpy as np
|
|||
import cupy as cp
|
||||
import math
|
||||
from scipy import stats
|
||||
from shared.enums import T_Code
|
||||
|
||||
def gerar_heatmap(depth_frame, dist_max_mm=3000.0):
|
||||
mask_valid = depth_frame <= dist_max_mm
|
||||
|
|
@ -12,11 +13,14 @@ def gerar_heatmap(depth_frame, dist_max_mm=3000.0):
|
|||
heatmap[~mask_valid] = [0, 0, 0]
|
||||
return heatmap
|
||||
|
||||
def calcular_threshold_anomalias(velocidade_ms, threshold_base=0.15, fator_sensibilidade=100):
|
||||
def calcular_threshold_anomalias(velocidade_ms=None, threshold_base=0.15, fator_sensibilidade=100):
|
||||
"""
|
||||
Calcula o threshold dinamicamente baseado na velocidade do robô.
|
||||
- velocidade_ms: Velocidade em metros por segundo (m/s).
|
||||
"""
|
||||
if velocidade_ms is None:
|
||||
from shared.contexto_global import ContextoGlobal
|
||||
velocidade_ms = ContextoGlobal.get_item(T_Code.Gerais, "dados", "velocidade_ms") or 0.0
|
||||
threshold = threshold_base + (velocidade_ms * fator_sensibilidade)
|
||||
return round(threshold, 3)
|
||||
|
||||
|
|
@ -88,6 +92,9 @@ def calcular_profundidades_setores(depth_solo, n_setores, largura, distancia_max
|
|||
return cp.asnumpy(cp.array(profundidades)), largura_setor
|
||||
|
||||
def histogram2d_gpu(x, y, bins, range):
|
||||
if x.size == 0 or y.size == 0:
|
||||
return cp.zeros(bins, dtype=cp.int32)
|
||||
|
||||
x_bins, y_bins = bins
|
||||
(x_min, x_max), (y_min, y_max) = range
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue