Add rotation, velocity and acceleration to vehicle data frame.

rpc/src/main/java/server/rpc/SimulationObserver.java

@@ -9,6 +9,7 @@ import org.json.simple.JSONArray;
 import org.json.simple.JSONObject;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
+import rpc.protobuf.DoubleArray;
 import rpc.protobuf.SimulationDataFrame;
 import rpc.protobuf.VehicleSimulationDataFrame;
 import simulation.environment.WorldModel;
@@ -19,10 +20,7 @@ import java.io.FileNotFoundException;
 import java.io.IOException;
 import java.io.PrintWriter;
 import java.io.StringWriter;
-import java.util.ArrayList;
-import java.util.HashMap;
-import java.util.List;
-import java.util.Map;
+import java.util.*;
 
 /**
  * SimulationObserver collects simulation result from simulators
@@ -121,10 +119,38 @@ public class SimulationObserver implements SimulationLoopNotifiable {
 
         String globalID = controller.getVehicleGlobalIDByLocalID(((PhysicalVehicle) simulationLoopExecutable).getId());
         SimulationDataFrame.Builder vframe = SimulationDataFrame.newBuilder();
-        vframe.setPosX((float) longitude).setPosY((float) latitude).setPosZ((float) position[2]);
+        vframe.setPosX((double) longitude).setPosY((double) latitude).setPosZ((double) position[2]);
         vframe.setSteering((float) ((PhysicalVehicle) simulationLoopExecutable).getSteeringAngle());
         vframe.setTotalTime(l);
         vframe.setDeltaTime(l1);
+
+        // set rotation
+        double[][] rotation = ((PhysicalVehicle) simulationLoopExecutable).getRotation().getData();
+        for (int i = 0; i < rotation.length; i++) {
+            DoubleArray.Builder builder = DoubleArray.newBuilder();
+            for (int j = 0; j < rotation[i].length; j++) {
+                builder.addValue(rotation[i][j]);
+            }
+            vframe.addRotation(builder.build());
+        }
+
+        // set velocity
+        double[] data = ((PhysicalVehicle) simulationLoopExecutable).getVelocity().toArray();
+        DoubleArray.Builder velocity = DoubleArray.newBuilder();
+        for (int i = 0; i < data.length; i++) {
+            velocity.addValue(data[i]);
+        }
+        vframe.setVelocity(velocity.build());
+
+        // set acceleration
+        double mass = ((PhysicalVehicle) simulationLoopExecutable).getMass();
+        data = ((PhysicalVehicle) simulationLoopExecutable).getForce().mapDivide(mass).toArray();
+        DoubleArray.Builder acceleration = DoubleArray.newBuilder();
+        for (int i = 0; i < data.length; i++) {
+            acceleration.addValue(data[i]);
+        }
+        vframe.setAcceleration(acceleration.build());
+
         frames.get(globalID).addFrames(vframe.build());
     }
 

rpc/src/main/proto/simulationController.proto

@@ -72,6 +72,10 @@ message GetSimulationResultRequest{
     int32 size = 2;
 }
 
+message DoubleArray{
+    repeated double value = 1 [packed=true];
+}
+
 message SimulationDataFrame{
     double posX = 1;
     double posY = 2;
@@ -80,9 +84,12 @@ message SimulationDataFrame{
     float steering = 4;
     float engine = 5;
     float brake = 6;
+    repeated DoubleArray rotation = 7;
+    DoubleArray velocity = 8;
+    DoubleArray acceleration = 9;
 
-    float deltaTime = 7;
-    float totalTime = 8;
+    float deltaTime = 10;
+    float totalTime = 11;
 };
 
 message PlannedTrajectory{