/// <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; } }