/// <summary>
/// Distance to the next stop line
/// </summary>
/// <param name="simVehicleState"></param>
/// <returns></returns>
public double? DistanceToNextStop(SimVehicleState simVehicleState)
{
if (this.RoadNetwork == null)
return null;
else
{
IAreaSubtypeId iasi = this.GetAreaId(simVehicleState);
if (iasi == null)
return null;
else if (iasi is ArbiterPerimeterId)
return null;
else
{
ArbiterLaneId ali = (ArbiterLaneId)iasi;
// get closest
ArbiterLanePartition alp = this.RoadNetwork.ArbiterSegments[ali.SegmentId].Lanes[ali].GetClosestPartition(simVehicleState.Position);
ArbiterWaypoint waypoint;
double distance;
RoadNetworkTools.NextStop(alp.Lane, alp, simVehicleState.Position, out waypoint, out distance);
if (waypoint == null)
return null;
else
return distance;
}
}
}
private double throttle; // throttle (between 0 and 1), the fraction of maximum torque commanded
#endregion Fields
#region Constructors
public DynamicsSimVehicle(SimVehicleState vehicleState, ObjectDirectory od)
{
this.simVehicleState = vehicleState;
InitializeState();
Connect(od);
}
/// <summary>
/// Update the vehicle
/// </summary>
/// <param name="worldState"></param>
public void Update(WorldState worldState)
{
// update the current state held in the sim
this.CurrentState = worldState.Vehicles[VehicleId];
}
/// <summary>
/// COnstructor
/// </summary>
/// <param name="state"></param>
/// <param name="length"></param>
/// <param name="width"></param>
public SimVehicle(SimVehicleState state, double length, double width)
{
this.VehicleState = state;
this.VehicleState.Length = length;
this.VehicleState.Width = width;
}
/// <summary>
/// COnstructor
/// </summary>
/// <param name="state"></param>
/// <param name="length"></param>
/// <param name="width"></param>
public SimVehicle(SimVehicleState state, double length, double width)
{
this.VehicleState = state;
this.VehicleState.Length = length;
this.VehicleState.Width = width;
}
private SceneEstimatorClusterPartition[] GetClusterPartitions(SimVehicleState svs, SimVehicleState ours)
{
List<SceneEstimatorClusterPartition> partitions = new List<SceneEstimatorClusterPartition>();
foreach(IVehicleArea iva in this.RoadNetwork.VehicleAreas)
{
if (ours.Position.DistanceTo(svs.Position) < 55)
{
if (iva.ContainsVehicle(svs.Position, svs.Length, svs.Width, svs.Heading))
{
SceneEstimatorClusterPartition secp = new SceneEstimatorClusterPartition();
secp.partitionID = iva.DefaultAreaId();
secp.probability = 1;
partitions.Add(secp);
}
}
else
{
if (iva.ContainsVehicle(svs.Position, svs.Length, svs.Width * 4.0, svs.Heading))
{
SceneEstimatorClusterPartition secp = new SceneEstimatorClusterPartition();
secp.partitionID = iva.DefaultAreaId();
secp.probability = 1;
partitions.Add(secp);
}
}
}
return partitions.ToArray();
}
/// <summary>
/// Make a vehicle state from a sim vehicle state
/// </summary>
/// <param name="simState"></param>
/// <returns></returns>
public VehicleState VehicleStateFromSim(SimVehicleState simState)
{
// vehicle state
VehicleState vs = new VehicleState();
// area
vs.Area = new List<AreaEstimate>();
// ae
AreaEstimate ae = new AreaEstimate();
ae.Probability = 1;
// get string id
string id = "";
// set id
ae.AreaId = id;
ae.AreaType = StateAreaType.Interconnect;
// get area
IAreaSubtypeId iasi = this.GetAreaId(simState);
if (iasi is ArbiterPerimeterId)
{
id = ((ArbiterPerimeterId)iasi).ToString();
ae.AreaType = StateAreaType.Zone;
}
else if (iasi is ArbiterLaneId)
{
ae.AreaId = ((ArbiterLaneId)iasi).ToString() + ".1";
ae.AreaType = StateAreaType.Lane;
}
// add ae
vs.Area.Add(ae);
// set others
vs.Heading = simState.Heading;
vs.Position = simState.Position;
// return
return vs;
}
/// <summary>
/// Gets area id of a vehicle's integer area id
/// </summary>
/// <param name="vehicleState"></param>
/// <returns></returns>
public IAreaSubtypeId GetAreaId(SimVehicleState vehicleState)
{
return RoadNetworkTools.GetClosest(this.RoadNetwork, vehicleState.Position);
}
public Coordinates[] VehiclePointsRelative(SimVehicleState svs, Coordinates r, Coordinates h)
{
Coordinates[] cs = this.VehiclePoints(svs);
for (int i = 0; i < cs.Length; i++)
{
cs[i] = cs[i] - r;
cs[i] = cs[i].Rotate(-h.ArcTan);
}
return cs;
}
public Polygon VehiclePolygon(SimVehicleState sos)
{
Coordinates headingOffset = sos.Heading.Rotate90();
Coordinates center = sos.Position + sos.Heading.Normalize(TahoeParams.FL - (sos.Length / 2.0));
List<Coordinates> coords = new List<Coordinates>();
coords.Add(center + sos.Heading.Normalize(sos.Length / 2.0) + headingOffset.Normalize(sos.Width / 2.0));
coords.Add(center + sos.Heading.Normalize(sos.Length / 2.0) - headingOffset.Normalize(sos.Width / 2.0));
coords.Add(center - sos.Heading.Normalize(sos.Length / 2.0) + headingOffset.Normalize(sos.Width / 2.0));
coords.Add(center - sos.Heading.Normalize(sos.Length / 2.0) - headingOffset.Normalize(sos.Width / 2.0));
Polygon p = new Polygon(coords, CoordinateMode.AbsoluteProjected);
return p;
}
public Coordinates[] VehiclePoints(SimVehicleState svs)
{
Polygon p = VehiclePolygon(svs);
LinePath lp = new LinePath(p);
LinePath.PointOnPath c = lp.StartPoint;
List<Coordinates> cs = new List<Coordinates>();
while (!c.Equals(lp.EndPoint))
{
cs.Add(lp.GetPoint(c));
c = lp.AdvancePoint(c, 0.1);
}
return cs.ToArray();
}
public PathRoadModel GetPathRoadEstimate(SimVehicleState state)
{
PathRoadModel ret = new PathRoadModel(new List<PathRoadModel.LaneEstimate>(), (double)SimulatorClient.GetCurrentTimestamp);
ArbiterLanePartition closestPartition = this.RoadNetwork.ClosestLane(state.Position).GetClosestPartition(state.Position);
LinePath path;
if (IsPartitionSameDirection(closestPartition, state.Heading)) {
path = BuildPath(closestPartition, true, 10, 40);
}
else {
path = BuildPath(closestPartition, false, 10, 40);
}
Matrix3 absTransform = Matrix3.Rotation(-state.Heading.ArcTan)*Matrix3.Translation(-state.Position.X, -state.Position.Y);
path.TransformInPlace(absTransform);
PathRoadModel.LaneEstimate center = new PathRoadModel.LaneEstimate(path, closestPartition.Lane.Width, closestPartition.PartitionId.ToString());
ret.laneEstimates.Add(center);
// get the lane to the left
if (closestPartition.Lane.LaneOnLeft != null) {
ArbiterLanePartition partition = closestPartition.Lane.LaneOnLeft.GetClosestPartition(state.Position);
if (closestPartition.NonLaneAdjacentPartitions.Contains(partition)) {
if (IsPartitionSameDirection(partition, state.Heading)) {
path = BuildPath(partition, true, 10, 25);
}
else {
path = BuildPath(partition, false, 10, 25);
}
path.TransformInPlace(absTransform);
PathRoadModel.LaneEstimate left = new PathRoadModel.LaneEstimate(path, partition.Lane.Width, partition.PartitionId.ToString());
ret.laneEstimates.Add(left);
}
}
if (closestPartition.Lane.LaneOnRight != null) {
ArbiterLanePartition partition = closestPartition.Lane.LaneOnRight.GetClosestPartition(state.Position);
if (closestPartition.NonLaneAdjacentPartitions.Contains(partition)) {
if (IsPartitionSameDirection(partition, state.Heading)) {
path = BuildPath(partition, true, 10, 25);
}
else {
path = BuildPath(partition, false, 10, 25);
}
path.TransformInPlace(absTransform);
PathRoadModel.LaneEstimate right = new PathRoadModel.LaneEstimate(path, partition.Lane.Width, partition.PartitionId.ToString());
ret.laneEstimates.Add(right);
}
}
return ret;
}
public LocalRoadEstimate GetLocalRoadEstimate(SimVehicleState state)
{
LocalRoadEstimate lre = new LocalRoadEstimate();
/*ArbiterLanePartition closestPartition = this.RoadNetwork.ClosestLane(state.Position).GetClosestPartition(state.Position);
LinePath path;
if (IsPartitionSameDirection(closestPartition, state.Heading)) {
path = BuildPath(closestPartition, true, 25, 25);
}
else {
path = BuildPath(closestPartition, false, 25, 25);
}
Matrix3 absTransform = Matrix3.Rotation(-state.Heading.ArcTan)*Matrix3.Translation(-state.Position.X, -state.Position.Y);
path.TransformInPlace(absTransform);
// get the closest point
LinePath.PointOnPath p0 = path.ZeroPoint;
List<Coordinates> points = new List<Coordinates>(5);
List<double> dists = new List<double>();
dists.Add(0);
points.Add(path.GetPoint(p0));
// iterate through and add points every five meteres
LinePath.PointOnPath pt = p0;
double sumDist = 0;
for (int i = 0; i < 3; i++) {
double dist = 5;
pt = path.AdvancePoint(pt, ref dist);
if (dist != 5) {
sumDist += (5-dist);
dists.Add(sumDist);
points.Add(path.GetPoint(pt));
}
if (dist > 0)
break;
}
pt = p0;
sumDist = 0;
for (int i = 0; i < 2; i++) {
double dist = -5;
pt = path.AdvancePoint(pt, ref dist);
if (dist != -5) {
sumDist += (5+dist);
dists.Insert(0, sumDist);
points.Insert(0, path.GetPoint(pt));
}
if (dist < 0)
break;
}
// do a least squares fit over the points
Matrix X = new Matrix(points.Count, 3);
Matrix W = new Matrix(points.Count, points.Count);
Matrix Y = new Matrix(points.Count, 1);
for (int i = 0; i < points.Count; i++) {
X[i, 0] = 1;
X[i, 1] = points[i].X;
X[i, 2] = points[i].X*points[i].X;
W[i, i] = 1/(dists[i] + 1);
Y[i, 0] = points[i].Y;
}
Matrix beta = null;
try {
Matrix X_T = X.Transpose();
Matrix inv = (X_T*W*X).Inverse;
beta = inv*X_T*W*Y;
}
catch (Exception) {
}
if (beta != null) {
lre.isModelValid = true;
lre.roadCurvature = beta[2, 0];
lre.roadCurvatureVar = double.MaxValue;
lre.roadHeading = beta[1, 0];
lre.roadHeadingVar = 0;
lre.centerLaneEstimate = new UrbanChallenge.Common.Sensors.LocalRoadEstimate.LaneEstimate();
lre.centerLaneEstimate.center = beta[0, 0];
lre.centerLaneEstimate.centerVar = 0;
lre.centerLaneEstimate.exists = true;
lre.centerLaneEstimate.id = closestPartition.PartitionId.ToString();
lre.centerLaneEstimate.width = closestPartition.Lane.Width;
lre.centerLaneEstimate.widthVar = 0;
lre.leftLaneEstimate = new UrbanChallenge.Common.Sensors.LocalRoadEstimate.LaneEstimate();
lre.leftLaneEstimate.exists = false;
if (closestPartition.Lane.LaneOnLeft != null) {
ArbiterLanePartition leftParition = closestPartition.Lane.LaneOnLeft.GetClosestPartition(state.Position);
if (closestPartition.NonLaneAdjacentPartitions.Contains(leftParition)) {
double dist;
if (IsPartitionSameDirection(leftParition, state.Heading)) {
dist = (state.Position-leftParition.Initial.Position).Cross(leftParition.Vector().Normalize());
}
else {
dist = (state.Position-leftParition.Final.Position).Cross(leftParition.Vector().Rotate180().Normalize());
}
lre.leftLaneEstimate.center = dist;
lre.leftLaneEstimate.centerVar = 0;
lre.leftLaneEstimate.exists = true;
lre.leftLaneEstimate.id = leftParition.PartitionId.ToString();
lre.leftLaneEstimate.width = leftParition.Lane.Width;
lre.leftLaneEstimate.widthVar = 0;
}
}
if (closestPartition.Lane.LaneOnRight != null) {
ArbiterLanePartition rightPartition = closestPartition.Lane.LaneOnRight.GetClosestPartition(state.Position);
if (closestPartition.NonLaneAdjacentPartitions.Contains(rightPartition)) {
double dist;
if (IsPartitionSameDirection(rightPartition, state.Heading)) {
dist = (state.Position-rightPartition.Initial.Position).Cross(rightPartition.Vector().Normalize());
}
else {
dist = (state.Position-rightPartition.Final.Position).Cross(rightPartition.Vector().Rotate180().Normalize());
}
lre.rightLaneEstimate.center = dist;
lre.rightLaneEstimate.centerVar = 0;
lre.rightLaneEstimate.exists = true;
lre.rightLaneEstimate.id = rightPartition.PartitionId.ToString();
lre.rightLaneEstimate.width = rightPartition.Lane.Width;
lre.rightLaneEstimate.widthVar = 0;
}
}
}
else {
lre.isModelValid = false;
}*/
lre.centerLaneEstimate.exists = false;
lre.leftLaneEstimate.exists = false;
lre.rightLaneEstimate.exists = false;
lre.stopLineEstimate = new StopLineEstimate();
double? distToStopline = DistanceToNextStop(state);
lre.stopLineEstimate.stopLineExists = distToStopline.HasValue;
lre.stopLineEstimate.distToStopline = distToStopline.GetValueOrDefault(double.MaxValue);
lre.stopLineEstimate.distToStoplineVar = 0;
lre.timestamp = (double)SimulatorClient.GetCurrentTimestamp;
return lre;
}
/// <summary>
/// Adds a vehicle to the world
/// </summary>
/// <param name="screenCenter"></param>
/// <returns></returns>
public SimVehicle AddVehicle(Coordinates screenCenter)
{
// deafualt id is # of elts
int idNumber = this.WorldService.Obstacles.Count + this.Vehicles.Count;
// create array to hold used vehicle and obstacle id's
bool[] usedIds = new bool[this.WorldService.Obstacles.Count + this.Vehicles.Count];
// loop over obstacles and get ids
foreach (SimObstacle ism in this.WorldService.Obstacles.Values)
{
// make sure num within # of obstacles and vehicles
if (ism.ObstacleId.Number < this.WorldService.Obstacles.Count + this.Vehicles.Count)
{
// check off
usedIds[ism.ObstacleId.Number] = true;
}
}
// loop over vehicles and get ids
foreach (SimVehicle ism in this.Vehicles.Values)
{
// make sure num within # of obstacles and vehicles
if (ism.VehicleId.Number < this.WorldService.Obstacles.Count + this.Vehicles.Count)
{
// check off
usedIds[ism.VehicleId.Number] = true;
}
}
// loop over checked off id's
for (int i = usedIds.Length - 1; i >= 0; i--)
{
// if find a false one set that id
if (usedIds[i] == false)
{
// set id num
idNumber = i;
}
}
// create vehicle id
SimVehicleId svi = new SimVehicleId(idNumber);
// get position (center of screen)
Coordinates position = screenCenter;
// create state
SimVehicleState svs = new SimVehicleState();
svs.canMove = true;
svs.Heading = new Coordinates(1, 0);
svs.IsBound = false;
svs.Position = position;
svs.Speed = 0;
svs.VehicleID = svi;
// create vehicle
SimVehicle stv = new SimVehicle(svs, TahoeParams.VL, TahoeParams.T);
// add to vehicles
this.Vehicles.Add(stv.VehicleId, stv);
// return
return(stv);
}
private AreaEstimate[] GetAreaEstimates(SimVehicleState svs)
{
List<AreaEstimate> estimates = new List<AreaEstimate>();
foreach (IVehicleArea iva in this.RoadNetwork.VehicleAreas)
{
if (iva.ContainsVehicle(svs.Position, svs.Length, svs.Width, svs.Heading))
{
AreaEstimate ae = new AreaEstimate();
ae.AreaId = iva.DefaultAreaId();
if(iva is ArbiterInterconnect)
ae.AreaType = StateAreaType.Interconnect;
else if(iva is ArbiterLane)
ae.AreaType = StateAreaType.Lane;
else
ae.AreaType = StateAreaType.Zone;
ae.Probability = 1;
estimates.Add(ae);
}
}
return estimates.ToArray();
}
/// <summary>
/// The simulation main thread
/// </summary>
public void Simulate()
{
// run sim at 10Hz
MMWaitableTimer timer = new MMWaitableTimer((uint)this.settings.SimCycleTime);
// notify
this.simulationMain.SimulationModeLabel.Text = "Simulation Running";
this.simulationMain.SimulationModeLabel.Image = global::Simulator.Properties.Resources.Light_Bulb_On_16_n_p;
// run while on
while (this.SimulationState == SimulationState.Running)
{
if (stepMode)
{
// if we're in step mode, then wait on the step event or time out
bool gotEvent = stepEvent.WaitOne(250, false);
// if we timed out, start the loop over
if (!gotEvent)
{
continue;
}
}
else
{
// wait for 10hz
timer.WaitEvent.WaitOne();
}
try
{
// get world state
WorldState ws = this.WorldService.GetWorldState();
lock (this.simulationMain.clientHandler)
{
// set world state to each client in the sim
foreach (string client in this.simulationMain.clientHandler.VehicleToClientMap.Values)
{
try
{
// update client
SimVehicleState nextState = this.simulationMain.clientHandler.AvailableClients[client].Update(ws, (double)this.settings.SimCycleTime / 1000.0);
if (nextState != null)
{
// update state
this.Vehicles[this.simulationMain.clientHandler.ClientToVehicleMap[client]].SimVehicleState = nextState;
}
else
{
throw new Exception("Received null SimVehicleState from " + client);
}
}
catch (Exception e)
{
if (!this.simulationMain.IsDisposed)
{
this.simulationMain.BeginInvoke(new MethodInvoker(delegate()
{
// notify
SimulatorOutput.WriteLine("Error Updating Client: " + client + ", Simulation Stopped");
// set state
this.simulationMain.SimulationModeLabel.Text = "Simulation Stopped";
this.simulationMain.SimulationModeLabel.Image = global::Simulator.Properties.Resources.Light_Bulb_Off_16_n_p;
// stop
this.EndSimulation();
}));
Console.WriteLine(e.ToString());
// leave
break;
}
}
}
}
// redraw
this.simulationMain.BeginInvoke(new MethodInvoker(delegate()
{
// notify
this.simulationMain.roadDisplay1.Invalidate();
}));
}
catch (Exception e)
{
if (!this.simulationMain.IsDisposed)
{
this.simulationMain.BeginInvoke(new MethodInvoker(delegate()
{
// notify
SimulatorOutput.WriteLine("Error in outer sim loop:" + e.ToString());
}));
}
}
}
}
/// <summary>
/// Adds a vehicle to the world
/// </summary>
/// <param name="screenCenter"></param>
/// <returns></returns>
public SimVehicle AddVehicle(Coordinates screenCenter)
{
// deafualt id is # of elts
int idNumber = this.WorldService.Obstacles.Count + this.Vehicles.Count;
// create array to hold used vehicle and obstacle id's
bool[] usedIds = new bool[this.WorldService.Obstacles.Count + this.Vehicles.Count];
// loop over obstacles and get ids
foreach (SimObstacle ism in this.WorldService.Obstacles.Values)
{
// make sure num within # of obstacles and vehicles
if (ism.ObstacleId.Number < this.WorldService.Obstacles.Count + this.Vehicles.Count)
{
// check off
usedIds[ism.ObstacleId.Number] = true;
}
}
// loop over vehicles and get ids
foreach (SimVehicle ism in this.Vehicles.Values)
{
// make sure num within # of obstacles and vehicles
if (ism.VehicleId.Number < this.WorldService.Obstacles.Count + this.Vehicles.Count)
{
// check off
usedIds[ism.VehicleId.Number] = true;
}
}
// loop over checked off id's
for (int i = usedIds.Length - 1; i >= 0; i--)
{
// if find a false one set that id
if (usedIds[i] == false)
{
// set id num
idNumber = i;
}
}
// create vehicle id
SimVehicleId svi = new SimVehicleId(idNumber);
// get position (center of screen)
Coordinates position = screenCenter;
// create state
SimVehicleState svs = new SimVehicleState();
svs.canMove = true;
svs.Heading = new Coordinates(1,0);
svs.IsBound = false;
svs.Position = position;
svs.Speed = 0;
svs.VehicleID = svi;
// create vehicle
SimVehicle stv = new SimVehicle(svs, TahoeParams.VL, TahoeParams.T);
// add to vehicles
this.Vehicles.Add(stv.VehicleId, stv);
// return
return stv;
}
