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