/// <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>
/// 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;
}
}
/// <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> /// Sets the vehicle in the client /// </summary> /// <param name="vehicleId"></param> public abstract void SetVehicle(SimVehicleId vehicleId);
/// <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>
/// Removes a client
/// </summary>
/// <param name="client"></param>
/// <returns></returns>
public bool Remove(string client)
{
// check if the client is associated with a vehicle
if (this.ClientToVehicleMap.ContainsKey(client))
{
// get vehicle associated with teh client
SimVehicleId svi = this.ClientToVehicleMap[client];
try
{
// remove vehicle from client map
if (this.ClientToVehicleMap.ContainsKey(client))
{
this.ClientToVehicleMap.Remove(client);
}
// remove client from vehicle map
if (this.VehicleToClientMap.ContainsKey(svi))
{
this.VehicleToClientMap.Remove(svi);
}
// lock the clients
lock (((ICollection)this.AvailableClients).SyncRoot)
{
// kill the client
this.AvailableClients[client].Kill();
// remove the client from the available clients
this.AvailableClients.Remove(client);
}
}
catch (Exception e)
{
// lock the clients
lock (((ICollection)this.AvailableClients).SyncRoot)
{
// remove the client from available
if (this.AvailableClients.ContainsKey(client))
{
this.AvailableClients.Remove(client);
}
}
Console.WriteLine(e.ToString());
}
// notify success
SimulatorOutput.WriteLine("Successfully removed client: " + client);
// rebind teh vehicle associated with the client
return(this.ReBind(svi));
}
else
{
// lock the clients
lock (((ICollection)this.AvailableClients).SyncRoot)
{
try
{
// kill the client
this.AvailableClients[client].Kill();
// remove the client from the available clients
this.AvailableClients.Remove(client);
}
catch (Exception e)
{
// remove the client from available
if (this.AvailableClients.ContainsKey(client))
{
this.AvailableClients.Remove(client);
}
Console.WriteLine(e.ToString());
}
}
// notify success
SimulatorOutput.WriteLine("Successfully removed client: " + client);
// return success
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);
}