public SumoCommand(Object input1, Object input2, Object input3, Object input)
{
this.cmd = new Command((int)input1);
this.input1 = (int)input1;
this.input2 = (int)input2;
cmd.Content().WriteUnsignedByte((int)input2);
cmd.Content().WriteStringASCII(input3.ToString());
if (input.GetType().Equals(typeof(StringList)))
{
StringList sl = (StringList)input;
cmd.Content().WriteUnsignedByte(Constants.TYPE_STRINGLIST);
cmd.Content().WriteInt(sl.Count());
foreach (string s in sl)
{
cmd.Content().WriteStringASCII(s);
}
}
else if (input.GetType().Equals(typeof(SumoStringList)))
{
SumoStringList sl = (SumoStringList)input;
cmd.Content().WriteUnsignedByte(Constants.TYPE_STRINGLIST);
cmd.Content().WriteInt(sl.getList().Count());
foreach (string s in sl.getList())
{
cmd.Content().WriteStringASCII(s);
}
}
else if (input.GetType().Equals(typeof(SumoTLSLogic)))
{
SumoTLSLogic stl = (SumoTLSLogic)input;
cmd.Content().WriteUnsignedByte(Constants.TYPE_COMPOUND);
cmd.Content().WriteInt(stl.phases.Count());
cmd.Content().WriteUnsignedByte(Constants.TYPE_STRING);
cmd.Content().WriteStringASCII(stl.subID);
cmd.Content().WriteUnsignedByte(Constants.TYPE_INTEGER);
cmd.Content().WriteInt(0);
cmd.Content().WriteUnsignedByte(Constants.TYPE_COMPOUND);
cmd.Content().WriteInt(0);
cmd.Content().WriteUnsignedByte(Constants.TYPE_INTEGER);
cmd.Content().WriteInt(stl.currentPhaseIndex);
cmd.Content().WriteUnsignedByte(Constants.TYPE_INTEGER);
cmd.Content().WriteInt(stl.phases.Count());
foreach (SumoTLSPhase phase in stl.phases)
{
Add_variable(phase);
}
}
else
{
Add_type(input);
Add_variable(input);
}
this.raw = new LinkedList <Object>();
this.raw.AddLast(input1);
this.raw.AddLast(input2);
this.raw.AddLast(input3);
this.raw.AddLast(input);
}
private void updateAllTrafficLights()
{
SumoStringList trafficLights = (SumoStringList)conn.Do_job_get(Trafficlights.GetIDList());
foreach (string s in trafficLights.getList())
{
//string phase = (string) conn.Do_job_get(Trafficlights.GetProgram(s));
string phase = (string)conn.Do_job_get(Trafficlights.GetRedYellowGreenState(s));
// TODO: ONLY WORKS FOR 3- or 4- lanes intersections!!!!!!!!!
int increment = (phase.Length == 6 ? 2 : 3);
for (int i = 0; i < phase.Length; i += increment)
{
switch (phase.Substring(i, 1))
{
case "G":
case "g":
trafficLights3D[s + i / increment].transform.Find("Traffic_light/rot").gameObject.SetActive(false);
trafficLights3D[s + i / increment].transform.Find("Traffic_light/gelb").gameObject.SetActive(false);
trafficLights3D[s + i / increment].transform.Find("Traffic_light/gruen").gameObject.SetActive(true);
trafficLights3D[s + i / increment].transform.Find("Traffic_light/rot1").gameObject.SetActive(false);
trafficLights3D[s + i / increment].transform.Find("Traffic_light/gelb1").gameObject.SetActive(false);
trafficLights3D[s + i / increment].transform.Find("Traffic_light/gruen1").gameObject.SetActive(true);
break;
case "Y":
case "y":
trafficLights3D[s + i / increment].transform.Find("Traffic_light/rot").gameObject.SetActive(false);
trafficLights3D[s + i / increment].transform.Find("Traffic_light/gelb").gameObject.SetActive(true);
trafficLights3D[s + i / increment].transform.Find("Traffic_light/gruen").gameObject.SetActive(false);
trafficLights3D[s + i / increment].transform.Find("Traffic_light/rot1").gameObject.SetActive(false);
trafficLights3D[s + i / increment].transform.Find("Traffic_light/gelb1").gameObject.SetActive(true);
trafficLights3D[s + i / increment].transform.Find("Traffic_light/gruen1").gameObject.SetActive(false);
break;
case "r":
case "R":
trafficLights3D[s + i / increment].transform.Find("Traffic_light/rot").gameObject.SetActive(true);
trafficLights3D[s + i / increment].transform.Find("Traffic_light/gelb").gameObject.SetActive(false);
trafficLights3D[s + i / increment].transform.Find("Traffic_light/gruen").gameObject.SetActive(false);
trafficLights3D[s + i / increment].transform.Find("Traffic_light/rot1").gameObject.SetActive(true);
trafficLights3D[s + i / increment].transform.Find("Traffic_light/gelb1").gameObject.SetActive(false);
trafficLights3D[s + i / increment].transform.Find("Traffic_light/gruen1").gameObject.SetActive(false);
break;
case "0":
case "o":
trafficLights3D[s + i / increment].transform.Find("Traffic_light/rot").gameObject.SetActive(false);
trafficLights3D[s + i / increment].transform.Find("Traffic_light/gelb").gameObject.SetActive(false);
trafficLights3D[s + i / increment].transform.Find("Traffic_light/gruen").gameObject.SetActive(false);
trafficLights3D[s + i / increment].transform.Find("Traffic_light/rot1").gameObject.SetActive(false);
trafficLights3D[s + i / increment].transform.Find("Traffic_light/gelb1").gameObject.SetActive(false);
trafficLights3D[s + i / increment].transform.Find("Traffic_light/gruen1").gameObject.SetActive(false);
break;
}
}
}
}
// Use this for initialization
void Start()
{
// Init Colors
colors = new List <Color> {
new Color(153f / 255f, 0f, 0f),
new Color(51f / 255f, 102f / 255f, 0f),
new Color(0f, 102f / 255f, 204f / 255f),
new Color(1f, 1f, 1f),
new Color(0f, 0f, 0f),
new Color(77f / 255f, 77f / 255f, 77f / 255f),
new Color(97f / 255f, 83f / 255f, 63f / 255f),
new Color(1f, 1f, 102f / 255f),
new Color(UnityEngine.Random.Range(0f, 1f), UnityEngine.Random.Range(0f, 1f), UnityEngine.Random.Range(0f, 1f)),
new Color(UnityEngine.Random.Range(0f, 1f), UnityEngine.Random.Range(0f, 1f), UnityEngine.Random.Range(0f, 1f)),
new Color(UnityEngine.Random.Range(0f, 1f), UnityEngine.Random.Range(0f, 1f), UnityEngine.Random.Range(0f, 1f)),
new Color(UnityEngine.Random.Range(0f, 1f), UnityEngine.Random.Range(0f, 1f), UnityEngine.Random.Range(0f, 1f))
};
// Read paths from text files
string sumoBinPath;
if (sumoGUIEnabled)
{
sumoBinPath = System.IO.File.ReadAllText(Application.dataPath + "\\Resources\\sumoBinPath.dat") + "\\sumo-gui.exe";
}
else
{
sumoBinPath = System.IO.File.ReadAllText(Application.dataPath + "\\Resources\\sumoBinPath.dat") + "\\sumo.exe";
}
string mapNet = (System.IO.File.ReadAllText(Application.dataPath + "\\Resources\\sumoFilesPath.dat") + "\\map.net.xml").Replace("/", "\\");
string routeNet = (System.IO.File.ReadAllText(Application.dataPath + "\\Resources\\sumoFilesPath.dat") + "\\map.rou.xml").Replace("/", "\\");
// Other vehicles
vehiclesInScene = new List <GameObject>();
vehicles = new Dictionary <string, SUMOCombinedPositionOrientation>();
vehicles3D = new Dictionary <string, GameObject>();
conn = new SumoTraciConnection(sumoBinPath, mapNet, routeNet);
conn.AddOption("step-length", stepLengthSeconds + "");
conn.AddOption("start", ""); // start sumo immediately
conn.AddOption("quit-on-end", ""); // stop sumo immediately after unity3D stopped
// start Traci Server
conn.RunServer(remoteIpAddress, Convert.ToInt32(this.remotePort));
if (enableEgoVehicle)
{
// Insert ego vehicle
// Get random route for being able to insert the ego vehicle (A new vehicle must have a route assigned)
SumoStringList routes = (SumoStringList)conn.Do_job_get(Route.GetIDList());
conn.Do_job_set(Vehicle.Add("egoVehicle", "DEFAULT_VEHTYPE", routes.Get(0), 0, 0, 0, (byte)0));
conn.Do_job_set(Vehicle.SetColor("egoVehicle", new SumoColor(127, 0, 0, 127)));
}
// Find all traffic lights
trafficLights3D = new Dictionary <string, GameObject>();
SumoStringList trafficLights = (SumoStringList)conn.Do_job_get(Trafficlights.GetIDList());
foreach (string s in trafficLights.getList())
{
var objects = Resources.FindObjectsOfTypeAll <GameObject>().Where(obj => obj.name.Equals("TrafficLight_" + s));
foreach (GameObject g in objects)
{
trafficLights3D.Add(s + g.transform.Find("index").GetChild(0).name, g);
}
}
// Timer for Realtime-Simulation
unity3DstartTime = Stopwatch.StartNew();
// Timer for traffic lights update (No need to do this every sim step)
trafficLightsUpdate = Stopwatch.StartNew();
// Timer for watchdog
watchdog = new Stopwatch();
}
// Update is called once per frame
void Update()
{
watchdog.Start();
// Simulation step with realtime check
if ((unity3DstartTime.ElapsedMilliseconds > stepLengthSeconds * 1000 * simStep) && (simStep < maxSimSteps))
{
// Update egoVehicle coordinates for forwarding them to SUMO
double xEgo = 0;;
double yEgo = 0;;
double angleEgo = 0;;
if (enableEgoVehicle)
{
egoVehicle = GameObject.Find("egoVehicle_Peugot_WASD(Clone)");
if (egoVehicle == null)
{
egoVehicle = GameObject.Find("egoVehicle_Peugot(Clone)");
}
xEgo = egoVehicle.transform.position.x;
yEgo = egoVehicle.transform.position.z;
angleEgo = egoVehicle.transform.rotation.eulerAngles.y;
}
// Only do next time step if it's necessary (not to fast)
conn.Do_timestep();
simStep++;
if (enableEgoVehicle)
{
// Update egoVehicle
conn.Do_job_set(Vehicle.MoveToVTD("egoVehicle", "0", 0, xEgo, yEgo, angleEgo, 2));
}
// Before updating all vehicles store the vehicles from last step for steering angle approximation (and prob. other interpolations)
lastStepVehicles = vehicles.ToDictionary(i => i.Key, i => i.Value);
// Get IDs of all vehicles in Simulation
SumoStringList vehicleIDs = (SumoStringList)conn.Do_job_get(Vehicle.GetIDList());
vehicles.Clear();
foreach (string id in vehicleIDs.getList())
{
SumoPosition3D position = (SumoPosition3D)conn.Do_job_get(Vehicle.GetPosition3D(id));
double yawAngle = (double)conn.Do_job_get(Vehicle.GetAngle(id));
vehicles.Add(id, new SUMOCombinedPositionOrientation(id, position, yawAngle));
}
// Update all 3D vehicle models
update3DVehiclesInScene();
// Update all traffic lights every refreshTimeTrafficLightsMS miliseconds
if (trafficLightsUpdate.ElapsedMilliseconds > refreshTimeTrafficLightsMS)
{
updateAllTrafficLights();
trafficLightsUpdate.Reset();
trafficLightsUpdate.Start();
}
}
// Watchdog
long stopTime = watchdog.ElapsedMilliseconds;
if (stopTime > stepLengthSeconds * 1000)
{
watchDogCount++;
print("Warning: Stepwidth of " + stepLengthSeconds * 1000 + "ms is too short for calculation loop, needed " + stopTime +
"ms, Exceeded " + watchDogCount + " times, Current total offset: " + (unity3DstartTime.ElapsedMilliseconds - 1000 * simStep * stepLengthSeconds + stopTime) + " ms");
}
watchdog.Reset();
}
public SumoCommand(Object input1, Object input2, Object input3, Object[] array)
{
this.cmd = new Command((int)input1);
this.input1 = (int)input1;
this.input2 = (int)input2;
cmd.Content().WriteUnsignedByte((int)input2);
cmd.Content().WriteStringASCII(input3.ToString());
if ((int)input2 == Constants.VAR_COLOR)
{
cmd.Content().WriteUnsignedByte(Constants.TYPE_COLOR);
for (int i = 0; i < array.Length; i++)
{
Add_variable(array[i]);
}
}
else if ((int)input2 == Constants.VAR_ROUTE)
{
cmd.Content().WriteUnsignedByte(Constants.TYPE_STRINGLIST);
SumoStringList sl = (SumoStringList)array[0];
cmd.Content().WriteInt(sl.getList().Count());
foreach (string s in sl.getList())
{
cmd.Content().WriteStringASCII(s);
}
}
else if ((int)input2 == Constants.CMD_REROUTE_EFFORT || (int)input2 == Constants.CMD_REROUTE_TRAVELTIME || (int)input2 == Constants.CMD_RESUME)
{
cmd.Content().WriteUnsignedByte(Constants.TYPE_COMPOUND);
cmd.Content().WriteInt(0);
}
else if ((int)input2 == Constants.VAR_VIEW_BOUNDARY)
{
cmd.Content().WriteUnsignedByte(Constants.TYPE_BOUNDINGBOX);
for (int i = 0; i < array.Length; i++)
{
Add_variable(array[i]);
}
}
else if ((int)input2 == Constants.VAR_VIEW_OFFSET)
{
cmd.Content().WriteUnsignedByte(Constants.POSITION_2D);
for (int i = 0; i < array.Length; i++)
{
Add_variable(array[i]);
}
}
else if ((int)input1 == Constants.CMD_SET_POLYGON_VARIABLE && (int)input2 == Constants.ADD)
{
cmd.Content().WriteUnsignedByte(Constants.TYPE_COMPOUND);
cmd.Content().WriteInt(5);
Add_type(array[3]);
Add_variable(array[3]);
Add_type(array[1]);
Add_variable(array[1]);
Add_type(array[2]);
Add_variable(array[2]);
Add_type(array[4]);
Add_variable(array[4]);
Add_type(array[0]);
Add_variable(array[0]);
}
else if ((int)input1 == Constants.CMD_SET_POI_VARIABLE && (int)input2 == Constants.VAR_POSITION)
{
cmd.Content().WriteUnsignedByte(Constants.POSITION_2D);
Add_variable(array[0]);
Add_variable(array[1]);
}
else if ((int)input1 == Constants.CMD_SET_POI_VARIABLE && (int)input2 == Constants.ADD)
{
cmd.Content().WriteUnsignedByte(Constants.TYPE_COMPOUND);
cmd.Content().WriteInt(4);
Add_type(array[3]);
Add_variable(array[3]);
Add_type(array[2]);
Add_variable(array[2]);
Add_type(array[4]);
Add_variable(array[4]);
cmd.Content().WriteUnsignedByte(Constants.POSITION_2D);
Add_variable(array[0]);
Add_variable(array[1]);
}
else
{
cmd.Content().WriteUnsignedByte(Constants.TYPE_COMPOUND);
cmd.Content().WriteInt(array.Length);
for (int i = 0; i < array.Length; i++)
{
Add_type(array[i]);
Add_variable(array[i]);
}
}
this.raw = new LinkedList <Object>();
this.raw.AddLast(input1);
this.raw.AddLast(input2);
this.raw.AddLast(input3);
this.raw.AddLast(array);
}
private void Add_variable(Object input)
{
if (input.GetType().Equals(typeof(int)))
{
this.cmd.Content().WriteInt((int)input);
}
else if (input.GetType().Equals(typeof(string)))
{
this.cmd.Content().WriteStringASCII((string)input);
}
else if (input.GetType().Equals(typeof(Double)))
{
this.cmd.Content().WriteDouble((Double)input);
}
else if (input.GetType().Equals(typeof(Byte)))
{
this.cmd.Content().WriteByte((Byte)input);
}
else if (input.GetType().Equals(typeof(bool)))
{
bool b = (bool)input;
cmd.Content().WriteUnsignedByte(b ? 1 : 0);
}
else if (input.GetType().Equals(typeof(SumoColor)))
{
SumoColor sc = (SumoColor)input;
this.cmd.Content().WriteByte(sc.r);
this.cmd.Content().WriteByte(sc.g);
this.cmd.Content().WriteByte(sc.b);
this.cmd.Content().WriteByte(sc.a);
}
else if (input.GetType().Equals(typeof(SumoGeometry)))
{
SumoGeometry sg = (SumoGeometry)input;
cmd.Content().WriteUnsignedByte(sg.coords.Count());
foreach (SumoPosition2D pos in sg.coords)
{
cmd.Content().WriteDouble(pos.x);
cmd.Content().WriteDouble(pos.y);
}
}
else if (input.GetType().Equals(typeof(SumoPosition2D)))
{
SumoPosition2D pos = (SumoPosition2D)input;
cmd.Content().WriteDouble(pos.x);
cmd.Content().WriteDouble(pos.y);
}
else if (input.GetType().Equals(typeof(SumoTLSPhase)))
{
SumoTLSPhase stp = (SumoTLSPhase)input;
this.cmd.Content().WriteUnsignedByte(Constants.TYPE_INTEGER);
cmd.Content().WriteInt(stp.duration);
this.cmd.Content().WriteUnsignedByte(Constants.TYPE_INTEGER);
cmd.Content().WriteInt(stp.duration1);
this.cmd.Content().WriteUnsignedByte(Constants.TYPE_INTEGER);
cmd.Content().WriteInt(stp.duration2);
this.cmd.Content().WriteUnsignedByte(Constants.TYPE_STRING);
cmd.Content().WriteStringASCII(stp.phasedef);
}
else if (input.GetType().Equals(typeof(SumoStringList)))
{
SumoStringList sl = (SumoStringList)input;
cmd.Content().WriteInt(sl.getList().Count());
foreach (string s in sl.getList())
{
cmd.Content().WriteStringASCII(s);
}
}
}
