fix lead message not filling

flowdriveai · Sep 25, 2022 · f7a9b12 · f7a9b12
1 parent 420cbe5
commit f7a9b12
Show file tree

Hide file tree

Showing 4 changed files with 20 additions and 25 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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) {