private Ema analogValue1Ema = new Ema(7); // filter out spikes from imperfect kinect platform pan measurement #endregion Fields #region Methods /// <summary> /// called when the AHRS/Compass reports new orientation. Preserves bearing to the goal. /// </summary> /// <param name="newDir"></param> protected void setCurrentDirection(proxibrick.DirectionDataDssSerializable newDir) { proxibrick.DirectionDataDssSerializable curDir = _state.MostRecentDirection; if (curDir == null || Math.Abs(newDir.heading - curDir.heading) > 0.9d || DateTime.Now > curDir.TimeStamp.AddSeconds(3.0d)) // only react on significant changes in direction { _state.MostRecentDirection = newDir; //.Clone() //Tracer.Trace("heading: " + newDir.heading); if (_mapperVicinity.turnState != null && !_mapperVicinity.turnState.hasFinished) { _mapperVicinity.turnState.directionCurrent = new Direction() { heading = newDir.heading, TimeStamp = newDir.TimeStamp.Ticks }; } // update mapper with Direction data: _mapperVicinity.robotDirection = new Direction() { TimeStamp = newDir.TimeStamp.Ticks, heading = newDir.heading, bearing = _mapperVicinity.robotDirection.bearing }; // set same bearing to the new Direction // update mapper with Odometry data: updateMapperWithOdometryData(); // update GUI (compass control): setGuiCurrentDirection(newDir); if (!_doUnitTest) { Decide(SensorEventSource.Compass); } } }
protected void setGuiCurrentAccelerometer(proxibrick.AccelerometerDataDssSerializable acc) { if (_mainWindow != null) { ccrwpf.Invoke invoke = new ccrwpf.Invoke(delegate() { _mainWindow.CurrentAccelerometer = new AccelerometerData() { TimeStamp = acc.TimeStamp.Ticks, accX = acc.accX, accY = acc.accY, accZ = acc.accZ }; } ); wpfServicePort.Post(invoke); Arbiter.Activate(TaskQueue, invoke.ResponsePort.Choice( s => { }, // delegate for success ex => { } //Tracer.Error(ex) // delegate for failure )); } }
/// <summary> /// convert sonar sweep into laser-like 180 degrees data /// </summary> /// <param name="update"></param> void trpbUpdateSonarNotification(proxibrick.UpdateSonarData update) { //LogInfo("TrackRoamerUsrfService: trpbUpdateNotification()"); //Tracer.Trace("TrackRoamerUsrfService: trpbUpdateNotification()"); try { proxibrick.SonarDataDssSerializable p = update.Body; int packetLength = p.RangeMeters.Length; // must be 26 packets, each covering 7 degrees; if (packetLength != 26) { Tracer.Error("TrackRoamerUsrfService::trpbUpdateNotification() not a standard measurement, angles.Count=" + packetLength + " (expected 26) -- ignored"); return; } int[] intRanges = new int[packetLength]; for(int i=0; i < packetLength ;i++) { // range = (ushort)(i * 40); ushort range = (ushort)Math.Round(p.RangeMeters[i] * 1000.0d); if (range > 0x1FF7) { range = 0x2000; // limit to 8192 mm; actual range about 4 meters } intRanges[i] = (int)range; } if (doWeeding) { if (intRanges[0] < (intRanges[1] + intRanges[2]) / 4) { intRanges[0] = (intRanges[1] + intRanges[2]) / 2; } if (intRanges[packetLength - 1] < (intRanges[packetLength - 2] + intRanges[packetLength - 3]) / 4) { intRanges[packetLength - 1] = (intRanges[packetLength - 2] + intRanges[packetLength - 3]) / 2; } for (int i = 1; i < packetLength-1; i++) { if (intRanges[i] < (intRanges[i - 1] + intRanges[i + 1]) * 3 / 8) { intRanges[i] = (intRanges[i - 1] + intRanges[i + 1]) / 2; } } } int angularRange = 180; int angularResolution = 1; int mesLength = angularRange + 1; // 181 int[] lsdRanges = new int[mesLength]; // millimeters, with 1 degree resolution int step = (int)Math.Round((double)mesLength / (double)packetLength); // typically round(6.96) = 7 // if we smooth the measurements, Decide() has better chance of sorting the values right. It does not like 7 degrees steps. // we need these for exponential moving average: double emaPeriod = 4.0d; double emaMultiplier = 2.0d / (1.0d + emaPeriod); double? emaPrev = null; int iRange = 0; for (int i = 0; i < mesLength; i++) // 0...181 { int angleIndex = Math.Min(i / step, packetLength-1); iRange = intRanges[angleIndex]; if (doAveraging) { // calculate exponential moving average - smooth the curve a bit: double? ema = !emaPrev.HasGoodValue() ? iRange : ((iRange - emaPrev) * emaMultiplier + emaPrev); emaPrev = ema; iRange = (int)Math.Round((double)ema); } //Tracer.Trace("&&&&&&&&&&&&&&&&&&&&&&&&&&&& i=" + i + " range=" + range + " ema=" + iRange); lsdRanges[i] = iRange; // 5000; // milimeters } _state.AngularRange = angularRange; _state.AngularResolution = angularResolution; _state.DistanceMeasurements = lsdRanges; _state.Units = sicklrf.Units.Millimeters; _state.TimeStamp = p.TimeStamp; _state.LinkState = "Measurement received"; // // Inform subscribed services that the state has changed. // _submgrPort.Post(new submgr.Submit(_state, DsspActions.ReplaceRequest)); } catch (Exception exc) { LogError(exc); } }
void trpbUpdateSonarNotification(proxibrick.UpdateSonarData update) { try { proxibrick.SonarDataDssSerializable sweep = update.Body; state.MostRecentSonar = sweep; state.SonarTimeStamp = sweep.TimeStamp; state.SonarLinkState = "Sonar Measurement received"; // // Inform subscribed services that the state has changed. // //submgrPort.Post(new submgr.Submit(state, DsspActions.ReplaceRequest)); } catch (Exception exc) { Tracer.Trace("trpbUpdateSonarNotification() - " + exc); } }
void trpbUpdateProximityNotification(proxibrick.UpdateProximityData update) { //LogInfo("DriveBehaviorServiceBase: trpbUpdateProximityNotification()"); //Tracer.Trace("DriveBehaviorServiceBase: trpbUpdateProximityNotification()"); try { proxibrick.ProximityDataDssSerializable prx = update.Body; state.MostRecentProximity = prx; // // Inform subscribed services that the state has changed. // //submgrPort.Post(new submgr.Submit(state, DsspActions.ReplaceRequest)); } catch (Exception exc) { Tracer.Trace("trpbUpdateProximityNotification() - " + exc); } }
/// <summary> /// update GUI with current direction /// </summary> /// <param name="dir"></param> protected void setGuiCurrentDirection(proxibrick.DirectionDataDssSerializable dir) { if (_mainWindow != null) { ccrwpf.Invoke invoke = new ccrwpf.Invoke(delegate() { _mainWindow.CurrentDirection = new DirectionData() { TimeStamp = dir.TimeStamp.Ticks, heading = dir.heading, bearing = _mapperVicinity.robotDirection.bearing }; } ); wpfServicePort.Post(invoke); Arbiter.Activate(TaskQueue, invoke.ResponsePort.Choice( s => { }, // delegate for success ex => { } //Tracer.Error(ex) // delegate for failure )); } }
void trpbUpdateDirectionNotification(proxibrick.UpdateDirectionData update) { //LogInfo("DriveBehaviorServiceBase: trpbUpdateDirectionNotification()"); //Tracer.Trace("DriveBehaviorServiceBase: trpbUpdateDirectionNotification()"); if (USE_DIRECTION_PROXIBRICK) { try { proxibrick.DirectionDataDssSerializable newDir = update.Body; setCurrentDirection(newDir); } catch (Exception exc) { Tracer.Trace("trpbUpdateDirectionNotification() - " + exc); } } }
void trpbUpdateProximityNotification(proxibrick.UpdateProximityData update) { //LogInfo("DriveBehaviorServiceBase: trpbUpdateProximityNotification()"); //Tracer.Trace("DriveBehaviorServiceBase: trpbUpdateProximityNotification()"); if (!_mapperVicinity.robotState.ignoreProximity) { try { proxibrick.ProximityDataDssSerializable prx = update.Body; _state.MostRecentProximity = prx; updateMapperWithOdometryData(); updateMapperWithProximityData(prx); setGuiCurrentProximity(prx); if (!_doUnitTest) { Decide(SensorEventSource.IrDirected); } } catch (Exception exc) { Tracer.Trace("trpbUpdateProximityNotification() - " + exc); } } }
void trpbUpdateAnalogNotification(proxibrick.UpdateAnalogData update) { //LogInfo("DriveBehaviorServiceBase: trpbUpdateAnalogNotification() value=" + update.Body); //Tracer.Trace("DriveBehaviorServiceBase: trpbUpdateAnalogNotification() analogValue1=" + update.Body.analogValue1 + " TimeStamp=" + update.Body.TimeStamp); //Tracer.Trace("DriveBehaviorServiceBase: trpbUpdateAnalogNotification() analogValue1=" + update.Body.analogValue1 + " TimeStamp=" + update.Body.TimeStamp + " angle=" + PanTiltAlignment.getInstance().degreesPanKinect(update.Body.analogValue1)); try { proxibrick.AnalogDataDssSerializable pds = update.Body; double analogValue1 = pds.analogValue1; // just overwrite the values in place, no need to lock or create another object: _state.MostRecentAnalogValues.analogValue1 = analogValue1; _state.MostRecentAnalogValues.TimeStamp = pds.TimeStamp; analogValue1 = analogValue1Ema.Compute(analogValue1); // filter out spikes from imperfect measurement _state.currentPanKinect = PanTiltAlignment.getInstance().degreesPanKinect(analogValue1); computeAndExecuteKinectPlatformTurn(); } catch (Exception exc) { Tracer.Trace("trpbUpdateAnalogNotification() - " + exc); } }
void trpbUpdateAccelerometerNotification(proxibrick.UpdateAccelerometerData update) { //LogInfo("DriveBehaviorServiceBase: trpbUpdateAccelerometerNotification()"); //Tracer.Trace("DriveBehaviorServiceBase: trpbUpdateAccelerometerNotification()"); if (USE_ORIENTATION_PROXIBRICK) { try { _state.MostRecentAccelerometer = update.Body; setGuiCurrentAccelerometer(_state.MostRecentAccelerometer); if (!_doUnitTest) { Decide(SensorEventSource.Orientation); } } catch (Exception exc) { Tracer.Trace("trpbUpdateAccelerometerNotification() - " + exc); } } }
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); } } } }
protected void setGuiCurrentProximity(proxibrick.ProximityDataDssSerializable dir) { if (_mainWindow != null) { ccrwpf.Invoke invoke = new ccrwpf.Invoke( delegate() { _mainWindow.CurrentProximity = new ProximityData() { TimeStamp = dir.TimeStamp.Ticks, mbbl = dir.mbbl, mbbr = dir.mbbr, mbl = dir.mbl, mbr = dir.mbr, mffl = dir.mffl, mffr = dir.mffr, mfl = dir.mfl, mfr = dir.mfr }; } ); wpfServicePort.Post(invoke); Arbiter.Activate(TaskQueue, invoke.ResponsePort.Choice( s => { }, // delegate for success ex => { } //Tracer.Trace(ex) // delegate for failure )); } }
protected void setGuiCurrentParkingSensor(proxibrick.ParkingSensorDataDssSerializable dir) { if (_mainWindow != null) { ccrwpf.Invoke invoke = new ccrwpf.Invoke( delegate() { _mainWindow.CurrentParkingSensor = new ParkingSensorData() { TimeStamp = dir.TimeStamp.Ticks, parkingSensorMetersLB = dir.parkingSensorMetersLB, parkingSensorMetersLF = dir.parkingSensorMetersLF, parkingSensorMetersRB = dir.parkingSensorMetersRB, parkingSensorMetersRF = dir.parkingSensorMetersRF }; } ); wpfServicePort.Post(invoke); Arbiter.Activate(TaskQueue, invoke.ResponsePort.Choice( s => { }, // delegate for success ex => { } //Tracer.Trace(ex) // delegate for failure )); } }