Commit d3446a30 authored by hengwen's avatar hengwen

Add rotation, velocity and acceleration to vehicle data frame.

parent a6479dbd
......@@ -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());
}
......
......@@ -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{
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment