/// <summary>
/// Check if two id's are equal
/// </summary>
/// <param name="obj"></param>
/// <returns></returns>
public override bool Equals(object obj)
{
if (obj is SimVehicleId)
{
SimVehicleId other = (SimVehicleId)obj;
return(this.number == other.Number);
}
else
{
return(false);
}
}
/// <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);
}
/// <summary> /// Sets the vehicle in the client /// </summary> /// <param name="vehicleId"></param> public abstract void SetVehicle(SimVehicleId vehicleId);
/// <summary>
/// Remove a client
/// </summary>
/// <param name="vehicleId"></param>
/// <returns></returns>
public bool Remove(SimVehicleId vehicleId)
{
// check to see if this vehicle is associated with a client
if (VehicleToClientMap.ContainsKey(vehicleId))
{
// get name of client vehicle associated with
string client = VehicleToClientMap[vehicleId];
try
{
// remove vehicle from client to vehicle map
this.ClientToVehicleMap.Remove(client);
// remove client from vehicle to client map
this.VehicleToClientMap.Remove(vehicleId);
// remove vehicle from client
this.AvailableClients[client].SetVehicle(null);
// notify success
SimulatorOutput.WriteLine("Successfully removed vehicle: " + vehicleId.ToString() + " from clients");
// return success
return true;
}
catch (Exception e)
{
// there was an error
SimulatorOutput.WriteLine("Error removing vehicle: " + vehicleId.ToString() + " from clients: \n" + e.ToString());
// cleanup vehicle to client map
if (this.VehicleToClientMap.ContainsKey(vehicleId))
this.VehicleToClientMap.Remove(vehicleId);
// cleanup client to vehicle map
if (this.ClientToVehicleMap.ContainsKey(client))
this.ClientToVehicleMap.Remove(client);
// return unsuccessful
return false;
}
}
else
{
return true;
}
}
/// <summary>
/// Associate the vehicle with a enw client
/// </summary>
/// <param name="vehicleId"></param>
/// <returns></returns>
public bool ReBind(SimVehicleId vehicleId)
{
// make sure that the vehicle is not already associated with a client
if (!VehicleToClientMap.ContainsKey(vehicleId))
{
// loop over all clients looking for an open client
foreach (string s in this.AvailableClients.Keys)
{
// make sure the client is not associated with a vehicle
if (!ClientToVehicleMap.ContainsKey(s))
{
try
{
// get the client
SimulatorClientFacade scf = this.AvailableClients[s];
// set the vehicle in the client
scf.SetVehicle(vehicleId);
// set vehicle as having client
VehicleToClientMap.Add(vehicleId, s);
// set client as having vehicle
ClientToVehicleMap.Add(s, vehicleId);
// success
SimulatorOutput.WriteLine("Vehicle: " + vehicleId.ToString() + " Bound to client: " + s);
// return that the rebind was successful
return true;
}
catch (Exception e)
{
// there was an error
SimulatorOutput.WriteLine("Error binding vehicle: " + vehicleId.ToString() + " to client: " + s + "\n" + e.ToString());
// cleanup vehicle to client map
if (this.VehicleToClientMap.ContainsKey(vehicleId))
this.VehicleToClientMap.Remove(vehicleId);
// cleanup client to vehicle map
if (this.ClientToVehicleMap.ContainsKey(s))
this.ClientToVehicleMap.Remove(s);
// return that the attempt was unsuccessful
return false;
}
}
}
// success
SimulatorOutput.WriteLine("Vehicle: " + vehicleId.ToString() + " Could not be bound to any clients, lack of availability");
// notify that we never got to this point
return false;
}
else
return true;
}
/// <summary>
/// Get the vehicles from the world and put them into sensors form
/// </summary>
/// <param name="ours"></param>
/// <returns></returns>
public SceneEstimatorTrackedClusterCollection VehiclesFromWorld(SimVehicleId ours, double ts)
{
// vehicle list
List<SceneEstimatorTrackedCluster> vehicles = new List<SceneEstimatorTrackedCluster>();
// get our vehicle
SimVehicleState ourVS = null;
foreach (SimVehicleState svs in this.worldState.Vehicles.Values)
{
if (svs.VehicleID.Equals(ours))
{
ourVS = svs;
}
}
// generate "tracked" clusters
foreach (SimVehicleState svs in this.worldState.Vehicles.Values)
{
// don't inclue our vehicle
if (!svs.VehicleID.Equals(ours))
{
// construct cluster
SceneEstimatorTrackedCluster setc = new SceneEstimatorTrackedCluster();
// set heading valid
setc.headingValid = true;
// closest point
Coordinates closestPoint = this.ClosestToPolygon(this.VehiclePolygon(svs), ourVS.Position);
setc.closestPoint = closestPoint;
// stopped
bool isStopped = Math.Abs(svs.Speed) < 0.01;
setc.isStopped = isStopped;
// speed
float speed = (float)Math.Abs(svs.Speed);
setc.speed = speed;
setc.speedValid = svs.SpeedValid;
// absolute heading
float absHeading = (float)(svs.Heading.ArcTan);
setc.absoluteHeading = absHeading;
// relative heading
float relHeading = absHeading - (float)(ourVS.Heading.ArcTan);
setc.relativeheading = relHeading;
// set target class
setc.targetClass = SceneEstimatorTargetClass.TARGET_CLASS_CARLIKE;
// set id
setc.id = svs.VehicleID.Number;
// cluster partitions
SceneEstimatorClusterPartition[] partitions = this.GetClusterPartitions(svs, ourVS);
setc.closestPartitions = partitions;
// state
setc.statusFlag = SceneEstimatorTargetStatusFlag.TARGET_STATE_ACTIVE;
// raw points
Coordinates[] points = this.VehiclePointsRelative(svs, ourVS.Position, ourVS.Heading);
setc.relativePoints = points;
// add
vehicles.Add(setc);
}
}
// array of clusters
SceneEstimatorTrackedClusterCollection setcc = new SceneEstimatorTrackedClusterCollection();
setcc.clusters = vehicles.ToArray();
setcc.timestamp = ts;
// return the clusters
return setcc;
}
/// <summary>
/// Turn sim obstacle states into obstacles
/// </summary>
/// <returns></returns>
public SceneEstimatorUntrackedClusterCollection ObstaclesFromWorld(SimVehicleId ours, Coordinates vehiclePosition, double vehicleHeading, double ts)
{
SceneEstimatorUntrackedClusterCollection seucc = new SceneEstimatorUntrackedClusterCollection();
seucc.timestamp = (double)SimulatorClient.GetCurrentTimestamp;
List<Polygon> obstaclePolygons = new List<Polygon>(worldState.Obstacles.Count);
foreach (SimObstacleState obstacle in worldState.Obstacles.Values) {
if(obstacle.ObstacleId.Number != ours.Number)
obstaclePolygons.Add(obstacle.ToPolygon());
}
// use the lidar to get the hits
lidar.GetHits(obstaclePolygons, vehiclePosition, vehicleHeading, seucc);
lidar2.GetHits(obstaclePolygons, vehiclePosition, vehicleHeading, seucc);
return seucc;
}
/// <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;
}
/// <summary>
/// Sets the vehicle
/// </summary>
/// <param name="vehicleId"></param>
public override void SetVehicle(SimVehicleId vehicleId)
{
if (vehicleId != null)
{
this.ClientVehicle = new ClientVehicle();
this.ClientVehicle.VehicleId = vehicleId;
Console.WriteLine(DateTime.Now.ToString() + ": Registered Vehicle with Vehicle Id: " + vehicleId.ToString());
}
else
{
if(this.ClientVehicle != null)
Console.WriteLine(DateTime.Now.ToString() + ": Deregistered Vehicle with Vehicle Id: " + this.ClientVehicle.VehicleId.ToString());
this.ClientVehicle = null;
}
}
