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