otimizacao do MPC
This commit is contained in:
Diego Freitas
parent
d1e8828e0f
commit
5de48f8ffd
Binary file not shown.
Binary file not shown.
|
|
@ -184,8 +184,8 @@ namespace AgroBase.Models
|
|||
{ TipoMovimentoDirecional.MovimentoLateral, 90 },
|
||||
{ TipoMovimentoDirecional.Diagnostico, 25 },
|
||||
};
|
||||
public static double AnguloInclinacaoRollMax { get; set; } = 15.0;
|
||||
public static double AnguloInclinacaoPitchMax { get; set; } = 30.0;
|
||||
public static double AnguloInclinacaoRollMax { get; set; } = 30.0;
|
||||
public static double AnguloInclinacaoPitchMax { get; set; } = 15.0;
|
||||
|
||||
public static SensorSinaleiroComportamentoModel ComportamentoPorStatus(StatusOperacao? _status = null)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -213,7 +213,7 @@ namespace AgroBase.Services.Operadores
|
|||
distanciaMargem = p.LarguraCorredor * 0.8
|
||||
})
|
||||
.ToList(),
|
||||
horizonte = 2.0,
|
||||
horizonte = 2.5,
|
||||
angulo_max_graus = _Controle.Angulo_Max,
|
||||
velocidade_min = FuncoesMatematicas.CalculaVelocidadeMsPercentual(_Controle.PercentualVelocidadeMin),
|
||||
velocidade_max = FuncoesMatematicas.CalculaVelocidadeMsPercentual(_Controle.PercentualVelocidadeMax),
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
|
|
@ -29,5 +29,5 @@
|
|||
"top_topics_and_observing_domains": [ ]
|
||||
} ],
|
||||
"hex_encoded_hmac_key": "40F346D3248C3AFDF2BEE1FE496DBD32F7CED6E5AE98B881ABC421AA7E7B5642",
|
||||
"next_scheduled_calculation_time": "13399758321058748"
|
||||
"next_scheduled_calculation_time": "13399758321059072"
|
||||
}
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
Binary file not shown.
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,3 +1,3 @@
|
|||
2025/08/11-17:15:07.450 43b4 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Local Storage\leveldb/MANIFEST-000001
|
||||
2025/08/11-17:15:07.459 43b4 Recovering log #3
|
||||
2025/08/11-17:15:07.463 43b4 Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Local Storage\leveldb/000003.log
|
||||
2025/08/13-13:54:08.741 7834 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Local Storage\leveldb/MANIFEST-000001
|
||||
2025/08/13-13:54:08.748 7834 Recovering log #3
|
||||
2025/08/13-13:54:08.751 7834 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/08/11-16:38:48.533 8fe4 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Local Storage\leveldb/MANIFEST-000001
|
||||
2025/08/11-16:38:48.540 8fe4 Recovering log #3
|
||||
2025/08/11-16:38:48.543 8fe4 Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Local Storage\leveldb/000003.log
|
||||
2025/08/13-13:43:58.156 80a4 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Local Storage\leveldb/MANIFEST-000001
|
||||
2025/08/13-13:43:58.162 80a4 Recovering log #3
|
||||
2025/08/13-13:43:58.165 80a4 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":"13399499518686465","port":443,"protocol_str":"quic"}],"anonymization":["DAAAAAcAAABmaWxlOi8vAA==",false,0],"network_stats":{"srtt":95712},"server":"https://tile.openstreetmap.org","supports_spdy":true}],"supports_quic":{"address":"192.168.26.32","used_quic":true},"version":5},"network_qualities":{"CAASABiAgICA+P////8B":"4G","CAESABiAgICA+P////8B":"4G","CAISABiAgICA+P////8B":"4G","CAYSABiAgICA+P////8B":"Offline"}}}
|
||||
{"net":{"http_server_properties":{"servers":[{"alternative_service":[{"advertised_alpns":["h3"],"expiration":"13399664052836984","port":443,"protocol_str":"quic"}],"anonymization":["DAAAAAcAAABmaWxlOi8vAA==",false,0],"network_stats":{"srtt":104807},"server":"https://tile.openstreetmap.org","supports_spdy":true}],"supports_quic":{"address":"2804:d78:627:be00:7198:48fb:b727:fd7c","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":1786217642.613677,"host":"bWGAftl61rqoc0YzqPncsLvQQh/iC2Bdp3ejUeGC83w=","mode":"force-https","sts_include_subdomains":true,"sts_observed":1754681642.613684}],"version":2}
|
||||
{"sts":[{"expiry":1786639441.537365,"host":"bWGAftl61rqoc0YzqPncsLvQQh/iC2Bdp3ejUeGC83w=","mode":"force-https","sts_include_subdomains":true,"sts_observed":1755103441.537367}],"version":2}
|
||||
File diff suppressed because one or more lines are too long
|
|
@ -1,3 +1,3 @@
|
|||
2025/08/11-17:17:21.484 43b4 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Session Storage/MANIFEST-000001
|
||||
2025/08/11-17:17:21.485 43b4 Recovering log #3
|
||||
2025/08/11-17:17:21.489 43b4 Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Session Storage/000003.log
|
||||
2025/08/13-14:33:53.727 7834 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Session Storage/MANIFEST-000001
|
||||
2025/08/13-14:33:53.729 7834 Recovering log #3
|
||||
2025/08/13-14:33:53.732 7834 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/08/11-16:44:31.240 8fe4 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Session Storage/MANIFEST-000001
|
||||
2025/08/11-16:44:31.242 8fe4 Recovering log #3
|
||||
2025/08/11-16:44:31.246 8fe4 Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Session Storage/000003.log
|
||||
2025/08/13-13:53:57.357 80a4 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Session Storage/MANIFEST-000001
|
||||
2025/08/13-13:53:57.358 80a4 Recovering log #3
|
||||
2025/08/13-13:53:57.361 80a4 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/08/11-17:15:07.352 5f14 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Site Characteristics Database/MANIFEST-000001
|
||||
2025/08/11-17:15:07.354 5f14 Recovering log #7
|
||||
2025/08/11-17:15:07.354 5f14 Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Site Characteristics Database/000007.log
|
||||
2025/08/13-13:54:08.660 9848 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Site Characteristics Database/MANIFEST-000001
|
||||
2025/08/13-13:54:08.662 9848 Recovering log #7
|
||||
2025/08/13-13:54:08.662 9848 Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Site Characteristics Database/000007.log
|
||||
|
|
|
|||
|
|
@ -1,3 +1,3 @@
|
|||
2025/08/11-16:38:48.452 418c Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Site Characteristics Database/MANIFEST-000001
|
||||
2025/08/11-16:38:48.453 418c Recovering log #7
|
||||
2025/08/11-16:38:48.454 418c Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Site Characteristics Database/000007.log
|
||||
2025/08/13-13:43:58.074 35b4 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Site Characteristics Database/MANIFEST-000001
|
||||
2025/08/13-13:43:58.076 35b4 Recovering log #7
|
||||
2025/08/13-13:43:58.076 35b4 Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Site Characteristics Database/000007.log
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
Binary file not shown.
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
|
|
@ -1 +1,221 @@
|
|||
{"type":"FeatureCollection","features":[{"type":"Feature","properties":{"Id":"1","Name":"11_08_2025_16_37_30_Manual","Length":0.0,"Dist1":0.0,"Dist2":0.0},"geometry":{"id":null,"type":"LineString","coordinates":[[0.0,0.0],[0.0,0.0],[0.0,0.0],[0.0,0.0],[0.0,0.0],[0.0,0.0],[0.0,0.0],[0.0,0.0],[0.0,0.0],[0.0,0.0],[0.0,0.0],[0.0,0.0],[0.0,0.0],[0.0,0.0],[0.0,0.0],[0.0,0.0],[0.0,0.0],[0.0,0.0],[0.0,0.0]]}}]}
|
||||
{
|
||||
"type": "FeatureCollection",
|
||||
"features": [
|
||||
{
|
||||
"type": "Feature",
|
||||
"properties": {
|
||||
"Id": "1",
|
||||
"Name": "CidadeJardimTerreno2",
|
||||
"Length": 14.558011415731592,
|
||||
"Dist1": 14.558011415731592,
|
||||
"Dist2": 0.0
|
||||
},
|
||||
"geometry": {
|
||||
"id": null,
|
||||
"type": "LineString",
|
||||
"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
|
||||
]
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "Feature",
|
||||
"properties": {
|
||||
"Id": "2",
|
||||
"Name": "CidadeJardimTerreno2",
|
||||
"Length": 14.771318274761821,
|
||||
"Dist1": 14.558011415731592,
|
||||
"Dist2": 0.0
|
||||
},
|
||||
"geometry": {
|
||||
"id": null,
|
||||
"type": "LineString",
|
||||
"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
|
||||
]
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "Feature",
|
||||
"properties": {
|
||||
"Id": "3",
|
||||
"Name": "CidadeJardimTerreno2",
|
||||
"Length": 14.827915524386164,
|
||||
"Dist1": 14.558011415731592,
|
||||
"Dist2": 0.0
|
||||
},
|
||||
"geometry": {
|
||||
"id": null,
|
||||
"type": "LineString",
|
||||
"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
|
||||
]
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "Feature",
|
||||
"properties": {
|
||||
"Id": "4",
|
||||
"Name": "CidadeJardimTerreno2",
|
||||
"Length": 14.785159385903514,
|
||||
"Dist1": 14.558011415731592,
|
||||
"Dist2": 0.0
|
||||
},
|
||||
"geometry": {
|
||||
"id": null,
|
||||
"type": "LineString",
|
||||
"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
|
||||
]
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "Feature",
|
||||
"properties": {
|
||||
"Id": "5",
|
||||
"Name": "CidadeJardimTerreno2",
|
||||
"Length": 14.80117692191233,
|
||||
"Dist1": 14.558011415731592,
|
||||
"Dist2": 0.0
|
||||
},
|
||||
"geometry": {
|
||||
"id": null,
|
||||
"type": "LineString",
|
||||
"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
|
||||
]
|
||||
]
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
|
|
@ -17,7 +17,7 @@
|
|||
<meta name="viewport" content="width=device-width,
|
||||
initial-scale=1.0, maximum-scale=1.0, user-scalable=no" />
|
||||
<style>
|
||||
#map_b86da2c0cf19440a67c00659ca7d57e9 {
|
||||
#map_e21a72a5cb050d5571718c30559c2190 {
|
||||
position: relative;
|
||||
width: 100.0%;
|
||||
height: 100.0%;
|
||||
|
|
@ -54,16 +54,16 @@
|
|||
<body>
|
||||
|
||||
|
||||
<div class="folium-map" id="map_b86da2c0cf19440a67c00659ca7d57e9" ></div>
|
||||
<div class="folium-map" id="map_e21a72a5cb050d5571718c30559c2190" ></div>
|
||||
|
||||
</body>
|
||||
<script>
|
||||
|
||||
|
||||
var map_b86da2c0cf19440a67c00659ca7d57e9 = L.map(
|
||||
"map_b86da2c0cf19440a67c00659ca7d57e9",
|
||||
var map_e21a72a5cb050d5571718c30559c2190 = L.map(
|
||||
"map_e21a72a5cb050d5571718c30559c2190",
|
||||
{
|
||||
center: [0.0, 0.0],
|
||||
center: [-22.172636164916668, -47.395186322185666],
|
||||
crs: L.CRS.EPSG3857,
|
||||
...{
|
||||
"zoom": 12,
|
||||
|
|
@ -78,7 +78,7 @@
|
|||
|
||||
|
||||
|
||||
var tile_layer_b64ed6ac3009000233329a32f2decf43 = L.tileLayer(
|
||||
var tile_layer_5d7b162daa8a21dbb285f6d50426847a = L.tileLayer(
|
||||
"https://tile.openstreetmap.org/{z}/{x}/{y}.png",
|
||||
{
|
||||
"minZoom": 0,
|
||||
|
|
@ -95,7 +95,7 @@
|
|||
);
|
||||
|
||||
|
||||
tile_layer_b64ed6ac3009000233329a32f2decf43.addTo(map_b86da2c0cf19440a67c00659ca7d57e9);
|
||||
tile_layer_5d7b162daa8a21dbb285f6d50426847a.addTo(map_e21a72a5cb050d5571718c30559c2190);
|
||||
|
||||
|
||||
|
||||
|
|
@ -111,7 +111,7 @@
|
|||
}*/
|
||||
});
|
||||
}
|
||||
function geo_json_92428d205e1566c4dea13c3b7eb4954d_onEachFeature(feature, layer) {
|
||||
function geo_json_27cdeaf4853e626f96b749d12eb65555_onEachFeature(feature, layer) {
|
||||
|
||||
layer.on({
|
||||
|
||||
|
|
@ -148,23 +148,23 @@
|
|||
}*/
|
||||
});
|
||||
};
|
||||
var geo_json_92428d205e1566c4dea13c3b7eb4954d = L.geoJson(null, {
|
||||
onEachFeature: geo_json_92428d205e1566c4dea13c3b7eb4954d_onEachFeature,
|
||||
var geo_json_27cdeaf4853e626f96b749d12eb65555 = L.geoJson(null, {
|
||||
onEachFeature: geo_json_27cdeaf4853e626f96b749d12eb65555_onEachFeature,
|
||||
|
||||
...{
|
||||
}
|
||||
});
|
||||
|
||||
function geo_json_92428d205e1566c4dea13c3b7eb4954d_add (data) {
|
||||
geo_json_92428d205e1566c4dea13c3b7eb4954d
|
||||
function geo_json_27cdeaf4853e626f96b749d12eb65555_add (data) {
|
||||
geo_json_27cdeaf4853e626f96b749d12eb65555
|
||||
.addData(data);
|
||||
}
|
||||
geo_json_92428d205e1566c4dea13c3b7eb4954d_add({"features": [{"geometry": {"coordinates": [[0.0, 0.0], [0.0, 0.0], [0.0, 0.0], [0.0, 0.0], [0.0, 0.0], [0.0, 0.0], [0.0, 0.0], [0.0, 0.0], [0.0, 0.0], [0.0, 0.0], [0.0, 0.0], [0.0, 0.0], [0.0, 0.0], [0.0, 0.0], [0.0, 0.0], [0.0, 0.0], [0.0, 0.0], [0.0, 0.0], [0.0, 0.0]], "id": null, "type": "LineString"}, "id": 0, "properties": {"Dist1": 0.0, "Dist2": 0.0, "Id": "1", "Length": 0.0, "Name": "11_08_2025_16_37_30_Manual"}, "type": "Feature"}], "type": "FeatureCollection"});
|
||||
geo_json_92428d205e1566c4dea13c3b7eb4954d.setStyle(function(feature) {return feature.properties.style;});
|
||||
geo_json_27cdeaf4853e626f96b749d12eb65555_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_27cdeaf4853e626f96b749d12eb65555.setStyle(function(feature) {return feature.properties.style;});
|
||||
|
||||
|
||||
|
||||
geo_json_92428d205e1566c4dea13c3b7eb4954d.addTo(map_b86da2c0cf19440a67c00659ca7d57e9);
|
||||
geo_json_27cdeaf4853e626f96b749d12eb65555.addTo(map_e21a72a5cb050d5571718c30559c2190);
|
||||
|
||||
</script>
|
||||
|
||||
|
|
@ -185,7 +185,7 @@
|
|||
}
|
||||
trajeto_json_add({"features": []});
|
||||
|
||||
trajeto_json.addTo(map_b86da2c0cf19440a67c00659ca7d57e9);
|
||||
trajeto_json.addTo(map_e21a72a5cb050d5571718c30559c2190);
|
||||
|
||||
function adicionarGeometria(novaGeometria) {
|
||||
trajeto_json.addData(novaGeometria);
|
||||
|
|
@ -243,7 +243,7 @@
|
|||
}
|
||||
trajeto_dinamico_json_add({"features": []});
|
||||
|
||||
trajeto_dinamico_json.addTo(map_b86da2c0cf19440a67c00659ca7d57e9);
|
||||
trajeto_dinamico_json.addTo(map_e21a72a5cb050d5571718c30559c2190);
|
||||
|
||||
function adicionarGeometriaDinamica(novaGeometria) {
|
||||
trajeto_dinamico_json.addData(novaGeometria);
|
||||
|
|
@ -296,9 +296,9 @@
|
|||
|
||||
var marcadorEquipamento = L.marker([0, 0], {
|
||||
icon: customIcon
|
||||
}).addTo(map_b86da2c0cf19440a67c00659ca7d57e9);
|
||||
}).addTo(map_e21a72a5cb050d5571718c30559c2190);
|
||||
|
||||
var marcadorBase = L.marker([0, 0], {}).addTo(map_b86da2c0cf19440a67c00659ca7d57e9);
|
||||
var marcadorBase = L.marker([0, 0], {}).addTo(map_e21a72a5cb050d5571718c30559c2190);
|
||||
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_b86da2c0cf19440a67c00659ca7d57e9.setView(novaPosicao, map_b86da2c0cf19440a67c00659ca7d57e9.getZoom());
|
||||
map_e21a72a5cb050d5571718c30559c2190.setView(novaPosicao, map_e21a72a5cb050d5571718c30559c2190.getZoom());
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -397,7 +397,7 @@
|
|||
function atualizarSelecaoRuas(selecionadas) {
|
||||
selecionadas = JSON.parse(selecionadas);
|
||||
RuasSelecionadas = Array.isArray(selecionadas) ? [...selecionadas] : [];
|
||||
geo_json_92428d205e1566c4dea13c3b7eb4954d.eachLayer(function (layer) {
|
||||
geo_json_27cdeaf4853e626f96b749d12eb65555.eachLayer(function (layer) {
|
||||
if (RuasSelecionadas.includes(parseInt(layer.feature.id))) {
|
||||
layer.setStyle({ color: 'blue' });
|
||||
} else {
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -1,8 +1,12 @@
|
|||
import time
|
||||
|
||||
import numpy as np
|
||||
from shared.enums import ModoOperacao, StatusCarroMapa, StatusModulo, StatusOperacao, T_Code, TipoMovimentoDirecional, TiposControladorDirecional
|
||||
from shared.contexto_global_redis import ContextoGlobalRedis, CtxKey
|
||||
from manager_worker.config import mostrar_log
|
||||
from manager_worker.modulos.mpc import get_mpc, comando_parado
|
||||
from manager_worker.modulos.pid import PIDAdaptativo
|
||||
from visual_worker.processamento.costmap_fuser import unpack_snapshot
|
||||
|
||||
def definir_comando(pid: PIDAdaptativo):
|
||||
try:
|
||||
|
|
@ -11,12 +15,27 @@ def definir_comando(pid: PIDAdaptativo):
|
|||
|
||||
_contexto = ContextoGlobalRedis.get_contexto()
|
||||
_equipamento = ContextoGlobalRedis.get_equipamento()
|
||||
_gps = ContextoGlobalRedis.get_modulo(T_Code.Gps)
|
||||
_snr = ContextoGlobalRedis.get_modulo(T_Code.Snr)
|
||||
_dados_vw = ContextoGlobalRedis.get(CtxKey.DadosVisualWorker, {})
|
||||
_trajetoria = _contexto.get("Trajetoria", {})
|
||||
|
||||
_snr = ContextoGlobalRedis.get_modulo(T_Code.Snr)
|
||||
visual_worker_ativado = _operacao.get("Snr", {}).get("sonar_ativado", False)
|
||||
visual_worker_operante = (_snr.get("saude", {}).get("status", StatusModulo.DESCONECTADO.value)) == StatusModulo.OPERANTE.value
|
||||
_dados_vw = ContextoGlobalRedis.get(CtxKey.DadosVisualWorker, {})
|
||||
_snapshot_vw = _dados_vw.get("matrizes", {}).get("confianca")
|
||||
custo = None
|
||||
nav = None
|
||||
dists = None
|
||||
escalas = None
|
||||
if not _snapshot_vw:
|
||||
visual_worker_atualizado = False
|
||||
else:
|
||||
visual_worker_atualizado = (time.time() - _snapshot_vw.get("ts", 0.0) <= 0.15)
|
||||
if visual_worker_atualizado:
|
||||
custo, conf, anom, nav = unpack_snapshot(_snapshot_vw)
|
||||
dists = _snapshot_vw.get("row_dist_m", None)
|
||||
escalas = _snapshot_vw.get("row_scale_x_m", None)
|
||||
|
||||
_gps = ContextoGlobalRedis.get_modulo(T_Code.Gps)
|
||||
_gps_pos_atualizada = _gps.get("heartbeat", 0) != _gps.get("old_heartbeat", 0)
|
||||
_gps_passos_travados = 0
|
||||
if not _gps_pos_atualizada:
|
||||
|
|
@ -40,7 +59,7 @@ def definir_comando(pid: PIDAdaptativo):
|
|||
},
|
||||
"Carro": {
|
||||
#"Velocidade": 1.0,
|
||||
"Velocidade": max(0.5, _contexto.get("Gerais", {}).get("velocidade_ms", 0.0)),
|
||||
"Velocidade": max(0.2, _contexto.get("Gerais", {}).get("velocidade_ms", 0.0)),
|
||||
"Status": _trajetoria.get("status", StatusCarroMapa.Parado.value),
|
||||
"ManobrandoEntreRuas": _trajetoria.get("manobrando", False),
|
||||
"DentroCorredor": _trajetoria.get("CorredorAtual", {}).get("dentro", False),
|
||||
|
|
@ -55,17 +74,24 @@ def definir_comando(pid: PIDAdaptativo):
|
|||
"PassosAtraso": _operacao.get("Dir", {}).get("mpc", {}).get("passos_atraso", 1),
|
||||
},
|
||||
"VisualWorker": {
|
||||
"Ativado": _operacao.get("Snr", {}).get("sonar_ativado", False),
|
||||
"Ativado": visual_worker_ativado,
|
||||
"Operante": visual_worker_operante,
|
||||
"Atualizado": visual_worker_atualizado,
|
||||
"MatrizCusto": {
|
||||
"Valida": visual_worker_ativado and visual_worker_operante and visual_worker_atualizado,
|
||||
"Custo": custo,
|
||||
"Navegavel": nav,
|
||||
"DistanciasRef": dists,
|
||||
"EscalasX": escalas,
|
||||
},
|
||||
"Camera": {
|
||||
"FovH": ContextoGlobalRedis.get(CtxKey.DadosVisualWorker, {}).get("parametros", {}).get("fov_h", 0.7),
|
||||
"DistanciaMax": _operacao.get("Snr", {}).get("distancia_maxima", 5000) / 1000.0,
|
||||
"DistanciaMin": _operacao.get("Snr", {}).get("distancia_minima", 500) / 1000.0,
|
||||
},
|
||||
"MatrizCusto": _dados_vw.get("matrizes", {}).get("custo", [[]]),
|
||||
}
|
||||
},
|
||||
"RegrasAtivas": {
|
||||
"matriz_custo": False,
|
||||
"matriz_custo": True,
|
||||
"deteccao_obstaculos": False,
|
||||
},
|
||||
"Equipamento": {
|
||||
|
|
@ -75,12 +101,14 @@ def definir_comando(pid: PIDAdaptativo):
|
|||
"angulo_pitch_max": _equipamento.get("angulo_pitch_max", 30.0),
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
#print(contexto["VisualWorker"])
|
||||
|
||||
if _tipo_controle == TiposControladorDirecional.MPC:
|
||||
comando = _regras_taticas(contexto)
|
||||
|
||||
if (comando["comando_definido"]):
|
||||
return comando["angulo"], comando["tipo"], comando["simulacao"], comando["parada_necessaria"]
|
||||
return comando["angulo"], comando["tipo"], comando["simulacao"], comando["parada_necessaria"], False
|
||||
else:
|
||||
op_modo = ModoOperacao(_operacao.get("modo", ModoOperacao.NaoDefinido.value))
|
||||
if op_modo == ModoOperacao.MapaGPS:
|
||||
|
|
@ -89,7 +117,7 @@ def definir_comando(pid: PIDAdaptativo):
|
|||
return _comando_mapeamento_visual(contexto)
|
||||
else:
|
||||
comando = _comando_direcional_parado()
|
||||
return comando["angulo"], comando["tipo"], comando["simulacao"], comando["parada_necessaria"]
|
||||
return comando["angulo"], comando["tipo"], comando["simulacao"], comando["parada_necessaria"], False
|
||||
|
||||
elif _tipo_controle == TiposControladorDirecional.PID:
|
||||
_controle = ContextoGlobalRedis.get_controle()
|
||||
|
|
@ -154,8 +182,8 @@ def _regras_taticas(contexto):
|
|||
|
||||
imu = ContextoGlobalRedis.get_modulo(T_Code.Imu)
|
||||
if imu is not None and imu.get("saude", {}).get("status", StatusModulo.DESCONECTADO.value) == StatusModulo.OPERANTE.value:
|
||||
ang_roll_max = contexto.get("Equipamento", {}).get("angulo_roll_max", 15.0)
|
||||
ang_pitch_max = contexto.get("Equipamento", {}).get("angulo_pitch_max", 30.0)
|
||||
ang_roll_max = contexto.get("Equipamento", {}).get("angulo_roll_max", 30.0)
|
||||
ang_pitch_max = contexto.get("Equipamento", {}).get("angulo_pitch_max", 15.0)
|
||||
if abs(imu.get("pitch", 0.0)) > ang_pitch_max or abs(imu.get("roll", 0.0)) > ang_roll_max:
|
||||
mostrar_log(f"🟥 Inclinação perigosa detectada. Parando movimentação. roll: {imu.get('roll')}, pitch: {imu.get('pitch')}, yaw: {imu.get('yaw')}")
|
||||
return _comando_direcional_parado(True)
|
||||
|
|
@ -203,7 +231,7 @@ def _comando_mapa_gps_mpc(contexto):
|
|||
|
||||
#input("⏸️ Pressione Enter para continuar...")
|
||||
|
||||
return comando.get("angulo", 0.0), comando.get("tipo", TipoMovimentoDirecional.RodasDianteiras.value), comando.get("simulacao", []), comando.get("parada_necessaria", False)
|
||||
return comando.get("angulo", 0.0), comando.get("tipo", TipoMovimentoDirecional.RodasDianteiras.value), comando.get("simulacao", []), comando.get("parada_necessaria", False), comando.get("erro", False)
|
||||
except Exception as e:
|
||||
mostrar_log(f"❌ Erro ao definir comando MapaGPS: {e}")
|
||||
|
||||
|
|
@ -213,7 +241,7 @@ def _comando_mapeamento_visual(contexto):
|
|||
comando = _comando_direcional_parado(True)
|
||||
if not contexto.get("VisualWorker", {}).get("Operante", False):
|
||||
mostrar_log("Sensores principais inativos! Impossível definir controle")
|
||||
return comando["angulo"], comando["tipo"], comando["simulacao"], comando["parada_necessaria"]
|
||||
return comando["angulo"], comando["tipo"], comando["simulacao"], comando["parada_necessaria"], False
|
||||
|
||||
# 🔍 Dados visuais disponíveis
|
||||
_dados_vw = ContextoGlobalRedis.get(CtxKey.DadosVisualWorker, {}).get("segmentacao", {})
|
||||
|
|
@ -247,7 +275,7 @@ def _comando_mapeamento_visual(contexto):
|
|||
"parada_necessaria": parada_necessaria
|
||||
}
|
||||
|
||||
return comando["angulo"], comando["tipo"], comando["simulacao"], comando["parada_necessaria"]
|
||||
return comando["angulo"], comando["tipo"], comando["simulacao"], comando["parada_necessaria"], False
|
||||
|
||||
except Exception as e:
|
||||
mostrar_log(f"❌ Erro ao definir comando MapeamentoVisual: {e}")
|
||||
|
|
|
|||
|
|
@ -3,12 +3,16 @@ from shared.contexto_global_redis import ContextoGlobalRedis, CtxKey
|
|||
from manager_worker.config import mostrar_log
|
||||
from manager_worker.filtros import FiltroVelocidade
|
||||
|
||||
def definir_comando(parada_necessaria: bool, filtro_vel: FiltroVelocidade):
|
||||
def definir_comando(parada_necessaria: bool, erro: bool, filtro_vel: FiltroVelocidade):
|
||||
try:
|
||||
_operacao = ContextoGlobalRedis.get_operacao()
|
||||
status_operacao = StatusOperacao(_operacao.get("status", StatusOperacao.NaoIniciado.value))
|
||||
finalizando = _operacao.get("finalizando", False)
|
||||
|
||||
if erro:
|
||||
mostrar_log("🟥 Movimento parado: erro ao definir comando direcional.")
|
||||
return 0.0
|
||||
|
||||
if parada_necessaria:
|
||||
mostrar_log("🟥 Movimento parado: direcional sem possibilidade de desvio.")
|
||||
return 0.0
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -23,13 +23,14 @@ class ProcessadorEmAndamento(ProcessadorBase):
|
|||
|
||||
def processar(self):
|
||||
try:
|
||||
angulo_sp, tipo_dir, simulacao, parada_necessaria = definir_comando_dir(self.pid_dir)
|
||||
velocidade_sp = definir_comando_mov(parada_necessaria, self.filtro_mov)
|
||||
angulo_sp, tipo_dir, simulacao, parada_necessaria, erro = definir_comando_dir(self.pid_dir)
|
||||
velocidade_sp = definir_comando_mov(parada_necessaria, erro, self.filtro_mov)
|
||||
return comando_controle(
|
||||
percentual_velocidade=velocidade_sp,
|
||||
angulo=angulo_sp,
|
||||
tipo_movimento=tipo_dir,
|
||||
simulacao=simulacao
|
||||
simulacao=simulacao,
|
||||
erro=erro
|
||||
)
|
||||
except Exception as e:
|
||||
print(f"Erro no processador EmAndamento: {e}")
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
|
|
@ -3,13 +3,14 @@ from shared.enums import ModoOperacao, StatusOperacao
|
|||
from manager_worker.config import mostrar_log
|
||||
from shared.contexto_global_redis import ContextoGlobalRedis, CtxKey
|
||||
|
||||
def comando_controle(percentual_velocidade: float, angulo: float, tipo_movimento: int, simulacao = []):
|
||||
def comando_controle(percentual_velocidade: float, angulo: float, tipo_movimento: int, simulacao = [], erro: bool = False):
|
||||
hb_atual = ContextoGlobalRedis.get_controle().get("heartbeat", 0)
|
||||
try:
|
||||
hb_atual = int(hb_atual) % 10
|
||||
except:
|
||||
hb_atual = 0
|
||||
heartbeat = (hb_atual + 1) % 10
|
||||
if erro == False:
|
||||
try:
|
||||
hb_atual = int(hb_atual) % 10
|
||||
except:
|
||||
hb_atual = 0
|
||||
hb_atual = (hb_atual + 1) % 10
|
||||
|
||||
ContextoGlobalRedis.atualizar_ctx_dict(
|
||||
CtxKey.DadosControle,
|
||||
|
|
@ -17,14 +18,14 @@ def comando_controle(percentual_velocidade: float, angulo: float, tipo_movimento
|
|||
tipo_movimento_direcional=tipo_movimento,
|
||||
velocidade_sp=percentual_velocidade,
|
||||
simulacao=simulacao,
|
||||
heartbeat=heartbeat
|
||||
heartbeat=hb_atual
|
||||
)
|
||||
return {
|
||||
"velocidade_sp": percentual_velocidade,
|
||||
"angulo_sp": angulo,
|
||||
"tipo_movimento_direcional": tipo_movimento,
|
||||
"simulacao": simulacao,
|
||||
"heartbeat": heartbeat
|
||||
"heartbeat": hb_atual
|
||||
}
|
||||
|
||||
def comando_parado():
|
||||
|
|
|
|||
Binary file not shown.
|
|
@ -111,4 +111,11 @@ class GPSHandler:
|
|||
except Exception as e:
|
||||
print(f"{time.time()} - [GPSHandler] ❌ Erro ao converter trajetoria para lat e long: {e}")
|
||||
return simulacao_latlon
|
||||
|
||||
def erro_angular(self, angulo_alvo, angulo_atual, angulo_caminho, peso_local=0.5):
|
||||
erro_com_ponto = (angulo_alvo - angulo_atual + np.pi) % (2 * np.pi) - np.pi
|
||||
erro_com_caminho = (angulo_caminho - angulo_atual + np.pi) % (2 * np.pi) - np.pi
|
||||
|
||||
erro_combinado = (1 - peso_local) * erro_com_caminho + peso_local * erro_com_ponto
|
||||
return abs(erro_combinado)
|
||||
|
||||
Binary file not shown.
|
|
@ -12,6 +12,7 @@ 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 visual_worker.processamento.costmap_fuser import CostmapFuser
|
||||
from shared.enums import StatusModulo, T_Code, CameraFrameType
|
||||
from shared.utils import analisar_linhas_por_profundidade, decode_image_base64, encode_image_base64, fazer_overlay
|
||||
from shared.gps_handler import GPSHandler
|
||||
|
|
@ -80,6 +81,7 @@ class CameraManager:
|
|||
self.solo_manager = AnaliseSoloManager()
|
||||
self.segmentacao_manager = SegmentacaoManager(self.camera.colormap_rgb, self.camera.classes)
|
||||
self.radar_manager = Radar2DManager()
|
||||
self.data_fuser = CostmapFuser(grid_shape=self.grid_ref_shape, K=3, M=2, fuse_method="q0.7", block_thr=0.7, central_cols=None, y_range_m=(0.5,5.0), near_is_bottom=True, fov_h_rad=np.radians(self.camera.parametros["fov_h"]))
|
||||
self.operante = True
|
||||
self._timestamp_analise = None
|
||||
|
||||
|
|
@ -106,14 +108,14 @@ class CameraManager:
|
|||
self.iniciando = False
|
||||
self.atualizar_saude_camera()
|
||||
|
||||
def _gerar_grid_referencia_geometrico(self, angulo_inclinacao_graus=26, altura_camera_m=0.74):
|
||||
grid_h = self.grid_ref_shape[0]
|
||||
def _gerar_grid_referencia_geometrico(self, angulo_inclinacao_graus=28.91, altura_camera_m=0.74):
|
||||
grid_w, grid_h = self.grid_ref_shape
|
||||
def dist_grid_calibrado(grid_h, i, fov, incl, altura):
|
||||
alpha_v = ((i + 0.5) / grid_h - 0.5) * np.radians(fov)
|
||||
gamma = np.radians(incl) + alpha_v
|
||||
d = (altura / np.tan(gamma)) * 1000.0
|
||||
return d
|
||||
d = np.array([dist_grid_calibrado(grid_h, i, -43.28, 28.91, 0.74) for i in range(grid_h)], dtype=np.float32)
|
||||
d = np.array([dist_grid_calibrado(grid_h, i, -43.28, angulo_inclinacao_graus, altura_camera_m) for i in range(grid_h)], dtype=np.float32)
|
||||
d = d[::-1] # ordena de baixo->cima como você queria
|
||||
return d # <-- ndarray, não list
|
||||
|
||||
|
|
@ -240,14 +242,6 @@ class CameraManager:
|
|||
continue
|
||||
elif self.operante and status == StatusModulo.OPERANTE and self.camera is not None:
|
||||
self._realizar_analises()
|
||||
#if self._ultimo_rgb_frame is not None and self._ultimo_rgb_frame.size > 0 and self._ultima_analise_matriz_custo is not None:
|
||||
# self._mostrar_debug_matriz_custo_fundida(self._ultimo_rgb_frame, self._ultima_analise_matriz_custo.get("matriz", [[]]), True)
|
||||
|
||||
#if self._ultima_analise_segmentacao is not None:
|
||||
# self.segmentacao_manager.display_segmentation_debug(self._ultimo_rgb_frame, 150)
|
||||
|
||||
#if self._ultimo_rgb_frame is not None and self._ultimo_rgb_frame.size > 0:
|
||||
# self._mostrar_grid_distancias_sobre_rgb(self._ultimo_rgb_frame, self.grid_ref)
|
||||
except Exception as e:
|
||||
self.mostrar_log(f"Erro no loop de analise continua: {e}")
|
||||
finally:
|
||||
|
|
@ -255,12 +249,13 @@ class CameraManager:
|
|||
novo_delay = max(0, (1.0 / freq) - latencia)
|
||||
#self.mostrar_log(
|
||||
# self._log_performance("Loop", { "freq": _freq, "fps": fps, "latencia": latencia }) +
|
||||
# self._log_performance("Radar", self._ultima_analise_radar) +
|
||||
# #self._log_performance("Radar", self._ultima_analise_radar) +
|
||||
# self._log_performance("Segmentacao", self._ultima_analise_segmentacao) +
|
||||
# self._log_performance("Matriz Confianca", self._ultima_analise_matriz_confianca) +
|
||||
# self._log_performance("Anomalias", self._ultima_analise_anomalias) +
|
||||
# self._log_performance("Solo", self._ultima_analise_solo) +
|
||||
# self._log_performance("Matriz Custo", self._ultima_analise_matriz_custo)
|
||||
# #self._log_performance("Anomalias", self._ultima_analise_anomalias) +
|
||||
# #self._log_performance("Solo", self._ultima_analise_solo) +
|
||||
# #self._log_performance("Matriz Custo", self._ultima_analise_matriz_custo) +
|
||||
# ""
|
||||
#)
|
||||
ultima_atualizacao = t0
|
||||
time.sleep(novo_delay)
|
||||
|
|
@ -307,8 +302,21 @@ class CameraManager:
|
|||
cv2.imshow("Grid de Referencia sobre RGB", img)
|
||||
cv2.waitKey(1)
|
||||
|
||||
|
||||
|
||||
def _realizar_analises(self):
|
||||
if self._depth_frame_necessario:
|
||||
depth_frame_np, depth_timestamp, depth_res = self.get_depth_frame()
|
||||
else:
|
||||
depth_frame_np = self._ultimo_depth_frame
|
||||
self._analise_segmentacao()
|
||||
parametros_camera = self.camera.parametros
|
||||
fov_h = parametros_camera["fov_h"]
|
||||
distancia_max_m = parametros_camera["distancia_maxima"] / 1000.0
|
||||
self._analise_matriz_confianca(depth_frame_np, distancia_max_m, fov_h)
|
||||
#key, vis = self.debug_show_costmap(rgb_frame=self._ultimo_rgb_frame, grid_dict=self._ultima_analise_matriz_confianca, grid_shape=self.grid_ref_shape, window_name="viz MPC", wait=1, text_mode="mini")
|
||||
key, vis = self.debug_show_visualworker(frame_bgr=self._ultimo_rgb_frame, grid=self._ultima_analise_matriz_confianca, wait=1, text_mode="full", draw_grid=True, draw_cells=True, draw_legend=True)
|
||||
|
||||
def _realizar_analises_async(self):
|
||||
executor = self._pool
|
||||
tarefas = []
|
||||
|
||||
|
|
@ -317,9 +325,12 @@ class CameraManager:
|
|||
distancia_max_m = parametros_camera["distancia_maxima"] / 1000.0
|
||||
percentual_solo = parametros_camera["percentual_altura_solo"] / 100.0
|
||||
|
||||
depth_frame_np, depth_timestamp, depth_res = self.get_depth_frame()
|
||||
if self._depth_frame_necessario:
|
||||
depth_frame_np, depth_timestamp, depth_res = self.get_depth_frame()
|
||||
else:
|
||||
depth_frame_np = self._ultimo_depth_frame
|
||||
|
||||
tarefas.append(executor.submit(self._analise_radar, depth_frame_np, fov_h, distancia_max_m))
|
||||
#tarefas.append(executor.submit(self._analise_radar, depth_frame_np, fov_h, distancia_max_m))
|
||||
|
||||
if True or not self._nova_segmentacao_disponivel:
|
||||
if not self._analisando_segmentacao:
|
||||
|
|
@ -330,7 +341,7 @@ class CameraManager:
|
|||
self._nova_segmentacao_disponivel = False
|
||||
tarefas.append(executor.submit(self._analise_matriz_confianca, depth_frame_np, distancia_max_m, fov_h))
|
||||
|
||||
if self._nova_grid_conf_disponivel:
|
||||
if False and self._nova_grid_conf_disponivel:
|
||||
limiar_conf: float = 0.4
|
||||
matriz_conf = self._ultima_analise_matriz_confianca["matriz"]
|
||||
if not self._analisando_anomalias:
|
||||
|
|
@ -348,21 +359,18 @@ class CameraManager:
|
|||
|
||||
|
||||
def _analise_segmentacao(self):
|
||||
if self._analisando_segmentacao:
|
||||
return
|
||||
if self._analisando_segmentacao: return
|
||||
self._analisando_segmentacao = True
|
||||
try:
|
||||
t0 = time.time()
|
||||
predictions, ts, res = self.get_segmentation_predictions()
|
||||
if ts == self._ts_segmentacao_anterior:
|
||||
return # já analisado
|
||||
if ts == self._ts_segmentacao_anterior: return
|
||||
self._ts_segmentacao_anterior = ts
|
||||
if predictions is not None:
|
||||
rgb_frame, _ts, res = self.get_rgb_frame()
|
||||
analise_segmentacao, log = self.segmentacao_manager.segmentar(predictions)
|
||||
t1 = time.time()
|
||||
if analise_segmentacao == None:
|
||||
self.mostrar_log(log)
|
||||
if analise_segmentacao == None: self.mostrar_log(log)
|
||||
analise_segmentacao["ultima_chamada"] = self._ultima_analise_segmentacao.get("ultima_chamada", t0)
|
||||
self._calcular_performance(t0, t1, analise_segmentacao)
|
||||
self._ultima_analise_segmentacao = analise_segmentacao
|
||||
|
|
@ -386,35 +394,31 @@ class CameraManager:
|
|||
#self.mostrar_log(f"Segmentacao concluida em {self._ultima_analise_segmentacao['latencia']:.4f} s, a {fps:.4f} FPS")
|
||||
|
||||
def _analise_matriz_confianca(self, depth_frame_np, dist_max, fov_h):
|
||||
if self._analisando_matriz_confianca:
|
||||
return
|
||||
if self._analisando_matriz_confianca: return
|
||||
self._analisando_matriz_confianca = True
|
||||
try:
|
||||
if depth_frame_np is None or depth_frame_np.size == 0:
|
||||
return
|
||||
if depth_frame_np is None or depth_frame_np.size == 0: return
|
||||
segmentacao = self._ultima_analise_segmentacao.get("classes")
|
||||
if segmentacao is None: return
|
||||
#segmentacao_vis = self._ultima_analise_segmentacao.get("mask_color", None)
|
||||
|
||||
if depth_frame_np is None or segmentacao is None:
|
||||
self.mostrar_log("❌ Depth frame ou segmentação inválidos para gerar matriz de confiança.")
|
||||
else:
|
||||
t0 = time.time()
|
||||
grid_conf = self._gerar_grid_confianca(depth_frame_np, segmentacao, dist_max)
|
||||
t1 = time.time()
|
||||
grid_conf["ultima_chamada"] = self._ultima_analise_matriz_confianca.get("ultima_chamada", t0)
|
||||
self._calcular_performance(t0, t1, grid_conf)
|
||||
self._ultima_analise_matriz_confianca = grid_conf
|
||||
matriz = self._ultima_analise_matriz_confianca["matriz"]
|
||||
self._ultima_analise_segmentacao["corredor_perfil"] = self.segmentacao_manager.calcular_perfil_corredor(matriz, fov_h)
|
||||
ContextoGlobalRedis.atualizar_ctx_dict(
|
||||
CtxKey.DadosVisualWorker,
|
||||
ts_analise=time.time(),
|
||||
matrizes__confianca=converter_valores_numpy(matriz),
|
||||
perfil_corredor__segmentacao=converter_valores_numpy(self._ultima_analise_segmentacao.get("corredor_perfil", []))
|
||||
)
|
||||
self._nova_grid_conf_disponivel = True
|
||||
#self._mostrar_debug_grid_confianca(self._ultimo_rgb_frame, grid_conf["matriz"], True, self._ultima_analise_segmentacao["mask_color"])
|
||||
t0 = time.time()
|
||||
#grid_conf = self._gerar_grid_confianca(depth_frame_np, segmentacao, dist_max)
|
||||
grid_conf = self._construir_grid_confianca(depth_frame_np, segmentacao, self.grid_ref, self.grid_ref_shape, self.camera.classes)
|
||||
#self.mostrar_log(grid_conf)
|
||||
t1 = time.time()
|
||||
grid_conf["ultima_chamada"] = self._ultima_analise_matriz_confianca.get("ultima_chamada", t0)
|
||||
self._calcular_performance(t0, t1, grid_conf)
|
||||
snapshot = self.data_fuser.update(grid_conf, ts=t1)
|
||||
self._ultima_analise_matriz_confianca = grid_conf
|
||||
#self._ultima_analise_segmentacao["corredor_perfil"] = self.segmentacao_manager.calcular_perfil_corredor(matriz, fov_h)
|
||||
ContextoGlobalRedis.atualizar_ctx_dict(
|
||||
CtxKey.DadosVisualWorker,
|
||||
ts_analise=time.time(),
|
||||
matrizes__confianca=snapshot
|
||||
#perfil_corredor__segmentacao=converter_valores_numpy(self._ultima_analise_segmentacao.get("corredor_perfil", []))
|
||||
)
|
||||
self._nova_grid_conf_disponivel = True
|
||||
#self._mostrar_debug_grid_confianca(self._ultimo_rgb_frame, grid_conf["matriz"], True, self._ultima_analise_segmentacao["mask_color"])
|
||||
except Exception as e:
|
||||
self.mostrar_log(f"❌ Erro na geracao da matriz de confianca: {e}")
|
||||
finally:
|
||||
|
|
@ -1239,3 +1243,397 @@ class CameraManager:
|
|||
cv2.waitKey(1)
|
||||
except Exception as e:
|
||||
self.mostrar_log(f"❌ Erro ao mostrar debug da matriz de custo: {e}")
|
||||
|
||||
|
||||
|
||||
def _construir_grid_confianca(
|
||||
self,
|
||||
depth_mm, # np.ndarray (H,W) em milímetros (0/NaN = inválido)
|
||||
seg_ids_512x288, # np.ndarray (288,512) com IDs de classe por pixel
|
||||
grid_ref, # np.ndarray (grid_h,) OU (grid_h, grid_w) em metros
|
||||
grid_shape=(15, 10), # (cols=15, rows=10)
|
||||
class_ids=None, # {'rua':X, 'cana':Y, 'obs':Z}
|
||||
valid_mm=(300, 10000), # faixa válida do depth (ajuste conforme tua OAK)
|
||||
range_m=(0.5, 5.0), # janela útil à frente (só p/ debug/checar)
|
||||
min_valid_frac=0.30, # % mínimo de pixels válidos p/ aceitar mediana
|
||||
conf_params=(0.30, 0.80),# t0,t1 p/ mapear %depth_valido -> conf_depth
|
||||
w=(0.6, 0.3, 0.1), # pesos do custo: classe, anom, (1-conf)
|
||||
anom_tau_min=0.22, # tolerância mínima de “aproximação” (m)
|
||||
anom_satur_m=0.50 # saturação da anomalia (m)
|
||||
):
|
||||
"""
|
||||
Retorna dict com arrays (grid_h, grid_w):
|
||||
pct_rua, pct_cana, pct_obs, z_med (m), z_ref (m),
|
||||
depth_valid_frac, conf, anom, custo, navegavel (0/1)
|
||||
"""
|
||||
if class_ids is None:
|
||||
# AJUSTE para os IDs reais do teu labelmap!
|
||||
class_ids = {'rua': 0, 'cana': 1, 'obs': 2}
|
||||
|
||||
grid_w, grid_h = grid_shape # (15, 10)
|
||||
H0, W0 = depth_mm.shape
|
||||
|
||||
# --- 1) Reduz o depth para 512x288 preservando rótulos (sem blur de escala) ---
|
||||
d_small = cv2.resize(depth_mm, (512, 288), interpolation=cv2.INTER_NEAREST).astype(np.float32)
|
||||
# marca inválidos
|
||||
d_small[(d_small < valid_mm[0]) | (d_small > valid_mm[1])] = np.nan
|
||||
|
||||
# --- 2) Bordas das células da grid 15x10 sobre a imagem 512x288 ---
|
||||
x_edges = np.linspace(0, 512, grid_w + 1, dtype=int) # 16 bordas
|
||||
y_edges = np.linspace(0, 288, grid_h + 1, dtype=int) # 11 bordas
|
||||
|
||||
# --- 3) Saídas (grid_h, grid_w) = (10, 15) ---
|
||||
pct_rua = np.zeros((grid_h, grid_w), np.float32)
|
||||
pct_cana = np.zeros((grid_h, grid_w), np.float32)
|
||||
pct_obs = np.zeros((grid_h, grid_w), np.float32)
|
||||
z_med = np.full((grid_h, grid_w), np.nan, np.float32)
|
||||
depth_valid_frac = np.zeros((grid_h, grid_w), np.float32)
|
||||
|
||||
# --- 4) Garante grid_ref 2D em metros ---
|
||||
if grid_ref.ndim == 1:
|
||||
# grid_ref é por LINHA (grid_h,)
|
||||
if grid_ref.shape[0] != grid_h:
|
||||
raise ValueError(f"grid_ref 1D deve ter len={grid_h}, veio {grid_ref.shape}")
|
||||
Z_ref = np.repeat(grid_ref[:, None], grid_w, axis=1)
|
||||
else:
|
||||
Z_ref = grid_ref
|
||||
if Z_ref.shape != (grid_h, grid_w):
|
||||
raise ValueError(f"grid_ref 2D deve ser {(grid_h, grid_w)}, veio {Z_ref.shape}")
|
||||
|
||||
# --- 5) Agregação por célula (150 células: tranquilo em tempo real) ---
|
||||
for j in range(grid_h):
|
||||
y0, y1 = y_edges[j], y_edges[j+1]
|
||||
seg_row = seg_ids_512x288[y0:y1, :] # (rows, 512)
|
||||
depth_row = d_small[y0:y1, :] # (rows, 512)
|
||||
for i in range(grid_w):
|
||||
x0, x1 = x_edges[i], x_edges[i+1]
|
||||
seg_block = seg_row[:, x0:x1]
|
||||
depth_block = depth_row[:, x0:x1]
|
||||
|
||||
n = seg_block.size
|
||||
if n == 0:
|
||||
continue
|
||||
|
||||
# % por classe
|
||||
n_rua = np.count_nonzero(seg_block == ClassesSegmentacao.RUA.value)
|
||||
n_cana = np.count_nonzero(seg_block == ClassesSegmentacao.CANA.value)
|
||||
n_obs = np.count_nonzero(seg_block == ClassesSegmentacao.OBSTACULO.value)
|
||||
pct_rua[j, i] = n_rua / n
|
||||
pct_cana[j, i] = n_cana / n
|
||||
pct_obs[j, i] = n_obs / n
|
||||
|
||||
# depth: mediana em metros + fração válida
|
||||
vals = depth_block[~np.isnan(depth_block)]
|
||||
valid = vals.size
|
||||
depth_valid_frac[j, i] = valid / n
|
||||
if valid >= max(int(min_valid_frac * n), 1):
|
||||
z_med[j, i] = np.nanmedian(vals) / 1000.0 # mm -> m
|
||||
|
||||
# --- 6) Confiança por célula ---
|
||||
t0, t1 = conf_params
|
||||
conf_seg = np.maximum.reduce([pct_rua, pct_cana, pct_obs])
|
||||
conf_dep = np.clip((depth_valid_frac - t0) / (t1 - t0), 0.0, 1.0)
|
||||
conf_cell = 0.6 * conf_seg + 0.4 * conf_dep
|
||||
|
||||
# --- 7) Anomalia (obstáculo = mais perto que o esperado) ---
|
||||
# ΔZ > 0 → medido está mais perto que a referência
|
||||
delta = Z_ref - z_med # m
|
||||
# z_med NaN → delta = 0 (sem evidência)
|
||||
delta = np.where(np.isnan(z_med), 0.0, np.maximum(delta, 0.0))
|
||||
anom_raw = np.clip(delta / anom_satur_m, 0.0, 1.0)
|
||||
# porta de tolerância mínima (tau) + confiança do depth
|
||||
anom = anom_raw * (delta > anom_tau_min).astype(np.float32) * conf_dep
|
||||
|
||||
# --- 8) Custo e navegabilidade ---
|
||||
nao_rua = 1.0 - pct_rua
|
||||
w1, w2, w3 = w
|
||||
custo = np.clip(w1 * nao_rua + w2 * anom + w3 * (1.0 - conf_cell), 0.0, 1.0)
|
||||
|
||||
navegavel = (pct_rua >= 0.55) & (anom < 0.4) & (conf_cell >= 0.5)
|
||||
|
||||
return {
|
||||
"pct_rua": pct_rua,
|
||||
"pct_cana": pct_cana,
|
||||
"pct_obs": pct_obs,
|
||||
"z_med": z_med, # metros
|
||||
"z_ref": Z_ref, # metros
|
||||
"depth_valid_frac": depth_valid_frac,
|
||||
"conf": conf_cell, # 0..1
|
||||
"anom": anom, # 0..1
|
||||
"custo": custo, # 0..1
|
||||
"navegavel": navegavel.astype(np.uint8) # 0/1
|
||||
}
|
||||
|
||||
def _put_text_centered(self, img, text, cx, cy, font_scale=0.4, thickness=1, color=(255,255,255), outline=True):
|
||||
font = cv2.FONT_HERSHEY_SIMPLEX
|
||||
(tw, th), baseline = cv2.getTextSize(text, font, font_scale, thickness)
|
||||
x = int(cx - tw/2)
|
||||
y = int(cy + th/2)
|
||||
if outline:
|
||||
cv2.putText(img, text, (x+1, y+1), font, font_scale, (0,0,0), thickness+2, cv2.LINE_AA)
|
||||
cv2.putText(img, text, (x, y), font, font_scale, color, thickness, cv2.LINE_AA)
|
||||
|
||||
def make_costmap_overlay(
|
||||
self,
|
||||
rgb_frame, # HxWx3 (BGR se veio do OpenCV)
|
||||
grid_dict, # dict com arrays (grid_h, grid_w): "custo","conf","anom","navegavel","z_med","z_ref","pct_rua","pct_cana","pct_obs"
|
||||
grid_shape=(15,10), # (cols, rows)
|
||||
alpha=0.45, # transparência do heatmap
|
||||
draw_borders=True,
|
||||
draw_cells=True, # desenha retângulos por célula
|
||||
draw_text_mode="mini", # "off" | "mini" | "full"
|
||||
draw_legend=True
|
||||
):
|
||||
#rgb_frame = cv2.resize(rgb_frame, (1080, 720), interpolation=cv2.INTER_AREA)
|
||||
H, W = rgb_frame.shape[:2]
|
||||
grid_w, grid_h = grid_shape
|
||||
custo = grid_dict["custo"].astype(np.float32) # (grid_h, grid_w) [0..1]
|
||||
conf = grid_dict.get("conf", None)
|
||||
anom = grid_dict.get("anom", None)
|
||||
nav = grid_dict.get("navegavel", None)
|
||||
z_med = grid_dict.get("z_med", None)
|
||||
z_ref = grid_dict.get("z_ref", None)
|
||||
p_rua = grid_dict.get("pct_rua", None)
|
||||
p_can = grid_dict.get("pct_cana", None)
|
||||
p_obs = grid_dict.get("pct_obs", None)
|
||||
|
||||
# 1) heatmap (upscale por vizinho p/ manter blocos nítidos)
|
||||
cost_u8 = np.clip(custo * 255.0, 0, 255).astype(np.uint8)
|
||||
cost_up = cv2.resize(cost_u8, (W, H), interpolation=cv2.INTER_NEAREST)
|
||||
try:
|
||||
cmap = cv2.COLORMAP_TURBO
|
||||
except AttributeError:
|
||||
cmap = cv2.COLORMAP_JET
|
||||
heat = cv2.applyColorMap(cost_up, cmap)
|
||||
|
||||
# 2) overlay
|
||||
out = cv2.addWeighted(heat, alpha, rgb_frame, 1.0 - alpha, 0)
|
||||
|
||||
# 3) grid edges
|
||||
x_edges = (np.linspace(0, W, grid_w+1)).astype(int)
|
||||
y_edges = (np.linspace(0, H, grid_h+1)).astype(int)
|
||||
|
||||
if draw_borders:
|
||||
for x in x_edges:
|
||||
cv2.line(out, (x, 0), (x, H-1), (50,50,50), 1, cv2.LINE_AA)
|
||||
for y in y_edges:
|
||||
cv2.line(out, (0, y), (W-1, y), (50,50,50), 1, cv2.LINE_AA)
|
||||
|
||||
# 4) por-célula (bordas coloridas + textos)
|
||||
if draw_cells:
|
||||
for j in range(grid_h):
|
||||
y0, y1 = y_edges[j], y_edges[j+1]
|
||||
cy = (y0 + y1) // 2
|
||||
for i in range(grid_w):
|
||||
x0, x1 = x_edges[i], x_edges[i+1]
|
||||
cx = (x0 + x1) // 2
|
||||
|
||||
c = float(custo[j, i])
|
||||
# bordinha: navegável verde, bloqueado vermelho, neutro cinza
|
||||
if nav is not None:
|
||||
if nav[j, i]:
|
||||
color = (60, 200, 60)
|
||||
else:
|
||||
color = (20, 20, 220) if c > 0.7 else (90, 90, 90)
|
||||
else:
|
||||
color = (90, 90, 90)
|
||||
cv2.rectangle(out, (x0, y0), (x1-1, y1-1), color, 1)
|
||||
|
||||
if draw_text_mode != "off":
|
||||
# texto curtinho (mini) ou completo (full)
|
||||
if draw_text_mode == "mini":
|
||||
txt = f"C{int(round(c*100))}"
|
||||
if conf is not None:
|
||||
txt += f" cf{int(round(conf[j,i]*100))}"
|
||||
if anom is not None:
|
||||
txt += f" A{int(round(anom[j,i]*100))}"
|
||||
self._put_text_centered(out, txt, cx, cy, font_scale=0.32, thickness=1, color=(255,255,255), outline=True)
|
||||
elif draw_text_mode == "full":
|
||||
line1 = f"C{int(round(c*100))}"
|
||||
if conf is not None: line1 += f" cf{conf[j,i]:.2f}"
|
||||
if anom is not None: line1 += f" A{int(round(anom[j,i]*100))}"
|
||||
self._put_text_centered(out, line1, cx, cy-8, font_scale=0.38, thickness=1,
|
||||
color=(255,255,255), outline=True)
|
||||
line2 = ""
|
||||
if z_med is not None and z_ref is not None and not np.isnan(z_med[j,i]):
|
||||
line2 = f"Z{z_med[j,i]:.1f}/{z_ref[j,i]:.1f}m"
|
||||
elif z_ref is not None:
|
||||
line2 = f"Zref {z_ref[j,i]:.1f}m"
|
||||
if line2:
|
||||
self._put_text_centered(out, line2, cx, cy+8, font_scale=0.36, thickness=1,
|
||||
color=(255,255,255), outline=True)
|
||||
if p_rua is not None and p_can is not None and p_obs is not None:
|
||||
line3 = f"R{int(p_rua[j,i]*100)} C{int(p_can[j,i]*100)} O{int(p_obs[j,i]*100)}"
|
||||
self._put_text_centered(out, line3, cx, cy+22, font_scale=0.34, thickness=1,
|
||||
color=(230,230,230), outline=True)
|
||||
|
||||
# 5) legenda opcional
|
||||
if draw_legend:
|
||||
pad = 8
|
||||
x0, y0 = pad, pad
|
||||
cv2.rectangle(out, (x0-4, y0-4), (x0+160, y0+72), (0,0,0), -1)
|
||||
cv2.putText(out, "Legenda:", (x0, y0+12), cv2.FONT_HERSHEY_SIMPLEX, 0.45, (255,255,255), 1, cv2.LINE_AA)
|
||||
cv2.putText(out, "C= custo (0..1)", (x0, y0+28), cv2.FONT_HERSHEY_SIMPLEX, 0.42, (200,200,200), 1, cv2.LINE_AA)
|
||||
cv2.putText(out, "cf= confianca", (x0, y0+44), cv2.FONT_HERSHEY_SIMPLEX, 0.42, (200,200,200), 1, cv2.LINE_AA)
|
||||
cv2.putText(out, "A= anomalia", (x0, y0+60), cv2.FONT_HERSHEY_SIMPLEX, 0.42, (200,200,200), 1, cv2.LINE_AA)
|
||||
|
||||
return out
|
||||
|
||||
def debug_show_costmap(self, rgb_frame, grid_dict, grid_shape=(15,10), window_name="debug_costmap", wait=1, text_mode="mini"):
|
||||
vis = self.make_costmap_overlay(rgb_frame, grid_dict, grid_shape=grid_shape, alpha=0.45, draw_borders=True, draw_cells=True, draw_text_mode=text_mode, draw_legend=True)
|
||||
cv2.namedWindow(window_name, cv2.WINDOW_NORMAL)
|
||||
cv2.imshow(window_name, vis)
|
||||
key = cv2.waitKey(wait) & 0xFF
|
||||
return key, vis
|
||||
|
||||
|
||||
|
||||
def _normalize_cost_robusto(self, C: np.ndarray):
|
||||
C = np.asarray(C, dtype=np.float32)
|
||||
p5, p95 = np.percentile(C, [5, 95])
|
||||
cmin, cmax = float(p5), float(max(p95, p5 + 1e-6))
|
||||
N = (C - cmin) / (cmax - cmin + 1e-9)
|
||||
return np.clip(N, 0.0, 1.0), cmin, cmax
|
||||
|
||||
def make_visualworker_overlay(
|
||||
self,
|
||||
frame_bgr, # frame da câmera (BGR, HxWx3)
|
||||
grid, # dict: {"Custo": Hc x Wc, "Navegavel": Hc x Wc, ...}
|
||||
alpha_cost=0.45, # transparência do heatmap
|
||||
draw_grid=True,
|
||||
draw_cells=True,
|
||||
text_mode="mini", # "off" | "mini" | "full"
|
||||
draw_legend=True,
|
||||
nav_overlay_alpha=0.35 # opacidade do overlay vermelho p/ NAV=False
|
||||
):
|
||||
"""
|
||||
Sobrepõe Custo (+ Navegável) do Visual Worker no frame RGB, em tempo real.
|
||||
Retorna uma imagem BGR com o overlay.
|
||||
"""
|
||||
C = np.asarray(grid.get("custo", None), dtype=np.float32)
|
||||
if C is None or C.size == 0:
|
||||
return frame_bgr.copy()
|
||||
|
||||
NAV = grid.get("navegavel", None)
|
||||
Hc, Wc = C.shape
|
||||
Hf, Wf = frame_bgr.shape[:2]
|
||||
|
||||
# 1) heatmap de custo (robusto a outliers)
|
||||
N, cmin, cmax = self._normalize_cost_robusto(C) # [0..1]
|
||||
u8 = (N * 255.0).astype(np.uint8)
|
||||
up = cv2.resize(u8, (Wf, Hf), interpolation=cv2.INTER_NEAREST)
|
||||
try:
|
||||
cmap = cv2.COLORMAP_TURBO
|
||||
except AttributeError:
|
||||
cmap = cv2.COLORMAP_JET
|
||||
heat = cv2.applyColorMap(up, cmap)
|
||||
out = cv2.addWeighted(heat, alpha_cost, frame_bgr, 1.0 - alpha_cost, 0)
|
||||
|
||||
# 2) overlay de não-navegável (vermelho)
|
||||
if NAV is not None:
|
||||
bad = (~np.asarray(NAV, dtype=bool)).astype(np.uint8) * 255 # 255 = bloqueado
|
||||
bad_up = cv2.resize(bad, (Wf, Hf), interpolation=cv2.INTER_NEAREST)
|
||||
mask = bad_up.astype(bool)
|
||||
red = out.copy()
|
||||
red[mask] = (0, 0, 255)
|
||||
out = cv2.addWeighted(red, nav_overlay_alpha, out, 1.0 - nav_overlay_alpha, 0)
|
||||
|
||||
# 3) grade
|
||||
x_edges = np.linspace(0, Wf, Wc + 1).astype(int)
|
||||
y_edges = np.linspace(0, Hf, Hc + 1).astype(int)
|
||||
if draw_grid:
|
||||
for x in x_edges:
|
||||
cv2.line(out, (x, 0), (x, Hf - 1), (60, 60, 60), 1, cv2.LINE_AA)
|
||||
for y in y_edges:
|
||||
cv2.line(out, (0, y), (Wf - 1, y), (60, 60, 60), 1, cv2.LINE_AA)
|
||||
|
||||
# 4) textos por célula (mini/full)
|
||||
if draw_cells and text_mode != "off":
|
||||
conf = grid.get("conf", None)
|
||||
anom = grid.get("anom", None)
|
||||
z_med = grid.get("z_med", None)
|
||||
z_ref = grid.get("z_ref", None)
|
||||
p_rua = grid.get("pct_rua", None)
|
||||
p_can = grid.get("pct_cana", None)
|
||||
p_obs = grid.get("pct_obs", None)
|
||||
|
||||
for j in range(Hc):
|
||||
y0, y1 = y_edges[j], y_edges[j + 1]
|
||||
cy = (y0 + y1) // 2
|
||||
for i in range(Wc):
|
||||
x0, x1 = x_edges[i], x_edges[i + 1]
|
||||
cx = (x0 + x1) // 2
|
||||
|
||||
# moldura fina, cor conforme NAV
|
||||
if NAV is not None:
|
||||
if NAV[j, i]:
|
||||
color = (60, 200, 60)
|
||||
else:
|
||||
color = (20, 20, 220) if C[j, i] > (cmin + 0.7*(cmax-cmin)) else (90, 90, 90)
|
||||
else:
|
||||
color = (90, 90, 90)
|
||||
cv2.rectangle(out, (x0, y0), (x1 - 1, y1 - 1), color, 1, cv2.LINE_AA)
|
||||
|
||||
# textos
|
||||
if text_mode == "mini":
|
||||
# custo em 0..100 + check NAV
|
||||
c100 = int(round(N[j, i] * 100))
|
||||
txt = f"C{c100}"
|
||||
if NAV is not None:
|
||||
txt += " N" if NAV[j, i] else " B"
|
||||
self._put_text_centered(out, txt, cx, cy, font_scale=0.32, thickness=1,
|
||||
color=(255, 255, 255), outline=True)
|
||||
elif text_mode == "full":
|
||||
l1 = f"C{N[j,i]*100:.0f}"
|
||||
if conf is not None: l1 += f" cf{conf[j,i]:.2f}"
|
||||
if anom is not None: l1 += f" A{anom[j,i]*100:.0f}"
|
||||
self._put_text_centered(out, l1, cx, cy - 8, font_scale=0.38, thickness=1,
|
||||
color=(255, 255, 255), outline=True)
|
||||
l2 = ""
|
||||
if z_med is not None and z_ref is not None and not np.isnan(z_med[j,i]):
|
||||
l2 = f"Z{z_med[j,i]:.1f}/{z_ref[j,i]:.1f}m"
|
||||
elif z_ref is not None:
|
||||
l2 = f"Zref {z_ref[j,i]:.1f}m"
|
||||
if l2:
|
||||
self._put_text_centered(out, l2, cx, cy + 8, font_scale=0.36, thickness=1,
|
||||
color=(255, 255, 255), outline=True)
|
||||
if p_rua is not None and p_can is not None and p_obs is not None:
|
||||
l3 = f"R{int(p_rua[j,i]*100)} C{int(p_can[j,i]*100)} O{int(p_obs[j,i]*100)}"
|
||||
self._put_text_centered(out, l3, cx, cy + 22, font_scale=0.34, thickness=1,
|
||||
color=(230, 230, 230), outline=True)
|
||||
|
||||
# 5) DistânciasRef por linha (se disponível)
|
||||
dist_ref = grid.get("DistanciasRef", None)
|
||||
if dist_ref is not None and len(dist_ref) == Hc:
|
||||
for j in range(Hc):
|
||||
y = (y_edges[j] + y_edges[j + 1]) // 2
|
||||
s = f"{float(dist_ref[j]):.2f}m"
|
||||
cv2.putText(out, s, (5, y + 10), cv2.FONT_HERSHEY_SIMPLEX, 0.42, (0, 0, 0), 2, cv2.LINE_AA)
|
||||
cv2.putText(out, s, (5, y + 10), cv2.FONT_HERSHEY_SIMPLEX, 0.42, (255, 255, 255), 1, cv2.LINE_AA)
|
||||
|
||||
# 6) legenda
|
||||
if draw_legend:
|
||||
pad = 8
|
||||
x0, y0 = pad, pad
|
||||
cv2.rectangle(out, (x0 - 4, y0 - 4), (x0 + 190, y0 + 74), (0, 0, 0), -1)
|
||||
cv2.putText(out, "Legenda", (x0, y0 + 14), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255,255,255), 1, cv2.LINE_AA)
|
||||
cv2.putText(out, "C: custo (0..1) norm.", (x0, y0 + 32), cv2.FONT_HERSHEY_SIMPLEX, 0.42, (200,200,200), 1, cv2.LINE_AA)
|
||||
cv2.putText(out, "N navegavel / B bloqueado", (x0, y0 + 50), cv2.FONT_HERSHEY_SIMPLEX, 0.42, (200,200,200), 1, cv2.LINE_AA)
|
||||
cv2.putText(out, "turbo=baixo jet=alto", (x0, y0 + 68), cv2.FONT_HERSHEY_SIMPLEX, 0.38, (160,160,160), 1, cv2.LINE_AA)
|
||||
|
||||
return out
|
||||
|
||||
def debug_show_visualworker(
|
||||
self,
|
||||
frame_bgr,
|
||||
grid,
|
||||
window_name="VW Cost/NAV",
|
||||
wait=1,
|
||||
**overlay_kwargs
|
||||
):
|
||||
vis = self.make_visualworker_overlay(frame_bgr, grid, **overlay_kwargs)
|
||||
cv2.namedWindow(window_name, cv2.WINDOW_NORMAL)
|
||||
cv2.imshow(window_name, vis)
|
||||
key = cv2.waitKey(wait) & 0xFF
|
||||
return key, vis
|
||||
|
||||
|
|
|
|||
|
|
@ -1,9 +1,9 @@
|
|||
{
|
||||
"camera": "oak-1",
|
||||
"camera": "oak-d",
|
||||
"modelo": "fast_scnn",
|
||||
"model_name": "ervas_medium_new",
|
||||
"main_class_name": "erva",
|
||||
"use_main_class": true,
|
||||
"model_name": "ruas_new",
|
||||
"main_class_name": "cana",
|
||||
"use_main_class": false,
|
||||
"resolucao": [512, 288],
|
||||
"roi_inicio": 0.0,
|
||||
"roi_tamanho": 1.0
|
||||
|
|
|
|||
Loading…
Reference in New Issue