public List <SimulatedVehicle> getList(String filePath)
{
CSVReader reader = new CSVReader(filePath);
if (reader != null)
{
reader.readHeader();
while (reader.readRecord())
{
SimulatedVehicle s = new SimulatedVehicle();
s.SimulationTime = Convert.ToSingle(reader.get(CSVCloumnName.SimulationTime));
s.PositionX = Convert.ToSingle(reader.get(CSVCloumnName.PositionX));
s.PositionY = Convert.ToSingle(reader.get(CSVCloumnName.PositionY));
s.Speed = Convert.ToSingle(reader.get(CSVCloumnName.Speed));
s.Acceleration = Convert.ToSingle(reader.get(CSVCloumnName.Acceleration));
s.SteeringWheel = Convert.ToSingle(reader.get(CSVCloumnName.SteeringWheel));
s.Offset = Convert.ToSingle(reader.get(CSVCloumnName.Offset));
s.BrakePedal = Convert.ToSingle(reader.get(CSVCloumnName.BrakePedal));
s.TotalDistance = Convert.ToSingle(reader.get(CSVCloumnName.TotalDistance));
s.Braking = Convert.ToSingle(reader.get(CSVCloumnName.Braking));
s.Reacting = Convert.ToSingle(reader.get(CSVCloumnName.Reacting));
s.Area = Convert.ToSingle(reader.get(CSVCloumnName.Area));
dataList.Add(s);
s = null;
}
return(dataList);
}
return(null);
}
private void skipUselessData()
{
SimulatedVehicle vehicleFirst = udp.getData();
while (true)
{
if (udp == null)
{
break;
}
SimulatedVehicle vehicleNext = udp.getData();
if (vehicleNext == null || vehicleNext.SimulationTime == vehicleFirst.SimulationTime)
{
continue;
}
else
{
play(vehicleNext);
PageList.Experience.refreshTextBlocks();
if (vissim != null)
{
vissim.set(currentVehicle);
}
break;
}
}
}
private void reset()
{
user = PageList.Main.User;
mode = PageList.Main.Selection.ModeSelect;
scene = PageList.Main.Selection.SceneSelect;
currentVehicle = null;
vehicles = new List <SimulatedVehicle>();
brakeActivity = new BrakeActivity();
reactActivity = new ReactActivity();
}
public void play(SimulatedVehicle vehicle)
{
if (vehicle != null && vehicle.SimulationTime > 0.000001)
{
preProcess(vehicle);
currentVehicle = vehicle;
vehicles.Add(vehicle);
speed.addRealTimePoint(mode, new Point(vehicle.SimulationTime, vehicle.Speed));
offset.addRealTimePoint(mode, new Point(vehicle.SimulationTime, vehicle.Offset));
accelerate.addRealTimePoint(mode, new Point(vehicle.SimulationTime, vehicle.Acceleration));
brake.addRealTimePoint(mode, new Point(vehicle.SimulationTime, vehicle.BrakePedal));
follow.addRealTimePoint(mode, new Point(vehicle.SimulationTime, vehicle.DistanceToNext));
setBrake(vehicle);
setReact(vehicle);
}
}
private void setTopAndBottom()
{
top = new SimulatedVehicle();
bottom = new SimulatedVehicle();
foreach (SimulatedVehicle vehicle in vehicles)
{
foreach (PropertyInfo p in vehicle.GetType().GetProperties())
{
if ((float)p.GetValue(vehicle) > (float)p.GetValue(top))
{
p.SetValue(top, p.GetValue(vehicle));
}
if ((float)p.GetValue(vehicle) < (float)p.GetValue(bottom))
{
p.SetValue(bottom, p.GetValue(vehicle));
}
}
}
}
private void setReact(SimulatedVehicle vehicle)
{
if (vehicle.Reacting > 0.000001)
{
if (!reacting)
{
reacting = true;
reactActivity.TimeStart = vehicle.SimulationTime;
}
reactActivity.ReactTime = (vehicle.SimulationTime - reactActivity.TimeStart) * 1000;
}
else
{
if (reacting)
{
reacting = false;
reactActivity.TimeEnd = vehicle.SimulationTime;
reactActivity.ReactTime = vehicle.SimulationTime - reactActivity.TimeStart;
}
}
}
private void setBrake(SimulatedVehicle vehicle)
{
if (vehicle.Braking > 0.000001)
{
if (!braking)
{
braking = true;
brakeActivity.BrakeStart = vehicle.TotalDistance;
}
brakeActivity.BrakeDistance = vehicle.TotalDistance - brakeActivity.BrakeStart;
}
else
{
if (braking)
{
braking = false;
brakeActivity.BrakeEnd = vehicle.TotalDistance;
brakeActivity.BrakeDistance = vehicle.TotalDistance - brakeActivity.BrakeStart;
}
}
}
private void setMarks()
{
marks = new List <DistractMark>();
float normalMark = 0;
float equal = 0.00001f;
for (int i = 0; i < vehicles.Count; i++)
{
if (Math.Abs(vehicles[i].Area - normalMark) >= equal)
{
DistractMark mark = new DistractMark();
mark.Start = vehicles[i];
mark.Top = new SimulatedVehicle();
mark.Bottom = new SimulatedVehicle();
while (++i < vehicles.Count - 1 && Math.Abs(vehicles[i].Area - normalMark) >= equal)
{
SimulatedVehicle vehicle = vehicles[i];
foreach (PropertyInfo p in vehicle.GetType().GetProperties())
{
if ((float)p.GetValue(vehicle) > (float)p.GetValue(mark.Top))
{
p.SetValue(mark.Top, p.GetValue(vehicle));
}
if ((float)p.GetValue(vehicle) < (float)p.GetValue(mark.Bottom))
{
p.SetValue(mark.Bottom, p.GetValue(vehicle));
}
}
}
mark.End = vehicles[i];
marks.Add(mark);
}
}
}
public void preProcess(SimulatedVehicle vehicle)
{
if (vehicle.Area < 0)
{
accident = 1;
}
if (PageList.Main.Selection.SceneSelect == UserSelections.SceneBrake ||
PageList.Main.Selection.SceneSelect == UserSelections.SceneLaneChange)
{
float initLane = 4;
vehicle.Offset += (vehicle.Lane - initLane) * 4;
}
if (vehicle.DistanceToNext > 2000)
{
float frontVehicleSpeed = (float)(70 / 3.6);
vehicle.DistanceToNext = currentVehicle.DistanceToNext
+ vehicle.TotalDistance - currentVehicle.TotalDistance
- frontVehicleSpeed * (vehicle.SimulationTime - currentVehicle.SimulationTime);
}
}
public void set(SimulatedVehicle vehicle)
{
if (vehicle != null)
{
if (!dataIn)
{
lastLane = (int)vehicle.Lane;
}
else
{
int laneToChange = (int)vehicle.Lane - lastLane + lane;
lane = laneToChange <= laneInVissim && laneToChange > 0 ? laneToChange : lane;
lastLane = (int)vehicle.Lane;
}
dataIn = true;
speed = vehicle.Speed;
if (vehicle.PositionX > 1206.141 && vehicle.PositionX <= 1736.125)
{
link = 1;
coord = 529.984 - (vehicle.PositionX - 1206.720);
}
if (vehicle.PositionX > 1190.628 && vehicle.PositionX <= 1206.141)
{
link = 10001;
coord = 15.00 - Math.Abs(vehicle.PositionX - 1190.628);
}
if (vehicle.PositionX > 1151.405 && vehicle.PositionX <= 1190.628)
{
link = 3;
coord = 39.323 - Math.Abs(vehicle.PositionX - 1151.405);
}
if (vehicle.PositionX < 1151.405)
{
link = 2;
}
signalState = (int)vehicle.TrafficLight;
}
}
/// <summary>
/// 传入一个字节数组，传出一个SimulatedVehicle对象
/// </summary>
/// <param name="buffer"></param>
/// <returns></returns>
SimulatedVehicle byteToSimulatedVehicle(byte[] buffer)
{
SimulatedVehicle s = new SimulatedVehicle();
s.SimulationTime = getFloat(buffer, UDPOffset.SimulationTime);
s.PositionX = getFloat(buffer, UDPOffset.PositionX);
s.PositionY = getFloat(buffer, UDPOffset.PositionY);
s.Speed = getFloat(buffer, UDPOffset.Speed);
s.Acceleration = getFloat(buffer, UDPOffset.Acceleration);
s.SteeringWheel = getFloat(buffer, UDPOffset.SteeringWheel);
s.Offset = getFloat(buffer, UDPOffset.Offset);
s.BrakePedal = getFloat(buffer, UDPOffset.BrakePedal);
s.TotalDistance = getFloat(buffer, UDPOffset.TotalDistance);
s.Braking = getFloat(buffer, UDPOffset.Braking);
s.Reacting = getFloat(buffer, UDPOffset.Reacting);
s.Area = getFloat(buffer, UDPOffset.Area);
s.DistanceToNext = getFloat(buffer, UDPOffset.DistanceToNext);
s.Lane = getFloat(buffer, UDPOffset.Lane);
s.TrafficLight = getFloat(buffer, UDPOffset.TrafficLight);
return(s);
}
// distance oriented
private void plotMarks()
{
if (currentScene == UserSelections.ScenePractice)
{
return;
}
SimulatedVehicle top = new SimulatedVehicle();
SimulatedVehicle bottom = new SimulatedVehicle();
int count = 0;
int index;
ExperienceUnit unit = null;
foreach (int mode in UserSelections.ModeList)
{
index = user.Index[UserSelections.getIndex(currentScene, mode)];
if (index != -1)
{
unit = user.Experiences[index];
foreach (PropertyInfo p in top.GetType().GetProperties())
{
if ((float)p.GetValue(unit.Top) > (float)p.GetValue(top))
{
p.SetValue(top, (float)p.GetValue(unit.Top));
}
if ((float)p.GetValue(unit.Bottom) < (float)p.GetValue(bottom))
{
p.SetValue(bottom, (float)p.GetValue(unit.Bottom));
}
}
++count;
}
}
if (count != 0)
{
foreach (DistractMark mark in unit.Marks)
{
float start = mark.Start.TotalDistance;
float end = mark.End.TotalDistance;
SpeedChart.AddAMarker(new List <Point>()
{
new Point(start, top.Speed),
new Point(start, bottom.Speed), new Point(end, bottom.Speed), new Point(end, top.Speed), new Point(start, top.Speed)
});
OffsetChart.AddAMarker(new List <Point>()
{
new Point(start, top.Offset),
new Point(start, bottom.Offset), new Point(end, bottom.Offset), new Point(end, top.Offset), new Point(start, top.Offset)
});
AccelerationChart.AddAMarker(new List <Point>()
{
new Point(start, top.Acceleration),
new Point(start, bottom.Acceleration), new Point(end, bottom.Acceleration), new Point(end, top.Acceleration), new Point(start, top.Acceleration)
});
BrakeChart.AddAMarker(new List <Point>()
{
new Point(start, top.BrakePedal),
new Point(start, bottom.BrakePedal), new Point(end, bottom.BrakePedal), new Point(end, top.BrakePedal), new Point(start, top.BrakePedal)
});
FollowChart.AddAMarker(new List <Point>()
{
new Point(start, top.DistanceToNext),
new Point(start, bottom.DistanceToNext), new Point(end, bottom.DistanceToNext), new Point(end, top.DistanceToNext), new Point(start, top.DistanceToNext)
});
}
}
if (showMark && axis == 1)
{
showMarkers();
}
else
{
hideMarkers();
}
}
FromFiles(string datafile, HistMode histmode,
double c, double p, double reftime)
{
string tmpdir = Util.TemporaryDir();
string datadir = Path.Combine(tmpdir, "data");
ZipFile.ExtractToDirectory(datafile, datadir);
string scenefile = Path.Combine(datadir, "scene.world");
string vehiclefile = Path.Combine(datadir, "trajectory.out");
string estimatefile = Path.Combine(datadir, "estimate.out");
string mapfile = Path.Combine(datadir, "maps.out");
string vismapfile = Path.Combine(datadir, "vismaps.out");
string tagfile = Path.Combine(datadir, "tags.out");
if (!File.Exists(scenefile))
{
scenefile = "";
}
if (!File.Exists(vismapfile))
{
vismapfile = "";
}
if (!File.Exists(tagfile))
{
tagfile = "";
}
TimedState trajectory;
TimedTrajectory estimate;
TimedMapModel map;
TimedMapModel vislandmarks;
Map landmarks = new Map(3);
TimedMessage tags;
PoseT dummyP = new PoseT();
trajectory = FP.TimedArrayFromDescriptor(File.ReadAllLines(vehiclefile), dummyP.StateSize);
estimate = FP.TrajectoryHistoryFromDescriptor(File.ReadAllText(estimatefile), dummyP.StateSize);
map = FP.MapHistoryFromDescriptor(File.ReadAllText(mapfile), 3);
if (!string.IsNullOrEmpty(vismapfile))
{
vislandmarks = FP.MapHistoryFromDescriptor(File.ReadAllText(vismapfile), 3);
for (int i = vislandmarks.Count; i < map.Count; i++)
{
vislandmarks.Add(Tuple.Create(map[i].Item1, new Map(3)));
}
}
else
{
vislandmarks = new TimedMapModel();
foreach (var entry in map)
{
vislandmarks.Add(Tuple.Create(entry.Item1, new Map(3)));
}
}
double[][] did = { new double[3] {
1e-3, 0, 0
},
new double[3] {
0, 1e-3, 0
},
new double[3] {
0, 0, 1e-3
} };
if (!string.IsNullOrEmpty(scenefile))
{
var explorer = SimulatedVehicle <MeasurerT, PoseT, MeasurementT> .
FromFile(File.ReadAllText(scenefile));
foreach (var landmark in explorer.Landmarks)
{
landmarks.Add(new Gaussian(landmark, did, 1.0));
}
}
if (!string.IsNullOrEmpty(tagfile))
{
tags = FP.TimedMessageFromDescriptor(File.ReadAllLines(tagfile));
}
else
{
tags = new TimedMessage();
}
bool online = (estimate[0].Item2.Count < estimate[estimate.Count - 1].Item2.Count &&
estimate.Count == trajectory.Count);
if (!online && histmode != HistMode.Timed)
{
Console.WriteLine("Warning: Offline mode is not compatible with the selected history mode. " +
"Switching to time-average.");
histmode = HistMode.Timed;
}
return(new Plot <MeasurerT, PoseT, MeasurementT>(trajectory, estimate, map, vislandmarks,
landmarks, c, p, reftime, histmode, tags, online));
}
