private IEnumerator<ITask> InitializeGui() { var runWindow = this.wpfServicePort.RunWindow(() => new libguiwpf.MainWindow(_state.followDirectionPidControllerAngularSpeed, _state.followDirectionPidControllerLinearSpeed, SoundSkinFactory.soundsBasePathDefault)); yield return (Choice)runWindow; var exception = (Exception)runWindow; if (exception != null) { LogError(exception); StartFailed(); yield break; } // need double cast because WPF adapter doesn't know about derived window types var userInterface = ((Window)runWindow) as libguiwpf.MainWindow; if (userInterface == null) { var e = new ApplicationException("User interface was expected to be libguiwpf.MainWindow"); LogError(e); throw e; } _mainWindow = userInterface; _mainWindow.Closing += new CancelEventHandler(_mainWindow_Closing); _mainWindow.PowerScaleAdjusted += delegate { PowerScale = _mainWindow.PowerScale; Tracer.Trace("PowerScaleAdjusted: " + PowerScale); }; _mainWindow.PidControllersUpdated += delegate { Tracer.Trace("PidControllersUpdated - saving state"); SaveState(_state); }; // for convenience mark the initial robot position: DetectedObstacle dobst1 = new DetectedObstacle() { geoPosition = (GeoPosition)_mapperVicinity.robotPosition.Clone(), firstSeen = DateTime.Now.Ticks, lastSeen = DateTime.Now.Ticks, detectorType = DetectorType.NONE, objectType = DetectedObjectType.Mark, timeToLiveSeconds = 3600 }; dobst1.SetColorByType(); lock (_mapperVicinity) { _mapperVicinity.Add(dobst1); } _mainWindow.setMapper(_mapperVicinity, _routePlanner); _mainWindow.PowerScale = PowerScale; _soundsHelper = new SoundsHelper(_mainWindow); }
protected void updateMapperWithLaserData(sicklrf.State laserData) { int numRays = laserData.DistanceMeasurements.Length; List<IDetectedObject> laserObjects = new List<IDetectedObject>(numRays / step + 5); for (int i = 0; i < laserData.DistanceMeasurements.Length; i += step) { double rangeMeters = laserData.DistanceMeasurements[i] / 1000.0d; // DistanceMeasurements is in millimeters; if (rangeMeters > minReliableRangeMeters && rangeMeters < maxReliableRangeMeters) { double relBearing = forwardAngle - i * 180.0d / numRays; GeoPosition pos1 = (GeoPosition)_mapperVicinity.robotPosition.Clone(); pos1.translate(new Direction() { heading = _mapperVicinity.robotDirection.heading, bearingRelative = relBearing }, new Distance(rangeMeters)); DetectedObstacle dobst1 = new DetectedObstacle() { geoPosition = pos1, firstSeen = laserData.TimeStamp.Ticks, lastSeen = laserData.TimeStamp.Ticks, detectorType = DetectorType.SONAR_SCANNING, objectType = DetectedObjectType.Obstacle }; dobst1.SetColorByType(); laserObjects.Add(dobst1); } } if (laserObjects.Count > 0) { int countBefore = 0; lock (_mapperVicinity) { countBefore = _mapperVicinity.Count; _mapperVicinity.AddRange(laserObjects); _mapperVicinity.computeMapPositions(); } //Tracer.Trace(string.Format("laser ready - added {0} to {1}", laserObjects.Count, countBefore)); } }
protected void updateMapperWithProximityData(proxibrick.ProximityDataDssSerializable proximityData) { double[] arrangedForMapper = new double[8]; arrangedForMapper[4] = proximityData.mfl; arrangedForMapper[5] = proximityData.mffl; arrangedForMapper[6] = proximityData.mffr; arrangedForMapper[7] = proximityData.mfr; arrangedForMapper[3] = proximityData.mbl; arrangedForMapper[2] = proximityData.mbbl; arrangedForMapper[1] = proximityData.mbbr; arrangedForMapper[0] = proximityData.mbr; for (int i = 0; i < arrangedForMapper.GetLength(0); i++) { double rangeMeters = arrangedForMapper[i]; if (rangeMeters < 1.4d) { rangeMeters += robotCornerDistanceMeters; // sensor is on the corner, adjust for that double relBearing = angles8[i] + 90.0d; GeoPosition pos1 = (GeoPosition)_mapperVicinity.robotPosition.Clone(); pos1.translate(new Direction() { heading = _mapperVicinity.robotDirection.heading, bearingRelative = relBearing }, new Distance(rangeMeters)); DetectedObstacle dobst1 = new DetectedObstacle() { geoPosition = pos1, firstSeen = proximityData.TimeStamp.Ticks, lastSeen = proximityData.TimeStamp.Ticks, detectorType = DetectorType.IR_DIRECTED, objectType = DetectedObjectType.Obstacle }; dobst1.SetColorByType(); lock (_mapperVicinity) { _mapperVicinity.Add(dobst1); } } } }