/// <summary>
/// Gets the world state
/// </summary>
/// <returns></returns>
public WorldState GetWorldState()
{
Dictionary<SimVehicleId, SimVehicleState> vehicleStates = new Dictionary<SimVehicleId,SimVehicleState>();
Dictionary<SimObstacleId, SimObstacleState> obstacleStates = new Dictionary<SimObstacleId,SimObstacleState>();
foreach (SimVehicle sv in this.simEngine.Vehicles.Values)
{
vehicleStates.Add(sv.SimVehicleState.VehicleID, sv.SimVehicleState);
/*SimObstacleState sos = new SimObstacleState();
sos.Heading = sv.Heading.Normalize();
sos.IsBlockage = false;
sos.Length = sv.Length;
sos.ObstacleId = new SimObstacleId(sv.VehicleId.Number);
sos.Position = sv.Position + sv.Heading.Normalize(TahoeParams.FL - (sv.Length / 2.0));
sos.Width = sv.Width;
obstacleStates.Add(sos.ObstacleId, sos);*/
}
foreach (SimObstacle so in this.Obstacles.Values)
{
SimObstacleState sos = new SimObstacleState();
sos.Heading = so.Heading.Normalize();
sos.IsBlockage = so.Blockage;
sos.Length = so.Length;
sos.ObstacleId = so.ObstacleId;
sos.Position = so.Position;
sos.Width = so.Width;
obstacleStates.Add(sos.ObstacleId, sos);
}
return new WorldState(vehicleStates, obstacleStates);
}
static void Main(string[] args)
{
SimSensor sensor = new SimSensor(Math.PI/4, -Math.PI/4, 1*Math.PI/180, SensorType.Scan, 30);
SimObstacleState obsState = new SimObstacleState();
obsState.Position = new Coordinates(5, 5);
obsState.Length = 4;
obsState.Width = 2;
obsState.Heading = new Coordinates(1, 0);
Polygon[] poly = new Polygon[] { obsState.ToPolygon() };
SceneEstimatorUntrackedClusterCollection clusters = new SceneEstimatorUntrackedClusterCollection();
sensor.GetHits(poly, Coordinates.Zero, 0, clusters);
SimulatorClient client = new SimulatorClient();
client.BeginClient();
}
