ajustes no IMU e deteccao de obstaculos
This commit is contained in:
Diego Freitas
parent
319c07af8c
commit
c86ab0b4c6
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -104,6 +104,8 @@
|
|||
this.tkbVelocidade = new System.Windows.Forms.TrackBar();
|
||||
this.tabMapa = new System.Windows.Forms.TabPage();
|
||||
this.picFechar = new System.Windows.Forms.PictureBox();
|
||||
this.txtInclinacao = new System.Windows.Forms.TextBox();
|
||||
this.lblInclinacao = new System.Windows.Forms.Label();
|
||||
this.pnlMapa.SuspendLayout();
|
||||
this.pnlSensores.SuspendLayout();
|
||||
this.statusStrip1.SuspendLayout();
|
||||
|
|
@ -412,6 +414,8 @@
|
|||
// pnlSensores
|
||||
//
|
||||
this.pnlSensores.AutoScroll = true;
|
||||
this.pnlSensores.Controls.Add(this.txtInclinacao);
|
||||
this.pnlSensores.Controls.Add(this.lblInclinacao);
|
||||
this.pnlSensores.Controls.Add(this.txtDT);
|
||||
this.pnlSensores.Controls.Add(this.lblDT);
|
||||
this.pnlSensores.Controls.Add(this.txtET);
|
||||
|
|
@ -472,7 +476,7 @@
|
|||
// txtDT
|
||||
//
|
||||
this.txtDT.Font = new System.Drawing.Font("Microsoft Sans Serif", 12.25F);
|
||||
this.txtDT.Location = new System.Drawing.Point(195, 819);
|
||||
this.txtDT.Location = new System.Drawing.Point(195, 847);
|
||||
this.txtDT.Name = "txtDT";
|
||||
this.txtDT.ReadOnly = true;
|
||||
this.txtDT.Size = new System.Drawing.Size(89, 26);
|
||||
|
|
@ -483,7 +487,7 @@
|
|||
// lblDT
|
||||
//
|
||||
this.lblDT.AutoSize = true;
|
||||
this.lblDT.Location = new System.Drawing.Point(160, 822);
|
||||
this.lblDT.Location = new System.Drawing.Point(160, 850);
|
||||
this.lblDT.Name = "lblDT";
|
||||
this.lblDT.Size = new System.Drawing.Size(30, 20);
|
||||
this.lblDT.TabIndex = 50;
|
||||
|
|
@ -492,7 +496,7 @@
|
|||
// txtET
|
||||
//
|
||||
this.txtET.Font = new System.Drawing.Font("Microsoft Sans Serif", 12.25F);
|
||||
this.txtET.Location = new System.Drawing.Point(41, 822);
|
||||
this.txtET.Location = new System.Drawing.Point(41, 850);
|
||||
this.txtET.Name = "txtET";
|
||||
this.txtET.ReadOnly = true;
|
||||
this.txtET.Size = new System.Drawing.Size(89, 26);
|
||||
|
|
@ -503,7 +507,7 @@
|
|||
// lblET
|
||||
//
|
||||
this.lblET.AutoSize = true;
|
||||
this.lblET.Location = new System.Drawing.Point(6, 825);
|
||||
this.lblET.Location = new System.Drawing.Point(6, 853);
|
||||
this.lblET.Name = "lblET";
|
||||
this.lblET.Size = new System.Drawing.Size(29, 20);
|
||||
this.lblET.TabIndex = 48;
|
||||
|
|
@ -512,7 +516,7 @@
|
|||
// txtDF
|
||||
//
|
||||
this.txtDF.Font = new System.Drawing.Font("Microsoft Sans Serif", 12.25F);
|
||||
this.txtDF.Location = new System.Drawing.Point(195, 787);
|
||||
this.txtDF.Location = new System.Drawing.Point(195, 815);
|
||||
this.txtDF.Name = "txtDF";
|
||||
this.txtDF.ReadOnly = true;
|
||||
this.txtDF.Size = new System.Drawing.Size(89, 26);
|
||||
|
|
@ -523,7 +527,7 @@
|
|||
// lblDF
|
||||
//
|
||||
this.lblDF.AutoSize = true;
|
||||
this.lblDF.Location = new System.Drawing.Point(160, 790);
|
||||
this.lblDF.Location = new System.Drawing.Point(160, 818);
|
||||
this.lblDF.Name = "lblDF";
|
||||
this.lblDF.Size = new System.Drawing.Size(31, 20);
|
||||
this.lblDF.TabIndex = 46;
|
||||
|
|
@ -532,7 +536,7 @@
|
|||
// txtEF
|
||||
//
|
||||
this.txtEF.Font = new System.Drawing.Font("Microsoft Sans Serif", 12.25F);
|
||||
this.txtEF.Location = new System.Drawing.Point(41, 790);
|
||||
this.txtEF.Location = new System.Drawing.Point(41, 818);
|
||||
this.txtEF.Name = "txtEF";
|
||||
this.txtEF.ReadOnly = true;
|
||||
this.txtEF.Size = new System.Drawing.Size(89, 26);
|
||||
|
|
@ -543,7 +547,7 @@
|
|||
// lblEF
|
||||
//
|
||||
this.lblEF.AutoSize = true;
|
||||
this.lblEF.Location = new System.Drawing.Point(6, 793);
|
||||
this.lblEF.Location = new System.Drawing.Point(6, 821);
|
||||
this.lblEF.Name = "lblEF";
|
||||
this.lblEF.Size = new System.Drawing.Size(30, 20);
|
||||
this.lblEF.TabIndex = 44;
|
||||
|
|
@ -879,6 +883,25 @@
|
|||
this.picFechar.TabIndex = 1;
|
||||
this.picFechar.TabStop = false;
|
||||
//
|
||||
// txtInclinacao
|
||||
//
|
||||
this.txtInclinacao.Font = new System.Drawing.Font("Microsoft Sans Serif", 12.25F);
|
||||
this.txtInclinacao.Location = new System.Drawing.Point(164, 768);
|
||||
this.txtInclinacao.Name = "txtInclinacao";
|
||||
this.txtInclinacao.ReadOnly = true;
|
||||
this.txtInclinacao.Size = new System.Drawing.Size(120, 26);
|
||||
this.txtInclinacao.TabIndex = 53;
|
||||
this.txtInclinacao.TextAlign = System.Windows.Forms.HorizontalAlignment.Center;
|
||||
//
|
||||
// lblInclinacao
|
||||
//
|
||||
this.lblInclinacao.AutoSize = true;
|
||||
this.lblInclinacao.Location = new System.Drawing.Point(6, 771);
|
||||
this.lblInclinacao.Name = "lblInclinacao";
|
||||
this.lblInclinacao.Size = new System.Drawing.Size(126, 20);
|
||||
this.lblInclinacao.TabIndex = 52;
|
||||
this.lblInclinacao.Text = "Inclinação (L / F)";
|
||||
//
|
||||
// frmDialogoMapa
|
||||
//
|
||||
this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F);
|
||||
|
|
@ -996,5 +1019,7 @@
|
|||
private System.Windows.Forms.Label lblDF;
|
||||
private System.Windows.Forms.TextBox txtEF;
|
||||
private System.Windows.Forms.Label lblEF;
|
||||
private System.Windows.Forms.TextBox txtInclinacao;
|
||||
private System.Windows.Forms.Label lblInclinacao;
|
||||
}
|
||||
}
|
||||
|
|
@ -207,6 +207,7 @@ namespace AgroBase.Forms.IHM
|
|||
txtDentroCorredor.Text = _Sensoriamento.Trajetoria.CorredorAtual.Dentro ? "Sim" : "Não";
|
||||
txtMargemCorredor.Text = _Sensoriamento.Trajetoria.NaMargemDoCorredor ? "Sim" : "Não";
|
||||
txtStatusCarro.Text = _Sensoriamento.Trajetoria.StatusCarro.ToString();
|
||||
txtInclinacao.Text = _Sensoriamento.IMU.InclinacaoLateral.ToString("0.00") + "/" + _Sensoriamento.IMU.InclinacaoFrontal.ToString("0.00");
|
||||
|
||||
var ModET = Variaveis.OperacaoEmAndamento.DispMvd.Dados.Modulos.FirstOrDefault(x => x.Modulo_ID == "ET");
|
||||
txtET.Text = (ModET?.MovMotor?.Corrente_Motor ?? 0).ToString("0.00");
|
||||
|
|
|
|||
|
|
@ -45,7 +45,7 @@ namespace AgroBase.Forms
|
|||
{
|
||||
PopularDadosGrid();
|
||||
|
||||
tmrLeitura = new AsyncTaskTimerModel("tmrLeitura", tmrLeitura_Tick, 500, this);
|
||||
tmrLeitura = new AsyncTaskTimerModel("tmrLeitura", tmrLeitura_Tick, 200, this);
|
||||
tmrLeitura.Start();
|
||||
|
||||
cmbAlgoritmo.Items.Clear();
|
||||
|
|
@ -60,9 +60,11 @@ namespace AgroBase.Forms
|
|||
private async Task tmrLeitura_Tick()
|
||||
{
|
||||
var Imu = Variaveis.OperacaoEmAndamento.Sensoriamento.IMU;
|
||||
AnguloX = Imu.InclinacaoLateral;
|
||||
AnguloY = Imu.InclinacaoFrontal;
|
||||
AnguloZ = Imu.Rotacao;
|
||||
AnguloX = (Imu.InclinacaoFrontal + 180) % 360;
|
||||
if (AnguloX > 180) AnguloX -= 360;
|
||||
AnguloY = (Imu.InclinacaoLateral + 180) % 360;
|
||||
if (AnguloY > 180) AnguloY -= 360;
|
||||
AnguloZ = -Imu.Rotacao;
|
||||
OrientacaoMagnetica = Imu.RotacaoCorrigida;
|
||||
|
||||
AtualizarDadosTela();
|
||||
|
|
|
|||
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": "13399758321059072"
|
||||
"next_scheduled_calculation_time": "13399758321059135"
|
||||
}
|
||||
|
|
|
|||
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/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
|
||||
2025/08/14-08:05:35.848 19d8 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Local Storage\leveldb/MANIFEST-000001
|
||||
2025/08/14-08:05:35.853 19d8 Recovering log #3
|
||||
2025/08/14-08:05:35.857 19d8 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/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
|
||||
2025/08/13-17:43:50.172 2850 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Local Storage\leveldb/MANIFEST-000001
|
||||
2025/08/13-17:43:50.179 2850 Recovering log #3
|
||||
2025/08/13-17:43:50.183 2850 Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Local Storage\leveldb/000003.log
|
||||
|
|
|
|||
Binary file not shown.
|
|
@ -1 +1 @@
|
|||
{"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"}}}
|
||||
{"net":{"http_server_properties":{"servers":[{"alternative_service":[{"advertised_alpns":["h3"],"expiration":"13399729543529307","port":443,"protocol_str":"quic"}],"anonymization":["DAAAAAcAAABmaWxlOi8vAA==",false,0],"network_stats":{"srtt":18335},"server":"https://tile.openstreetmap.org","supports_spdy":true}],"supports_quic":{"address":"2804:d78:627:be00:409f:ed6:2a0d:4b81","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":1786639441.537365,"host":"bWGAftl61rqoc0YzqPncsLvQQh/iC2Bdp3ejUeGC83w=","mode":"force-https","sts_include_subdomains":true,"sts_observed":1755103441.537367}],"version":2}
|
||||
{"sts":[{"expiry":1786705536.321524,"host":"bWGAftl61rqoc0YzqPncsLvQQh/iC2Bdp3ejUeGC83w=","mode":"force-https","sts_include_subdomains":true,"sts_observed":1755169536.321528}],"version":2}
|
||||
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
|
|
@ -1,3 +1,3 @@
|
|||
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
|
||||
2025/08/14-08:08:01.412 19d8 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Session Storage/MANIFEST-000001
|
||||
2025/08/14-08:08:01.413 19d8 Recovering log #3
|
||||
2025/08/14-08:08:01.418 19d8 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/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
|
||||
2025/08/13-17:45:26.419 2850 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Session Storage/MANIFEST-000001
|
||||
2025/08/13-17:45:26.420 2850 Recovering log #3
|
||||
2025/08/13-17:45:26.423 2850 Reusing old log C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Session Storage/000003.log
|
||||
|
|
|
|||
Binary file not shown.
|
|
@ -1,3 +1,3 @@
|
|||
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
|
||||
2025/08/14-08:05:35.765 9414 Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Site Characteristics Database/MANIFEST-000001
|
||||
2025/08/14-08:05:35.766 9414 Recovering log #7
|
||||
2025/08/14-08:05:35.767 9414 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/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
|
||||
2025/08/13-17:43:50.080 675c Reusing MANIFEST C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\x64\Debug\AgroBase.exe.WebView2\EBWebView\Default\Site Characteristics Database/MANIFEST-000001
|
||||
2025/08/13-17:43:50.082 675c Recovering log #7
|
||||
2025/08/13-17:43:50.082 675c 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.
Binary file not shown.
Binary file not shown.
|
|
@ -1 +1 @@
|
|||
138.0.3351.121
|
||||
139.0.3405.86
|
||||
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
|
|
@ -6,7 +6,7 @@
|
|||
"Extensao": ".blob",
|
||||
"Versao": "2_1",
|
||||
"TipoArquivo": 1,
|
||||
"ArquivoDownload": "weed_detector_model-2_1.blob"
|
||||
"ArquivoDownload": "models/weed_detector_model-2_1.blob"
|
||||
},
|
||||
{
|
||||
"id": 2,
|
||||
|
|
@ -15,7 +15,7 @@
|
|||
"Extensao": ".txt",
|
||||
"Versao": "2_1",
|
||||
"TipoArquivo": 1,
|
||||
"ArquivoDownload": "weed_detector_model-2_1.txt"
|
||||
"ArquivoDownload": "models/weed_detector_labelmap-2_1.txt"
|
||||
},
|
||||
{
|
||||
"id": 3,
|
||||
|
|
@ -24,7 +24,7 @@
|
|||
"Extensao": ".blob",
|
||||
"Versao": "1_1",
|
||||
"TipoArquivo": 0,
|
||||
"ArquivoDownload": "street_detector_model-1_1.blob",
|
||||
"ArquivoDownload": "models/street_detector_model-1_1.blob",
|
||||
},
|
||||
{
|
||||
"id": 4,
|
||||
|
|
@ -33,7 +33,7 @@
|
|||
"Extensao": ".txt",
|
||||
"Versao": "1_1",
|
||||
"TipoArquivo": 0,
|
||||
"ArquivoDownload": "street_detector_labelmap-1_1.txt"
|
||||
"ArquivoDownload": "models/street_detector_labelmap-1_1.txt"
|
||||
},
|
||||
{
|
||||
"id": 5,
|
||||
|
|
@ -42,7 +42,7 @@
|
|||
"Extensao": ".par",
|
||||
"Versao": "1_1",
|
||||
"TipoArquivo": 2,
|
||||
"ArquivoDownload": "parametersAtu-1_1.par",
|
||||
"ArquivoDownload": "parameters/parametersAtu-1_1.par",
|
||||
},
|
||||
{
|
||||
"id": 6,
|
||||
|
|
@ -51,7 +51,7 @@
|
|||
"Extensao": ".par",
|
||||
"Versao": "2_0",
|
||||
"TipoArquivo": 3,
|
||||
"ArquivoDownload": "parametersMvd-2_0.par"
|
||||
"ArquivoDownload": "parameters/parametersMvd-2_0.par"
|
||||
},
|
||||
{
|
||||
"id": 7,
|
||||
|
|
@ -60,7 +60,7 @@
|
|||
"Extensao": ".par",
|
||||
"Versao": "1_3",
|
||||
"TipoArquivo": 4,
|
||||
"ArquivoDownload": "parametersSen-1_3.par"
|
||||
"ArquivoDownload": "parameters/parametersSen-1_3.par"
|
||||
},
|
||||
{
|
||||
"id": 8,
|
||||
|
|
@ -69,7 +69,7 @@
|
|||
"Extensao": ".pin",
|
||||
"Versao": "1_0",
|
||||
"TipoArquivo": 6,
|
||||
"ArquivoDownload": "pinoutAtu-1_0.pin"
|
||||
"ArquivoDownload": "parameters/pinoutAtu-1_0.pin"
|
||||
},
|
||||
{
|
||||
"id": 9,
|
||||
|
|
@ -78,7 +78,7 @@
|
|||
"Extensao": ".pin",
|
||||
"Versao": "1_2",
|
||||
"TipoArquivo": 7,
|
||||
"ArquivoDownload": "pinoutSen-1_2.pin"
|
||||
"ArquivoDownload": "parameters/pinoutSen-1_2.pin"
|
||||
},
|
||||
{
|
||||
"id": 10,
|
||||
|
|
@ -91,51 +91,33 @@
|
|||
},
|
||||
{
|
||||
"id": 11,
|
||||
"Arquivo": "green_detector",
|
||||
"Diretorio": "Python\\Scripts\\",
|
||||
"Extensao": ".py",
|
||||
"Versao": "1_0",
|
||||
"TipoArquivo": 10,
|
||||
"ArquivoDownload": "green_detector-1_0.py"
|
||||
},
|
||||
{
|
||||
"id": 12,
|
||||
"Arquivo": "street_detector_oak",
|
||||
"Diretorio": "Python\\Scripts\\",
|
||||
"Extensao": ".py",
|
||||
"Versao": "1_0",
|
||||
"TipoArquivo": 8,
|
||||
"ArquivoDownload": "street_detector_oak-1_0.py"
|
||||
},
|
||||
{
|
||||
"id": 13,
|
||||
"Arquivo": "map_load",
|
||||
"Diretorio": "Python\\Scripts\\",
|
||||
"Extensao": ".py",
|
||||
"Versao": "1_0",
|
||||
"TipoArquivo": 11,
|
||||
"ArquivoDownload": "map_load-1_0.py"
|
||||
"ArquivoDownload": "scripts/map_load-1_0.py"
|
||||
},
|
||||
{
|
||||
"id": 14,
|
||||
"id": 12,
|
||||
"Arquivo": "map_follow",
|
||||
"Diretorio": "Python\\Scripts\\",
|
||||
"Extensao": ".py",
|
||||
"Versao": "1_0",
|
||||
"TipoArquivo": 12,
|
||||
"ArquivoDownload": "map_follow-1_0.py"
|
||||
"ArquivoDownload": "scripts/map_follow-1_0.py"
|
||||
},
|
||||
{
|
||||
"id": 15,
|
||||
"id": 13,
|
||||
"Arquivo": "gps_viewer",
|
||||
"Diretorio": "Python\\Scripts\\",
|
||||
"Extensao": ".py",
|
||||
"Versao": "1_0",
|
||||
"TipoArquivo": 13,
|
||||
"ArquivoDownload": "gps_viewer-1_0.py"
|
||||
"ArquivoDownload": "scripts/gps_viewer-1_0.py"
|
||||
},
|
||||
{
|
||||
"id": 16,
|
||||
"id": 14,
|
||||
"Arquivo": "modelo_3d",
|
||||
"Diretorio": "Python\\Output\\",
|
||||
"Extensao": ".obj",
|
||||
|
|
@ -144,7 +126,7 @@
|
|||
"ArquivoDownload": "modelo_3d-1_0.obj"
|
||||
},
|
||||
{
|
||||
"id": 17,
|
||||
"id": 15,
|
||||
"Arquivo": "modelo_3d",
|
||||
"Diretorio": "Python\\Output\\",
|
||||
"Extensao": ".mtl",
|
||||
|
|
@ -152,22 +134,4 @@
|
|||
"TipoArquivo": 14,
|
||||
"ArquivoDownload": "modelo_3d-1_0.mtl"
|
||||
},
|
||||
{
|
||||
"id": 18,
|
||||
"Arquivo": "list_oak",
|
||||
"Diretorio": "Python\\Scripts\\",
|
||||
"Extensao": ".py",
|
||||
"Versao": "1_0",
|
||||
"TipoArquivo": 15,
|
||||
"ArquivoDownload": "list_oak-1_0.py"
|
||||
},
|
||||
{
|
||||
"id": 19,
|
||||
"Arquivo": "mpc_controller",
|
||||
"Diretorio": "Python\\Scripts\\",
|
||||
"Extensao": ".py",
|
||||
"Versao": "1_0",
|
||||
"TipoArquivo": 16,
|
||||
"ArquivoDownload": "mpc_controller-1_0.py"
|
||||
}
|
||||
]
|
||||
|
|
@ -17,7 +17,7 @@
|
|||
<meta name="viewport" content="width=device-width,
|
||||
initial-scale=1.0, maximum-scale=1.0, user-scalable=no" />
|
||||
<style>
|
||||
#map_e21a72a5cb050d5571718c30559c2190 {
|
||||
#map_fbec21a667da83a7a2d972cb198fec24 {
|
||||
position: relative;
|
||||
width: 100.0%;
|
||||
height: 100.0%;
|
||||
|
|
@ -54,14 +54,14 @@
|
|||
<body>
|
||||
|
||||
|
||||
<div class="folium-map" id="map_e21a72a5cb050d5571718c30559c2190" ></div>
|
||||
<div class="folium-map" id="map_fbec21a667da83a7a2d972cb198fec24" ></div>
|
||||
|
||||
</body>
|
||||
<script>
|
||||
|
||||
|
||||
var map_e21a72a5cb050d5571718c30559c2190 = L.map(
|
||||
"map_e21a72a5cb050d5571718c30559c2190",
|
||||
var map_fbec21a667da83a7a2d972cb198fec24 = L.map(
|
||||
"map_fbec21a667da83a7a2d972cb198fec24",
|
||||
{
|
||||
center: [-22.172636164916668, -47.395186322185666],
|
||||
crs: L.CRS.EPSG3857,
|
||||
|
|
@ -78,7 +78,7 @@
|
|||
|
||||
|
||||
|
||||
var tile_layer_5d7b162daa8a21dbb285f6d50426847a = L.tileLayer(
|
||||
var tile_layer_d332ab9929d02ae6fda534ad4f881d9a = L.tileLayer(
|
||||
"https://tile.openstreetmap.org/{z}/{x}/{y}.png",
|
||||
{
|
||||
"minZoom": 0,
|
||||
|
|
@ -95,7 +95,7 @@
|
|||
);
|
||||
|
||||
|
||||
tile_layer_5d7b162daa8a21dbb285f6d50426847a.addTo(map_e21a72a5cb050d5571718c30559c2190);
|
||||
tile_layer_d332ab9929d02ae6fda534ad4f881d9a.addTo(map_fbec21a667da83a7a2d972cb198fec24);
|
||||
|
||||
|
||||
|
||||
|
|
@ -111,7 +111,7 @@
|
|||
}*/
|
||||
});
|
||||
}
|
||||
function geo_json_27cdeaf4853e626f96b749d12eb65555_onEachFeature(feature, layer) {
|
||||
function geo_json_2af9830ef68420da295e9fbbf418b0d8_onEachFeature(feature, layer) {
|
||||
|
||||
layer.on({
|
||||
|
||||
|
|
@ -148,23 +148,23 @@
|
|||
}*/
|
||||
});
|
||||
};
|
||||
var geo_json_27cdeaf4853e626f96b749d12eb65555 = L.geoJson(null, {
|
||||
onEachFeature: geo_json_27cdeaf4853e626f96b749d12eb65555_onEachFeature,
|
||||
var geo_json_2af9830ef68420da295e9fbbf418b0d8 = L.geoJson(null, {
|
||||
onEachFeature: geo_json_2af9830ef68420da295e9fbbf418b0d8_onEachFeature,
|
||||
|
||||
...{
|
||||
}
|
||||
});
|
||||
|
||||
function geo_json_27cdeaf4853e626f96b749d12eb65555_add (data) {
|
||||
geo_json_27cdeaf4853e626f96b749d12eb65555
|
||||
function geo_json_2af9830ef68420da295e9fbbf418b0d8_add (data) {
|
||||
geo_json_2af9830ef68420da295e9fbbf418b0d8
|
||||
.addData(data);
|
||||
}
|
||||
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_2af9830ef68420da295e9fbbf418b0d8_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_2af9830ef68420da295e9fbbf418b0d8.setStyle(function(feature) {return feature.properties.style;});
|
||||
|
||||
|
||||
|
||||
geo_json_27cdeaf4853e626f96b749d12eb65555.addTo(map_e21a72a5cb050d5571718c30559c2190);
|
||||
geo_json_2af9830ef68420da295e9fbbf418b0d8.addTo(map_fbec21a667da83a7a2d972cb198fec24);
|
||||
|
||||
</script>
|
||||
|
||||
|
|
@ -185,7 +185,7 @@
|
|||
}
|
||||
trajeto_json_add({"features": []});
|
||||
|
||||
trajeto_json.addTo(map_e21a72a5cb050d5571718c30559c2190);
|
||||
trajeto_json.addTo(map_fbec21a667da83a7a2d972cb198fec24);
|
||||
|
||||
function adicionarGeometria(novaGeometria) {
|
||||
trajeto_json.addData(novaGeometria);
|
||||
|
|
@ -243,7 +243,7 @@
|
|||
}
|
||||
trajeto_dinamico_json_add({"features": []});
|
||||
|
||||
trajeto_dinamico_json.addTo(map_e21a72a5cb050d5571718c30559c2190);
|
||||
trajeto_dinamico_json.addTo(map_fbec21a667da83a7a2d972cb198fec24);
|
||||
|
||||
function adicionarGeometriaDinamica(novaGeometria) {
|
||||
trajeto_dinamico_json.addData(novaGeometria);
|
||||
|
|
@ -296,9 +296,9 @@
|
|||
|
||||
var marcadorEquipamento = L.marker([0, 0], {
|
||||
icon: customIcon
|
||||
}).addTo(map_e21a72a5cb050d5571718c30559c2190);
|
||||
}).addTo(map_fbec21a667da83a7a2d972cb198fec24);
|
||||
|
||||
var marcadorBase = L.marker([0, 0], {}).addTo(map_e21a72a5cb050d5571718c30559c2190);
|
||||
var marcadorBase = L.marker([0, 0], {}).addTo(map_fbec21a667da83a7a2d972cb198fec24);
|
||||
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_e21a72a5cb050d5571718c30559c2190.setView(novaPosicao, map_e21a72a5cb050d5571718c30559c2190.getZoom());
|
||||
map_fbec21a667da83a7a2d972cb198fec24.setView(novaPosicao, map_fbec21a667da83a7a2d972cb198fec24.getZoom());
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -397,7 +397,7 @@
|
|||
function atualizarSelecaoRuas(selecionadas) {
|
||||
selecionadas = JSON.parse(selecionadas);
|
||||
RuasSelecionadas = Array.isArray(selecionadas) ? [...selecionadas] : [];
|
||||
geo_json_27cdeaf4853e626f96b749d12eb65555.eachLayer(function (layer) {
|
||||
geo_json_2af9830ef68420da295e9fbbf418b0d8.eachLayer(function (layer) {
|
||||
if (RuasSelecionadas.includes(parseInt(layer.feature.id))) {
|
||||
layer.setStyle({ color: 'blue' });
|
||||
} else {
|
||||
|
|
|
|||
Binary file not shown.
|
|
@ -78,7 +78,7 @@ class CameraOak:
|
|||
|
||||
if self.tem_imu:
|
||||
self.q_imu = self.device.getOutputQueue(name="imu", maxSize=50, blocking=False)
|
||||
self.imu = IMUCamera(self.q_imu)
|
||||
self.imu = IMUCamera(self.q_imu, freq=100, angulo_inicial=26.3)
|
||||
|
||||
if self.modelo_ia_onboard is not None:
|
||||
self.q_nn = self.device.getOutputQueue(name="nn", maxSize=1, blocking=False)
|
||||
|
|
|
|||
Binary file not shown.
|
|
@ -8,12 +8,12 @@ from shared.enums import StatusModulo, T_Code
|
|||
from health_worker.modulos.base import ModuloDiagnosticoBase
|
||||
|
||||
class IMUCamera(ModuloDiagnosticoBase):
|
||||
def __init__(self, queue, freq=10):
|
||||
def __init__(self, queue, freq=10, angulo_inicial=26.3):
|
||||
self.freq = freq
|
||||
self.imu_queue = queue
|
||||
self.filtro_imu = Madgwick(beta=0.8)
|
||||
self.filtro_imu = Madgwick(beta=0.8, frequency=freq)
|
||||
|
||||
self.roll_inicial = 63.7 # graus
|
||||
self.roll_inicial = 90 - angulo_inicial # graus
|
||||
self.pitch_inicial = 0.0
|
||||
self.yaw_inicial = 0.0
|
||||
|
||||
|
|
@ -24,6 +24,10 @@ class IMUCamera(ModuloDiagnosticoBase):
|
|||
self.imu_em_falha = False
|
||||
self.tempo_erro = 0
|
||||
|
||||
self.v_world = np.zeros(3, dtype=np.float32)
|
||||
self.last_imu_ts = None
|
||||
self.g = 9.80665
|
||||
|
||||
self.ativo = True
|
||||
self.imu_thread = threading.Thread(target=self.imu_task_loop, daemon=True)
|
||||
self.imu_thread.start()
|
||||
|
|
@ -58,12 +62,12 @@ class IMUCamera(ModuloDiagnosticoBase):
|
|||
az = accel.z
|
||||
|
||||
gx = np.deg2rad(gyro.x)
|
||||
gy_ = np.deg2rad(gyro.y)
|
||||
gy = np.deg2rad(gyro.y)
|
||||
gz = np.deg2rad(gyro.z)
|
||||
|
||||
self.q = self.filtro_imu.updateIMU(
|
||||
q=self.q,
|
||||
gyr=np.array([gx, gy_, gz]),
|
||||
gyr=np.array([gx, gy, gz]),
|
||||
acc=np.array([ax, ay, az])
|
||||
)
|
||||
r = R.from_quat([self.q[1], self.q[2], self.q[3], self.q[0]])
|
||||
|
|
@ -73,6 +77,43 @@ class IMUCamera(ModuloDiagnosticoBase):
|
|||
pitch -= self.pitch_inicial
|
||||
yaw -= self.yaw_inicial
|
||||
|
||||
Rwb = r.as_matrix()
|
||||
# dt estimado por pacote
|
||||
now = time.time()
|
||||
if self.last_imu_ts is None:
|
||||
dt = 1.0 / max(1e-3, f_tick) # fallback
|
||||
else:
|
||||
dt = max(0.0, now - self.last_imu_ts)
|
||||
self.last_imu_ts = now
|
||||
dt = min(dt, 0.05) # clamp anti-bursts (<=50 ms)
|
||||
self.filtro_imu.Dt = float(dt)
|
||||
|
||||
# 1) aceleração no mundo
|
||||
a_body = np.array([ax, ay, az], dtype=np.float64)
|
||||
a_world = Rwb @ a_body
|
||||
|
||||
# 2) remove gravidade
|
||||
a_lin = a_world - np.array([0.0, 0.0, self.g], dtype=np.float64)
|
||||
|
||||
# 3) filtra ruído muito baixo (deadzone)
|
||||
dead = 0.05 # m/s²
|
||||
a_lin[np.abs(a_lin) < dead] = 0.0
|
||||
|
||||
# 4) ZUPT (Zero-velocity update) se parado:
|
||||
# - giros muito baixos e módulo da aceleração ~ g
|
||||
if (np.linalg.norm([gx, gy, gz]) < np.deg2rad(2.0)
|
||||
and abs(np.linalg.norm(a_body) - self.g) < 0.12):
|
||||
# parado: puxe para zero agressivo
|
||||
self.v_world *= 0.2
|
||||
else:
|
||||
# 5) integra velocidade
|
||||
self.v_world += (a_lin * dt)
|
||||
|
||||
# 6) projeta no plano do chão (z=0) e computa velocidade escalar
|
||||
v_planar = self.v_world.copy()
|
||||
v_planar[2] = 0.0
|
||||
velocidade_mps = float(np.linalg.norm(v_planar))
|
||||
|
||||
t1 = time.time()
|
||||
latencia = t1 - t0
|
||||
f_exec = 1.0 / (latencia + 1e-9)
|
||||
|
|
@ -81,6 +122,7 @@ class IMUCamera(ModuloDiagnosticoBase):
|
|||
roll=round(roll, 2),
|
||||
pitch=round(pitch, 2),
|
||||
yaw=round(yaw, 2),
|
||||
vel_mps=round(velocidade_mps, 3),
|
||||
timestamp=t1,
|
||||
latencia=latencia,
|
||||
frequencia=f_tick
|
||||
|
|
|
|||
Binary file not shown.
|
|
@ -26,6 +26,8 @@ def definir_comando(pid: PIDAdaptativo):
|
|||
nav = None
|
||||
dists = None
|
||||
escalas = None
|
||||
d_obs_min = None
|
||||
block = None
|
||||
if not _snapshot_vw:
|
||||
visual_worker_atualizado = False
|
||||
else:
|
||||
|
|
@ -34,6 +36,8 @@ def definir_comando(pid: PIDAdaptativo):
|
|||
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)
|
||||
d_obs_min = _snapshot_vw.get("d_obs_min", None)
|
||||
block = _snapshot_vw.get("block", None)
|
||||
|
||||
_gps = ContextoGlobalRedis.get_modulo(T_Code.Gps)
|
||||
_gps_pos_atualizada = _gps.get("heartbeat", 0) != _gps.get("old_heartbeat", 0)
|
||||
|
|
@ -58,7 +62,6 @@ def definir_comando(pid: PIDAdaptativo):
|
|||
"PassosAtraso": _gps_passos_travados
|
||||
},
|
||||
"Carro": {
|
||||
#"Velocidade": 1.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),
|
||||
|
|
@ -83,6 +86,8 @@ def definir_comando(pid: PIDAdaptativo):
|
|||
"Navegavel": nav,
|
||||
"DistanciasRef": dists,
|
||||
"EscalasX": escalas,
|
||||
"DistanciaObsMin": d_obs_min,
|
||||
"Block": block
|
||||
},
|
||||
"Camera": {
|
||||
"FovH": ContextoGlobalRedis.get(CtxKey.DadosVisualWorker, {}).get("parametros", {}).get("fov_h", 0.7),
|
||||
|
|
@ -91,8 +96,8 @@ def definir_comando(pid: PIDAdaptativo):
|
|||
}
|
||||
},
|
||||
"RegrasAtivas": {
|
||||
"matriz_custo": True,
|
||||
"deteccao_obstaculos": False,
|
||||
"matriz_custo": False,
|
||||
"deteccao_obstaculos": True,
|
||||
},
|
||||
"Equipamento": {
|
||||
"largura": _equipamento.get("largura", 0.85),
|
||||
|
|
@ -173,8 +178,10 @@ def _regras_taticas(contexto):
|
|||
mostrar_log("⚠️ Possível travamento detectado (velocidade quase zero).")
|
||||
# Pode evoluir para uma lógica de recuperação no futuro
|
||||
|
||||
vw_ativado = contexto.get("VisualWorker", {}).get("Ativado", False)
|
||||
vw_operante = contexto.get("VisualWorker", {}).get("Operante", False)
|
||||
dados_vw = contexto.get("VisualWorker", {})
|
||||
|
||||
vw_ativado = dados_vw.get("Ativado", False)
|
||||
vw_operante = dados_vw.get("Operante", False)
|
||||
|
||||
if vw_ativado and not vw_operante:
|
||||
#mostrar_log("⚠️ Sensores principais inativos. Rodando controle no modo básico.")
|
||||
|
|
@ -192,19 +199,50 @@ def _regras_taticas(contexto):
|
|||
mostrar_log("⚠️ Sensores principais desativados. Rodando controle no modo básico.")
|
||||
return _comando_direcional_parado(False)
|
||||
|
||||
# >>> NOVO: gate de bloqueio/parede com block + d_obs_min <<<
|
||||
if vw_operante and contexto.get("RegrasAtivas", {}).get("deteccao_obstaculos", False):
|
||||
analise = ContextoGlobalRedis.get(CtxKey.DadosVisualWorker, {})
|
||||
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 _comando_direcional_parado(True)
|
||||
|
||||
block = dados_vw.get("MatrizCusto", {}).get("Block", None)
|
||||
d_obs_min = dados_vw.get("MatrizCusto", {}).get("DistanciaObsMin", None)
|
||||
|
||||
#print(block)
|
||||
|
||||
# parâmetros de parada (ajuste se quiser puxar do config)
|
||||
a_max_freio = 0.8 # m/s²
|
||||
margem_parada = 0.25
|
||||
|
||||
# distância necessária p/ parar com folga
|
||||
dist_freio = max(0.30, (velocidade_media ** 2) / (2.0 * a_max_freio))
|
||||
dist_necessaria = dist_freio + margem_parada
|
||||
|
||||
blocked = bool(block.get("blocked")) if isinstance(block, dict) else False
|
||||
reason = (block.get("reason") if isinstance(block, dict) else "none") or "none"
|
||||
|
||||
if blocked:
|
||||
mostrar_log(f"🟥 Bloqueio detectado ({reason}). d_obs_min={d_obs_min if d_obs_min is not None else '–'} m. Parando.")
|
||||
return _comando_direcional_parado(True)
|
||||
|
||||
if (d_obs_min is not None) and (d_obs_min <= dist_necessaria):
|
||||
mostrar_log(f"🟥 Obstáculo à {d_obs_min:.2f} m < distancia necessária {dist_necessaria:.2f} m. Parando.")
|
||||
return _comando_direcional_parado(True)
|
||||
|
||||
# opcional: dica de veto lateral pro MPC (se você consumir)
|
||||
side_bias = None
|
||||
if isinstance(block, dict):
|
||||
sb = block.get("side_bias") or {}
|
||||
side_bias = sb.get("value", None)
|
||||
if side_bias is not None:
|
||||
hints = contexto.setdefault("DirecionalHints", {})
|
||||
if side_bias > 0.25: # mais fechado à direita -> evite virar p/ direita
|
||||
hints["vetar_direita"] = True
|
||||
elif side_bias < -0.25: # mais fechado à esquerda -> evite virar p/ esquerda
|
||||
hints["vetar_esquerda"] = True
|
||||
|
||||
# se chegou até aqui, pode seguir
|
||||
return _comando_direcional_parado(False)
|
||||
|
||||
except Exception as e:
|
||||
mostrar_log(f"❌ Erro ao processar regras taticas do direcional: {e}")
|
||||
return _comando_direcional_parado(True)
|
||||
|
||||
def _comando_mapa_gps_mpc(contexto):
|
||||
try:
|
||||
|
|
|
|||
Binary file not shown.
|
|
@ -12,7 +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 visual_worker.processamento.costmap_fuser import CostmapFuser, unpack_snapshot
|
||||
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
|
||||
|
|
@ -73,6 +73,7 @@ class CameraManager:
|
|||
self.mostrar_log(f"❌ Camera com ID {mx_id} não iniciada.")
|
||||
else:
|
||||
self.mostrar_log(f"📷 Camera visual selecionada: {self.camera.modelo} - {self.camera.mx_id}")
|
||||
self.largura_robo_m = ContextoGlobalRedis.get(CtxKey.DadosEquipamento, {}).get("largura", 0.85)
|
||||
self.grid_ref_shape = (15, 10)
|
||||
self.grid_ref = self._gerar_grid_referencia_geometrico()
|
||||
self.depth_referencia = None
|
||||
|
|
@ -81,7 +82,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.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"]), robot_width=self.largura_robo_m)
|
||||
self.operante = True
|
||||
self._timestamp_analise = None
|
||||
|
||||
|
|
@ -111,9 +112,9 @@ class CameraManager:
|
|||
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)
|
||||
alpha_v = ((i + 0.5) / grid_h - 0.5) * np.radians(fov) # (i + 0.5) = centro da linha, (i + 0.0) = baixo da linha, (i + 1.0) = topo da linha
|
||||
gamma = np.radians(incl) + alpha_v
|
||||
d = (altura / np.tan(gamma)) * 1000.0
|
||||
d = (altura / np.tan(gamma)) #* 1000.0
|
||||
return d
|
||||
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
|
||||
|
|
@ -314,7 +315,7 @@ class CameraManager:
|
|||
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)
|
||||
#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
|
||||
|
|
@ -419,6 +420,14 @@ class CameraManager:
|
|||
)
|
||||
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"])
|
||||
#key, vis = self.debug_show_visualworker(frame_bgr=self._ultimo_rgb_frame, grid=grid_conf, wait=1, text_mode="full", draw_grid=True, draw_cells=True, draw_legend=True)
|
||||
|
||||
vel = 0.0
|
||||
imu = ContextoGlobalRedis.get_modulo(T_Code.Imu)
|
||||
if imu is not None and imu.get("saude", {}).get("status", StatusModulo.DESCONECTADO.value) == StatusModulo.OPERANTE.value:
|
||||
vel = imu.get("vel_mps", 0.0)
|
||||
|
||||
vis, metrics = self.debug_blockage_imshow(self._ultimo_rgb_frame, snapshot, velocidade_media=vel)
|
||||
except Exception as e:
|
||||
self.mostrar_log(f"❌ Erro na geracao da matriz de confianca: {e}")
|
||||
finally:
|
||||
|
|
@ -505,7 +514,7 @@ class CameraManager:
|
|||
self._analisando_matriz_custo = True
|
||||
try:
|
||||
t0 = time.time()
|
||||
largura_robo = ContextoGlobalRedis.get(CtxKey.DadosEquipamento, {}).get("largura", 0.85)
|
||||
largura_robo = self.largura_robo_m
|
||||
matriz_custo = self._gerar_matriz_custo_fundida(grid_conf, largura_robo, fov_h)
|
||||
t1 = time.time()
|
||||
matriz_custo["ultima_chamada"] = self._ultima_analise_matriz_custo.get("ultima_chamada", t0)
|
||||
|
|
@ -1637,3 +1646,440 @@ class CameraManager:
|
|||
key = cv2.waitKey(wait) & 0xFF
|
||||
return key, vis
|
||||
|
||||
|
||||
def _snap_to_array_u8(self, snap_value, H, W, name="array"):
|
||||
try:
|
||||
a = np.asarray(snap_value, dtype=np.uint8)
|
||||
if a.ndim == 1 and a.size == H*W:
|
||||
return a.reshape(H, W)
|
||||
if a.ndim == 2 and a.shape == (H, W):
|
||||
return a
|
||||
a = np.array(snap_value, dtype=np.uint8)
|
||||
if a.shape == (H, W):
|
||||
return a
|
||||
except Exception as e:
|
||||
self.mostrar_log(f"[VW-SNAP] falha convertendo {name}: {e}")
|
||||
self.mostrar_log(f"[VW-SNAP] shape invalido para {name}: esperado {(H,W)}, veio {np.shape(snap_value)}")
|
||||
return None
|
||||
|
||||
# NEW: pega primeiro campo existente e aplica escala (útil p/ z em m vs mm)
|
||||
def _snap_pick_numeric(self, snap: dict, names: list[str], H: int, W: int, *, scale: float = 1.0, as_float=True):
|
||||
import numpy as np, cv2
|
||||
for nm in names:
|
||||
if nm in snap and snap[nm] is not None:
|
||||
try:
|
||||
arr = np.asarray(snap[nm], dtype=np.float32 if as_float else np.int32)
|
||||
if arr.ndim == 1 and arr.size == H*W:
|
||||
arr = arr.reshape(H, W)
|
||||
elif arr.ndim != 2 or arr.shape != (H, W):
|
||||
# tenta redimensionar mantendo nearest neighbor (quando fizer sentido)
|
||||
arr = cv2.resize(arr.astype(np.float32), (W, H), interpolation=cv2.INTER_NEAREST)
|
||||
if scale != 1.0:
|
||||
arr = arr * scale
|
||||
return arr
|
||||
except Exception as e:
|
||||
self.mostrar_log(f"[VW-SNAP] erro lendo {nm}: {e}")
|
||||
return None
|
||||
|
||||
# UPDATED: agora lê conf/anom/z/pcts do SNAP e escreve no modo 'full'
|
||||
def make_visualworker_overlay_from_snap(
|
||||
self,
|
||||
frame_bgr,
|
||||
snap: dict,
|
||||
*,
|
||||
alpha_cost=0.45,
|
||||
draw_grid=True,
|
||||
draw_cells=True,
|
||||
text_mode="mini",
|
||||
draw_legend=True,
|
||||
nav_overlay_alpha=0.35,
|
||||
align_near_to_bottom=True
|
||||
):
|
||||
vis_fallback = frame_bgr.copy() if frame_bgr is not None else None
|
||||
try:
|
||||
Hf, Wf = frame_bgr.shape[:2]
|
||||
Wc = int(snap.get("grid_w", 0))
|
||||
Hc = int(snap.get("grid_h", 0))
|
||||
if Hc <= 0 or Wc <= 0:
|
||||
self.mostrar_log("[VW-SNAP] grid_w/grid_h ausentes no snap")
|
||||
return vis_fallback
|
||||
|
||||
custo_u8 = self._snap_to_array_u8(snap.get("custo_u8"), Hc, Wc, "custo_u8")
|
||||
if custo_u8 is None:
|
||||
return vis_fallback
|
||||
|
||||
# NAV: 1=navegavel, 0=bloqueado
|
||||
nav = None
|
||||
if snap.get("nav_mask") is not None:
|
||||
nav = self._snap_to_array_u8(snap.get("nav_mask"), Hc, Wc, "nav_mask")
|
||||
if nav is not None:
|
||||
nav = nav.astype(bool)
|
||||
|
||||
# orientacao
|
||||
near_is_bottom = bool(snap.get("fuse", {}).get("near_is_bottom", True))
|
||||
if align_near_to_bottom and not near_is_bottom:
|
||||
custo_u8 = np.flipud(custo_u8)
|
||||
if nav is not None:
|
||||
nav = np.flipud(nav)
|
||||
|
||||
# 1) heatmap (custo_u8 já está 0..255)
|
||||
up = cv2.resize(custo_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) NAV overlay
|
||||
if nav is not None:
|
||||
bad = (~nav).astype(np.uint8) * 255
|
||||
bad_up = cv2.resize(bad, (Wf, Hf), interpolation=cv2.INTER_NEAREST)
|
||||
mask = bad_up.astype(bool)
|
||||
overlay = out.copy()
|
||||
overlay[mask] = (0, 0, 255)
|
||||
out = cv2.addWeighted(overlay, 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)
|
||||
|
||||
# --- NEW: ler campos extras do SNAP ---
|
||||
# conf/anom em u8 → [0..1]
|
||||
conf_u8 = snap.get("conf_u8", None)
|
||||
conf = self._snap_to_array_u8(conf_u8, Hc, Wc, "conf_u8")/255.0 if conf_u8 is not None else None
|
||||
if conf is not None and align_near_to_bottom and not near_is_bottom: conf = np.flipud(conf)
|
||||
|
||||
anom_u8 = snap.get("anom_u8", None)
|
||||
anom = self._snap_to_array_u8(anom_u8, Hc, Wc, "anom_u8")/255.0 if anom_u8 is not None else None
|
||||
if anom is not None and align_near_to_bottom and not near_is_bottom: anom = np.flipud(anom)
|
||||
|
||||
# profundidade (se existir no snap): aceita *_m ou *_mm
|
||||
z_med = self._snap_pick_numeric(snap, ["z_med_m", "z_med_mm"], Hc, Wc, scale=1.0) # se mm, troque scale=0.001
|
||||
z_ref = self._snap_pick_numeric(snap, ["z_ref_m", "z_ref_mm"], Hc, Wc, scale=1.0)
|
||||
if z_med is not None and align_near_to_bottom and not near_is_bottom: z_med = np.flipud(z_med)
|
||||
if z_ref is not None and align_near_to_bottom and not near_is_bottom: z_ref = np.flipud(z_ref)
|
||||
|
||||
# classes (se existirem em u8 → 0..1)
|
||||
pct_rua = pct_cana = pct_obs = None
|
||||
if snap.get("pct_rua_u8") is not None:
|
||||
pct_rua = self._snap_to_array_u8(snap["pct_rua_u8"], Hc, Wc, "pct_rua_u8")/255.0
|
||||
if align_near_to_bottom and not near_is_bottom: pct_rua = np.flipud(pct_rua)
|
||||
if snap.get("pct_cana_u8") is not None:
|
||||
pct_cana = self._snap_to_array_u8(snap["pct_cana_u8"], Hc, Wc, "pct_cana_u8")/255.0
|
||||
if align_near_to_bottom and not near_is_bottom: pct_cana = np.flipud(pct_cana)
|
||||
if snap.get("pct_obs_u8") is not None:
|
||||
pct_obs = self._snap_to_array_u8(snap["pct_obs_u8"], Hc, Wc, "pct_obs_u8")/255.0
|
||||
if align_near_to_bottom and not near_is_bottom: pct_obs = np.flipud(pct_obs)
|
||||
|
||||
# 4) textos por célula
|
||||
if draw_cells and text_mode != "off":
|
||||
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
|
||||
|
||||
if nav is not None:
|
||||
color = (60,200,60) if nav[j,i] else (20,20,220)
|
||||
else:
|
||||
color = (90,90,90)
|
||||
cv2.rectangle(out, (x0, y0), (x1-1, y1-1), color, 1, cv2.LINE_AA)
|
||||
|
||||
if text_mode == "mini":
|
||||
txt = f"C{int(custo_u8[j,i])}"
|
||||
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":
|
||||
# linha 1: custo + conf + anom
|
||||
l1 = f"C{int(custo_u8[j,i])}"
|
||||
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-10, font_scale=0.38, thickness=1, color=(255,255,255), outline=True)
|
||||
|
||||
# linha 2: profundidade (se houver)
|
||||
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]:.2f}/{z_ref[j,i]:.2f}m"
|
||||
elif z_ref is not None:
|
||||
l2 = f"Zref {z_ref[j,i]:.2f}m"
|
||||
if l2:
|
||||
self._put_text_centered(out, l2, cx, cy+4, font_scale=0.36, thickness=1, color=(255,255,255), outline=True)
|
||||
|
||||
# linha 3: percentuais de classe (se houver)
|
||||
if pct_rua is not None and pct_cana is not None and pct_obs is not None:
|
||||
l3 = f"R{int(pct_rua[j,i]*100)} C{int(pct_cana[j,i]*100)} O{int(pct_obs[j,i]*100)}"
|
||||
self._put_text_centered(out, l3, cx, cy+18, font_scale=0.34, thickness=1, color=(230,230,230), outline=True)
|
||||
|
||||
# distâncias por linha (row_dist_m) — já estão “no referencial do snap”
|
||||
row_dist = snap.get("row_dist_m", None)
|
||||
if row_dist is not None and len(row_dist) == Hc:
|
||||
dist_draw = row_dist[::-1] if (align_near_to_bottom and not near_is_bottom) else row_dist
|
||||
for j in range(Hc):
|
||||
y = (y_edges[j] + y_edges[j + 1]) // 2
|
||||
s = f"{float(dist_draw[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)
|
||||
|
||||
# corredor central (se existir)
|
||||
cc = snap.get("fuse", {}).get("central_cols", None)
|
||||
if cc and len(cc) == 2:
|
||||
i0, i1 = int(cc[0]), int(cc[1])
|
||||
if 0 <= i0 <= Wc and 0 <= i1 <= Wc:
|
||||
cv2.line(out, (x_edges[i0], 0), (x_edges[i0], Hf-1), (255,255,0), 2, cv2.LINE_AA)
|
||||
cv2.line(out, (x_edges[i1], 0), (x_edges[i1], Hf-1), (255,255,0), 2, cv2.LINE_AA)
|
||||
|
||||
if draw_legend:
|
||||
pad = 8
|
||||
x0, y0 = pad, pad
|
||||
cv2.rectangle(out, (x0-4, y0-4), (x0+220, y0+104), (0,0,0), -1)
|
||||
cv2.putText(out, "VW SNAP overlay", (x0, y0+14), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255,255,255), 1, cv2.LINE_AA)
|
||||
cv2.putText(out, "C: 0..255 (fused)", (x0, y0+32), cv2.FONT_HERSHEY_SIMPLEX, 0.42, (200,200,200), 1, cv2.LINE_AA)
|
||||
cv2.putText(out, "cf: conf (0..1), A: %", (x0, y0+50), cv2.FONT_HERSHEY_SIMPLEX, 0.42, (200,200,200), 1, cv2.LINE_AA)
|
||||
cv2.putText(out, "Z: med/ref (m) se houver", (x0, y0+68), cv2.FONT_HERSHEY_SIMPLEX, 0.42, (200,200,200), 1, cv2.LINE_AA)
|
||||
cv2.putText(out, "N nav / B bloqueado", (x0, y0+86), cv2.FONT_HERSHEY_SIMPLEX, 0.42, (200,200,200), 1, cv2.LINE_AA)
|
||||
cv2.putText(out, f"{Wc}x{Hc}", (x0, y0+102), cv2.FONT_HERSHEY_SIMPLEX, 0.42, (160,160,160), 1, cv2.LINE_AA)
|
||||
|
||||
return out
|
||||
|
||||
except Exception as e:
|
||||
self.mostrar_log(f"[VW-SNAP] overlay error: {e}")
|
||||
return vis_fallback
|
||||
|
||||
|
||||
|
||||
|
||||
# === cole seu _compute_blockage_metrics aqui (sem mudanças) ===
|
||||
def _colorize_masks(self, anom_f, custo_f, conf_f, thr_anom_block=0.50, thr_cost_block=0.65, thr_conf_low=0.35):
|
||||
"""Overlay BGR com soma segura (clamped) nas regiões de máscara."""
|
||||
H, W = anom_f.shape
|
||||
over = np.zeros((H, W, 3), np.uint8)
|
||||
|
||||
mask_anom = (anom_f >= thr_anom_block)
|
||||
mask_cost = (custo_f >= thr_cost_block)
|
||||
mask_conf = (conf_f < thr_conf_low)
|
||||
|
||||
def add_color(mask, bgr):
|
||||
if not np.any(mask):
|
||||
return
|
||||
# soma segura com clip
|
||||
tmp = over[mask].astype(np.int16)
|
||||
tmp += np.array(bgr, dtype=np.int16)
|
||||
np.clip(tmp, 0, 255, out=tmp)
|
||||
over[mask] = tmp.astype(np.uint8)
|
||||
|
||||
add_color(mask_anom, (255, 0, 255)) # magenta
|
||||
add_color(mask_cost, ( 0,165,255)) # laranja
|
||||
add_color(mask_conf, (255, 0, 0)) # azul
|
||||
|
||||
return over, mask_anom, mask_cost, mask_conf
|
||||
|
||||
def debug_blockage_imshow(
|
||||
self,
|
||||
rgb_frame,
|
||||
snapshot,
|
||||
robot_width_m=0.84, margin_m=0.12,
|
||||
thr_anom_block=0.50, thr_cost_block=0.65, thr_conf_low=0.35,
|
||||
rho_block_central=0.70, rho_block_global=0.60,
|
||||
near_is_bottom=True,
|
||||
win_name="Block Debug",
|
||||
alpha=0.35,
|
||||
velocidade_media=0.97, # << NOVO (m/s)
|
||||
a_max_freio=0.8, # << NOVO (m/s²)
|
||||
margem_parada=0.25, # << NOVO (m)
|
||||
):
|
||||
# --- unpack do snapshot ---
|
||||
custo_f, conf_f, anom_f, nav_f = unpack_snapshot(snapshot)
|
||||
row_dist_m = np.asarray(snapshot.get("row_dist_m"), dtype=np.float32)
|
||||
row_scale_x_m = np.asarray(snapshot.get("row_scale_x_m"), dtype=np.float32)
|
||||
fuse = snapshot.get("fuse", {})
|
||||
central_cols = fuse.get("central_cols", None)
|
||||
if central_cols is None:
|
||||
# fallback: terço central
|
||||
H, W = custo_f.shape
|
||||
central_cols = (W//3, 2*W//3)
|
||||
else:
|
||||
central_cols = (int(central_cols[0]), int(central_cols[1]))
|
||||
|
||||
# prioriza near_is_bottom do snap, se vier
|
||||
if "near_is_bottom" in fuse:
|
||||
near_is_bottom = bool(fuse["near_is_bottom"])
|
||||
|
||||
Hf, Wf = rgb_frame.shape[:2]
|
||||
H, W = custo_f.shape
|
||||
|
||||
# --- métricas: usa do snap se existir; senão calcula ---
|
||||
metrics = snapshot.get("block")
|
||||
|
||||
blocked = bool(metrics.get("blocked", False))
|
||||
reason = metrics.get("reason", "none") or "none"
|
||||
d_obs_min = metrics.get("d_obs_min", None)
|
||||
j_block = metrics.get("j_block", None)
|
||||
cov_cent = metrics.get("coverage", {}).get("central_max", 0.0)
|
||||
cov_glob = metrics.get("coverage", {}).get("global", 0.0)
|
||||
sb = metrics.get("side_bias", {}) or {}
|
||||
side_val = float(sb.get("value", 0.0))
|
||||
left_frac = float(sb.get("left_frac", 0.0))
|
||||
right_frac= float(sb.get("right_frac", 0.0))
|
||||
|
||||
# --- overlays ---
|
||||
mask_rgb, mask_anom, mask_cost, mask_conf = self._colorize_masks(
|
||||
anom_f, custo_f, conf_f,
|
||||
thr_anom_block=thr_anom_block,
|
||||
thr_cost_block=thr_cost_block,
|
||||
thr_conf_low=thr_conf_low
|
||||
)
|
||||
mask_rgb_resized = cv2.resize(mask_rgb, (Wf, Hf), interpolation=cv2.INTER_NEAREST)
|
||||
|
||||
vis = rgb_frame.copy()
|
||||
vis = cv2.addWeighted(vis, 1.0, mask_rgb_resized, alpha, 0)
|
||||
|
||||
# --- desenha linha/retângulo na j_block ---
|
||||
if j_block is not None and 0 <= j_block < H:
|
||||
y = int((j_block + 0.5) * Hf / H)
|
||||
color_line = (0, 0, 255) if blocked else (0, 255, 255)
|
||||
cv2.line(vis, (0, y), (Wf, y), color_line, 2)
|
||||
|
||||
c0, c1 = central_cols
|
||||
c0 = max(0, min(W-1, int(c0)))
|
||||
c1 = max(0, min(W, int(c1)))
|
||||
if c1 <= c0:
|
||||
c0, c1 = W//3, 2*W//3
|
||||
|
||||
width_need_m = robot_width_m + margin_m
|
||||
sx = row_scale_x_m[j_block] if row_scale_x_m.size == H else (width_need_m / max(1, (c1 - c0)))
|
||||
if not np.isfinite(sx) or sx <= 1e-6:
|
||||
ncols = (c1 - c0)
|
||||
else:
|
||||
ncols = int(np.ceil(width_need_m / sx))
|
||||
ncols = max(1, min(W, ncols))
|
||||
|
||||
mid = (c0 + c1) // 2
|
||||
half = ncols // 2
|
||||
a = max(0, mid - half)
|
||||
b = min(W, a + ncols)
|
||||
a = max(0, b - ncols)
|
||||
|
||||
x1 = int(a * Wf / W)
|
||||
x2 = int(b * Wf / W)
|
||||
cv2.rectangle(vis, (x1, max(0, y-12)), (x2, min(Hf-1, y+12)), (0, 255, 0), 2)
|
||||
|
||||
txt = f"{reason.upper()} | d={d_obs_min:.2f} m" if d_obs_min is not None else f"{reason.upper()} | d=–"
|
||||
cv2.putText(vis, txt, (10, max(20, y-10)), cv2.FONT_HERSHEY_SIMPLEX, 0.6, color_line, 2, cv2.LINE_AA)
|
||||
|
||||
# --- HUDs (legíveis) ---
|
||||
x0, y0 = 10, 30
|
||||
bar_w, bar_h = 200, 12
|
||||
line_gap = 8
|
||||
font = cv2.FONT_HERSHEY_SIMPLEX
|
||||
font_scale = 0.60
|
||||
thick = 2
|
||||
|
||||
def put_text_outlined(img, text, org, font, font_scale, color_fg, thickness):
|
||||
# contorno preto
|
||||
cv2.putText(img, text, org, font, font_scale, (0,0,0), thickness+2, cv2.LINE_AA)
|
||||
# texto
|
||||
cv2.putText(img, text, org, font, font_scale, color_fg, thickness, cv2.LINE_AA)
|
||||
|
||||
def draw_bar(img, label, frac, top_left, color_fill=(0,255,255)):
|
||||
x, y = top_left
|
||||
# label acima da barra
|
||||
put_text_outlined(img, label, (x, y-2), font, font_scale, (255,255,255), thick)
|
||||
# moldura
|
||||
cv2.rectangle(img, (x, y+4), (x + bar_w, y + 4 + bar_h), (220,220,220), 1)
|
||||
# preenchimento
|
||||
fw = int(bar_w * float(np.clip(frac, 0, 1)))
|
||||
if fw > 0:
|
||||
cv2.rectangle(img, (x, y+4), (x + fw, y + 4 + bar_h), color_fill, -1)
|
||||
|
||||
# mede a altura ocupada pelo bloco para pintar um painel ao fundo
|
||||
panel_h = (
|
||||
# 2 barras (cada uma tem label+barra) + gaps
|
||||
(bar_h + 4) * 2 + (line_gap + 14) * 2
|
||||
)
|
||||
panel_w = max(260, bar_w + 70)
|
||||
|
||||
# painel semi-transparente
|
||||
overlay = vis.copy()
|
||||
cv2.rectangle(overlay, (x0-6, y0-6), (x0-6 + panel_w, y0-6 + panel_h), (0,0,0), -1)
|
||||
cv2.addWeighted(overlay, 0.35, vis, 0.65, 0, vis)
|
||||
|
||||
# barras Global / Central
|
||||
draw_bar(vis, f"Global {cov_glob:.2f}", cov_glob, (x0, y0))
|
||||
y0 += bar_h + line_gap + 14
|
||||
draw_bar(vis, f"Central {cov_cent:.2f}", cov_cent, (x0, y0))
|
||||
y0 += bar_h + line_gap + 14
|
||||
|
||||
# SideBias
|
||||
put_text_outlined(vis, f"SideBias {side_val:+.2f}", (x0, y0),
|
||||
font, font_scale, (255,255,255), thick)
|
||||
cx = x0 + bar_w//2
|
||||
y_bar = y0 + 14
|
||||
# linha base -1..+1
|
||||
cv2.line(vis, (x0, y_bar), (x0+bar_w, y_bar), (220,220,220), 1)
|
||||
# marca central
|
||||
cv2.line(vis, (cx, y_bar-4), (cx, y_bar+4), (255,255,255), 1)
|
||||
# cursor do bias
|
||||
bx = int(cx + (bar_w//2) * float(np.clip(side_val, -1, 1)))
|
||||
cv2.circle(vis, (bx, y_bar), 5, (0,255,0), -1)
|
||||
|
||||
# L / R
|
||||
put_text_outlined(vis, f"L:{left_frac:.2f} R:{right_frac:.2f}", (x0, y_bar + 20),
|
||||
font, font_scale, (255,255,255), thick)
|
||||
|
||||
# --- decisão PARAR / LIVRE ---
|
||||
v = float(max(0.0, velocidade_media))
|
||||
dist_freio = max(0.30, (v*v) / max(1e-9, 2.0 * a_max_freio))
|
||||
dist_necessaria = dist_freio + margem_parada
|
||||
|
||||
tem_obs_perto = (d_obs_min is not None) and np.isfinite(d_obs_min) and (d_obs_min <= dist_necessaria)
|
||||
deve_parar = bool(blocked) or tem_obs_perto
|
||||
# --- DECISION HUD: PARAR / LIVRE ---
|
||||
# empurra um pouco pra baixo do L/R
|
||||
y0_dec = y_bar + 50
|
||||
|
||||
dec_txt = "PARAR" if deve_parar else "LIVRE"
|
||||
dec_col = (0, 0, 255) if deve_parar else (0, 200, 0)
|
||||
dec_info = (f"v={v:.2f} m/s d_obs={('-' if d_obs_min is None else f'{d_obs_min:.2f} m')} "
|
||||
f"d_necess={dist_necessaria:.2f} m")
|
||||
|
||||
# painel por trás para legibilidade
|
||||
panel_w2 = max(280, bar_w + 100)
|
||||
overlay2 = vis.copy()
|
||||
cv2.rectangle(overlay2, (x0-6, y0_dec-24), (x0-6 + panel_w2, y0_dec+26), (0,0,0), -1)
|
||||
cv2.addWeighted(overlay2, 0.35, vis, 0.65, 0, vis)
|
||||
|
||||
# linha 1: PARAR / LIVRE (grande)
|
||||
put_text_outlined(vis, f"{dec_txt}", (x0, y0_dec-4),
|
||||
font, 0.80, dec_col, thick)
|
||||
|
||||
# linha 2: detalhes (menor)
|
||||
put_text_outlined(vis, dec_info, (x0, y0_dec+18),
|
||||
font, 0.58, (255,255,255), thick)
|
||||
|
||||
|
||||
legend = [
|
||||
("Anom >= thr", (255, 0, 255)),
|
||||
("Custo >= thr", (0,165,255)),
|
||||
("Conf < thr", (255, 0, 0)),
|
||||
]
|
||||
lx, ly = 10, Hf - 10 - 18*len(legend)
|
||||
for i, (txt, col) in enumerate(legend):
|
||||
y = ly + i*18
|
||||
cv2.rectangle(vis, (lx, y-12), (lx+18, y+2), col, -1)
|
||||
cv2.putText(vis, txt, (lx+24, y), cv2.FONT_HERSHEY_SIMPLEX, 0.48, (255,255,255), 1, cv2.LINE_AA)
|
||||
|
||||
status_txt = f"BLOCKED: {blocked} ({reason})"
|
||||
status_col = (0,0,255) if blocked else (0,255,0)
|
||||
cv2.putText(vis, status_txt, (Wf - 10 - 8*len(status_txt), 24),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.6, status_col, 2, cv2.LINE_AA)
|
||||
|
||||
cv2.imshow(win_name, vis)
|
||||
cv2.waitKey(1)
|
||||
return vis, metrics
|
||||
|
||||
|
|
@ -14,7 +14,8 @@ class CostmapFuser:
|
|||
central_cols=None, # (i0, i1) inclusivo; None = 3 colunas centrais
|
||||
y_range_m=(0.5, 5.0), # m: perto..longe (se não houver z_ref)
|
||||
near_is_bottom=True, # linha de baixo é mais perto?
|
||||
fov_h_rad=None
|
||||
fov_h_rad=None,
|
||||
robot_width=0.84
|
||||
):
|
||||
self.grid_w, self.grid_h = grid_shape
|
||||
self.K = int(K)
|
||||
|
|
@ -24,6 +25,7 @@ class CostmapFuser:
|
|||
self.y_range_m = y_range_m
|
||||
self.near_is_bottom = near_is_bottom
|
||||
self.fov_h_rad = fov_h_rad
|
||||
self.robot_width_m = robot_width
|
||||
|
||||
if central_cols is None:
|
||||
# 3 colunas centrais
|
||||
|
|
@ -178,11 +180,11 @@ class CostmapFuser:
|
|||
zref_f = z
|
||||
break
|
||||
|
||||
d_obs_min = self._compute_d_obs_min(custo_f, nav_f, zref_f)
|
||||
#d_obs_min = self._compute_d_obs_min(custo_f, nav_f, zref_f)
|
||||
|
||||
# distâncias por linha (m), usando z_ref se houver; senão, mapeamento linear y_range_m
|
||||
row_dist = self._row_distances(zref_f).astype(np.float32) # shape (grid_h,)
|
||||
row_dist_m = row_dist.astype(np.float32) / 1000.0
|
||||
row_dist_m = row_dist.astype(np.float32)
|
||||
|
||||
# escala X por linha (m/col)
|
||||
row_scale_x = self._row_scale_x(row_dist) # (H,) ou None
|
||||
|
|
@ -208,7 +210,7 @@ class CostmapFuser:
|
|||
"near_is_bottom": bool(self.near_is_bottom),
|
||||
},
|
||||
"y_range_m": [float(self.y_range_m[0]), float(self.y_range_m[1])],
|
||||
"d_obs_min": (None if d_obs_min is None else float(d_obs_min)),
|
||||
#"d_obs_min": (None if d_obs_min is None else float(d_obs_min)),
|
||||
"row_dist_m": row_dist_m.tolist(),
|
||||
"row_scale_x_m": row_scale_x.tolist(),
|
||||
"custo_u8": to_u8_list(custo_f),
|
||||
|
|
@ -222,8 +224,130 @@ class CostmapFuser:
|
|||
# zref_u8 = np.clip(zref_f / self.y_range_m[1] * 255.0, 0, 255).astype(np.uint8)
|
||||
# snap["zref_u8"] = zref_u8.ravel().tolist()
|
||||
|
||||
block = self._compute_blockage_metrics(
|
||||
custo_f, anom_f, conf_f, nav_f,
|
||||
row_dist_m, row_scale_x,
|
||||
central_cols=self.central_cols,
|
||||
robot_width_m=self.robot_width_m, # defina no __init__ ou config
|
||||
margin_m=0.12,
|
||||
)
|
||||
snap["block"] = block
|
||||
snap["d_obs_min"] = block["d_obs_min"] # mantém campo raiz por compatibilidade
|
||||
|
||||
return snap
|
||||
|
||||
def _compute_blockage_metrics(
|
||||
self,
|
||||
custo_f, anom_f, conf_f, nav_f,
|
||||
row_dist_m, row_scale_x_m, central_cols,
|
||||
robot_width_m=0.84, margin_m=0.12,
|
||||
thr_anom_block=0.50, thr_cost_block=0.65, thr_conf_low=0.35,
|
||||
rho_block_central=0.70, rho_block_global=0.60,
|
||||
near_is_bottom=True
|
||||
):
|
||||
"""
|
||||
Retorna dict com:
|
||||
- d_obs_min (m) ou None
|
||||
- blocked (bool)
|
||||
- reason ('obstacle','blackout','narrow','none')
|
||||
- coverage: {'central_max':..., 'global':...}
|
||||
- side_bias: {'value': -1..+1, 'left_frac':..., 'right_frac':...}
|
||||
- j_block (índice da linha que bloqueia) ou None
|
||||
"""
|
||||
|
||||
H, W = custo_f.shape
|
||||
c0, c1 = central_cols # intervalo central sugerido pelo seu snap
|
||||
c0 = max(0, min(W-1, int(c0)))
|
||||
c1 = max(0, min(W, int(c1)))
|
||||
if c1 <= c0:
|
||||
c0, c1 = W//3, 2*W//3 # fallback
|
||||
|
||||
# 1) Máscaras “inseguras”
|
||||
mask_anom = (anom_f >= thr_anom_block)
|
||||
mask_cost = (custo_f >= thr_cost_block)
|
||||
mask_conf = (conf_f < thr_conf_low)
|
||||
unsafe = mask_anom | mask_cost | mask_conf
|
||||
|
||||
# 2) Largura em colunas por linha (corredor = robô + margem)
|
||||
width_need_m = robot_width_m + margin_m
|
||||
cols_need = []
|
||||
for j in range(H):
|
||||
sx = row_scale_x_m[j] if row_scale_x_m is not None else (width_need_m / max(1, (c1 - c0)))
|
||||
if sx is None or sx <= 1e-6:
|
||||
cols_need.append(c1 - c0) # fallback
|
||||
else:
|
||||
ncols = int(np.ceil(width_need_m / sx))
|
||||
cols_need.append(max(1, min(W, ncols)))
|
||||
cols_need = np.asarray(cols_need, dtype=int)
|
||||
|
||||
# 3) Varredura por linha: janela central com largura cols_need[j]
|
||||
def central_window(j, ncols):
|
||||
# centra no meio de [c0,c1)
|
||||
mid = (c0 + c1) // 2
|
||||
half = ncols // 2
|
||||
a = max(0, mid - half)
|
||||
b = min(W, a + ncols)
|
||||
# ajusta se estourou esquerda/direita
|
||||
a = max(0, b - ncols)
|
||||
return a, b
|
||||
|
||||
coverage_central = np.zeros(H, np.float32)
|
||||
j_block = None
|
||||
for j in (range(H-1, -1, -1) if near_is_bottom else range(H)): # começa pelo "mais perto"
|
||||
a, b = central_window(j, cols_need[j])
|
||||
unsafe_row = unsafe[j, a:b]
|
||||
coverage = unsafe_row.mean() if (b > a) else 1.0
|
||||
coverage_central[j] = coverage
|
||||
if coverage >= rho_block_central:
|
||||
j_block = j
|
||||
break
|
||||
|
||||
# 4) Cobertura global (fallback diagnóstico)
|
||||
global_cov = unsafe.mean()
|
||||
|
||||
# 5) d_obs_min em metros
|
||||
d_obs_min = None if j_block is None else float(row_dist_m[j_block])
|
||||
|
||||
# 6) Viés lateral (onde está mais “fechado”)
|
||||
# mede cobertura do lado esquerdo vs direito dentro do intervalo [c0,c1)
|
||||
left = unsafe[:, c0:(c0+c1)//2].mean() if (c1-c0) >= 2 else 0.0
|
||||
right = unsafe[:, (c0+c1)//2:c1].mean() if (c1-c0) >= 2 else 0.0
|
||||
side_bias_val = float(np.clip((right - left) / max(1e-6, (right + left)), -1.0, 1.0))
|
||||
|
||||
# 7) Decisão de bloqueio e razão
|
||||
blocked = False
|
||||
reason = "none"
|
||||
if j_block is not None:
|
||||
blocked = True
|
||||
reason = "obstacle"
|
||||
elif global_cov >= rho_block_global and (conf_f.mean() < 0.45):
|
||||
blocked = True
|
||||
reason = "blackout" # visão ruim / depth ruim geral
|
||||
# opcional: “narrow” se central ok mas laterais muito ruins
|
||||
elif coverage_central.max() > 0.45 and (left > 0.7 or right > 0.7):
|
||||
reason = "narrow"
|
||||
|
||||
return {
|
||||
"d_obs_min": d_obs_min,
|
||||
"blocked": bool(blocked),
|
||||
"reason": reason,
|
||||
"coverage": {
|
||||
"central_max": float(coverage_central.max()),
|
||||
"global": float(global_cov),
|
||||
},
|
||||
"side_bias": {
|
||||
"value": side_bias_val, # <0 = mais fechado à esquerda; >0 = direita
|
||||
"left_frac": float(left),
|
||||
"right_frac": float(right),
|
||||
},
|
||||
"j_block": (None if j_block is None else int(j_block)),
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
def unpack_snapshot(snap):
|
||||
H, W = snap["grid_h"], snap["grid_w"]
|
||||
custo_u8 = np.array(snap["custo_u8"], dtype=np.uint8).reshape(H, W)
|
||||
|
|
|
|||
Loading…
Reference in New Issue