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