From f7a9b125730dccba75d0c51dd594fe85cd906f26 Mon Sep 17 00:00:00 2001
From: MankaranSingh <mankaran32@gmail.com>
Date: Sun, 25 Sep 2022 16:01:59 +0530
Subject: [PATCH] fix lead message not filling

---
 .../ui/java/ai.flow.app/OnRoadScreen.java     |  8 ++++----
 .../java/ai.flow.vision/ParsedOutputs.java    | 10 +++++-----
 .../vision/java/ai.flow.vision/Parser.java    |  7 -------
 .../messages/MsgModelDataV2.java              | 20 ++++++++++---------
 4 files changed, 20 insertions(+), 25 deletions(-)

diff --git a/selfdrive/ui/java/ai.flow.app/OnRoadScreen.java b/selfdrive/ui/java/ai.flow.app/OnRoadScreen.java
index f37ccb47..5bdc961f 100644
--- a/selfdrive/ui/java/ai.flow.app/OnRoadScreen.java
+++ b/selfdrive/ui/java/ai.flow.app/OnRoadScreen.java
@@ -426,8 +426,8 @@ public void updateModelOutputs(){
             edge1 = Draw.getLaneCameraFrame(parsed.roadEdges.get(1), K, Rt, 0.3f);
 
             lead1s = Draw.getTriangleCameraFrame(parsed.leads.get(0), K, Rt, leadDrawScale);
-            lead2s = Draw.getTriangleCameraFrame(parsed.leads.get(0), K, Rt, leadDrawScale);
-            lead3s = Draw.getTriangleCameraFrame(parsed.leads.get(0), K, Rt, leadDrawScale);
+            lead2s = Draw.getTriangleCameraFrame(parsed.leads.get(1), K, Rt, leadDrawScale);
+            lead3s = Draw.getTriangleCameraFrame(parsed.leads.get(2), K, Rt, leadDrawScale);
         }
     }
 
@@ -511,8 +511,8 @@ public void drawModelOutputs() {
         }
 
         drawLeadTriangle(lead1s, colorLead, parsed.leads.get(0).prob);
-        drawLeadTriangle(lead2s, colorLead, parsed.leads.get(1).prob);
-        drawLeadTriangle(lead3s, colorLead, parsed.leads.get(2).prob);
+        //drawLeadTriangle(lead2s, colorLead, parsed.leads.get(1).prob);
+        //drawLeadTriangle(lead3s, colorLead, parsed.leads.get(2).prob);
         appContext.shapeRenderer.end();
 
         Gdx.gl.glDisable(Gdx.gl.GL_BLEND);
diff --git a/selfdrive/vision/java/ai.flow.vision/ParsedOutputs.java b/selfdrive/vision/java/ai.flow.vision/ParsedOutputs.java
index 4878bd06..726ff10e 100644
--- a/selfdrive/vision/java/ai.flow.vision/ParsedOutputs.java
+++ b/selfdrive/vision/java/ai.flow.vision/ParsedOutputs.java
@@ -5,11 +5,11 @@
 import java.util.Arrays;
 
 public class ParsedOutputs {
-    public ArrayList<float[]> position;
-    public ArrayList<float[]> orientation;
-    public ArrayList<float[]> velocity;
-    public ArrayList<float[]> orientationRate;
-    public ArrayList<float[]> acceleration;
+    public ArrayList<float[]> position; // path
+    public ArrayList<float[]> orientation; // path
+    public ArrayList<float[]> velocity; // path
+    public ArrayList<float[]> orientationRate; // path
+    public ArrayList<float[]> acceleration; // path
 
     public ArrayList<ArrayList<float[]>> laneLines;
     public ArrayList<float[]> laneLineStds;
diff --git a/selfdrive/vision/java/ai.flow.vision/Parser.java b/selfdrive/vision/java/ai.flow.vision/Parser.java
index a1994c48..5306b15d 100644
--- a/selfdrive/vision/java/ai.flow.vision/Parser.java
+++ b/selfdrive/vision/java/ai.flow.vision/Parser.java
@@ -66,13 +66,6 @@ public class Parser {
     public float[] transStd = parsed.transStd;
     public float[] rot = parsed.rot;
     public float[] rotStd = parsed.rotStd;
-
-    public ArrayList<float[]> position = parsed.position;
-    public ArrayList<float[]> orientation = parsed.orientation;
-    public ArrayList<float[]> velocity = parsed.velocity;
-    public ArrayList<float[]> orientationRate = parsed.orientationRate;
-    public ArrayList<float[]> acceleration = parsed.acceleration;
-
     public float[] laneLineProbs = parsed.laneLineProbs;
     public ArrayList<float[]> laneLineStds = parsed.laneLineStds;
     public ArrayList<ArrayList<float[]>> laneLines = parsed.laneLines;
diff --git a/selfdrive/vision/java/ai.flow.vision/messages/MsgModelDataV2.java b/selfdrive/vision/java/ai.flow.vision/messages/MsgModelDataV2.java
index bcf1ad82..a13b7c25 100644
--- a/selfdrive/vision/java/ai.flow.vision/messages/MsgModelDataV2.java
+++ b/selfdrive/vision/java/ai.flow.vision/messages/MsgModelDataV2.java
@@ -323,18 +323,20 @@ public void fillParsed(ParsedOutputs parsed, Definitions.ModelDataV2.Reader msg,
         }
 
         for (int i = 0; i < 4; i++) {
-            parsed.leads.get(0).xyva[i] = xyva1.get(i);
-            parsed.leads.get(1).xyva[i] = xyva2.get(i);
-            parsed.leads.get(2).xyva[i] = xyva3.get(i);
+            parsed.leads.get(0).xyva[i] = msg.getLeads().get(0).getXyva().get(i);
+            parsed.leads.get(1).xyva[i] = msg.getLeads().get(1).getXyva().get(i);
+            parsed.leads.get(2).xyva[i] = msg.getLeads().get(2).getXyva().get(i);
 
-            parsed.leads.get(0).xyvaStd[i] = xyvaStd1.get(i);
-            parsed.leads.get(1).xyvaStd[i] = xyvaStd2.get(i);
-            parsed.leads.get(2).xyvaStd[i] = xyvaStd3.get(i);
+            parsed.leads.get(0).xyvaStd[i] = msg.getLeads().get(0).getXyvaStd().get(i);
+            parsed.leads.get(1).xyvaStd[i] = msg.getLeads().get(1).getXyvaStd().get(i);
+            parsed.leads.get(2).xyvaStd[i] = msg.getLeads().get(2).getXyvaStd().get(i);
         }
 
-        parsed.leads.get(0).prob = leads1.getProb();
-        parsed.leads.get(1).prob = leads2.getProb();
-        parsed.leads.get(2).prob = leads3.getProb();
+        parsed.leads.get(0).prob = msg.getLeads().get(0).getProb();
+        parsed.leads.get(1).prob = msg.getLeads().get(1).getProb();
+        parsed.leads.get(2).prob = msg.getLeads().get(2).getProb();
+
+
 
         if (full) {