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
));
}
}
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);
}
}
}
private static double[] angles8 = { 37.5d, 62.5d, 117.5d, 142.5d, 217.5d, 242.5d, -62.5d, -37.5d }; // eight IR sensors on the corners
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);
}
}
}
}
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>
/// evaluates proximity and parking sensor data and translates all of it into
/// actionable restrictions in CollisionState object
/// </summary>
public void computeCollisionState(RoutePlan plan, double?intendedVelocity)
{
CollisionState cs = _state.collisionState;
//cs.initRestrictive();
cs.initPermissive(FreeDistanceMm, MaximumForwardVelocityMmSec, MaximumBackwardVelocityMmSec);
// initialize to the maximums:
canMoveForwardDistanceM = FreeDistanceMm * 2.0d / 1000.0d;
canMoveBackwardsDistanceM = FreeDistanceMm * 2.0d / 1000.0d;
//Tracer.Trace("IR ffl=" + _state.MostRecentProximity.mfl + "m PS MetersLF=" + _state.MostRecentParkingSensor.parkingSensorMetersLF + "m");
if (intendedVelocity == null)
{
intendedVelocity = _state.Velocity;
}
bool isMovingForward = intendedVelocity > 0.01d; // m/s
bool isMovingBackwards = intendedVelocity < -0.01d;
int?distance = null; // mm
if (plan != null && plan.isGoodPlan)
{
double?bestHeadingRelative = plan.bestHeadingRelative(_mapperVicinity);
if (bestHeadingRelative != null && bestHeadingRelative.Value > -45.0d && bestHeadingRelative.Value < 45.0d)
{
distance = (int)(plan.legMeters.Value * 1000.0d);
if (distance < ObstacleDistanceMm)
{
cs.canMoveByPlan = false;
}
else
{
cs.canMoveByPlan = true;
}
}
}
DateTime sensorInvalidateTime = DateTime.Now.AddSeconds(-ignoreOldSensorReadingsIntervalSec); // ignore sensor readings that are older than this time
StringBuilder msb = new StringBuilder(); // build the message here
// we compute full state every time, but format the message so it is relevant to the direction we move to.
// forward sensors: -------------------------------------------------------------------------------------------
proxibrick.ProximityDataDssSerializable MostRecentProximity = _state.MostRecentProximity;
proxibrick.ParkingSensorDataDssSerializable MostRecentParkingSensor = _state.MostRecentParkingSensor;
if (MostRecentProximity != null && MostRecentProximity.TimeStamp > sensorInvalidateTime)
{
double mfl = MostRecentProximity.mfl;
if (mfl < proxiTreshold)
{
if (isMovingForward)
{
msb.AppendFormat("Front Left Side proximity detected obstacle at {0}m; ", mfl);
}
cs.canTurnLeft &= mfl > proxiTresholdTurn1;
if (mfl < canMoveForwardDistanceM)
{
canMoveForwardDistanceM = mfl;
}
}
double mffl = MostRecentProximity.mffl;
if (mffl < proxiTreshold)
{
if (isMovingForward)
{
msb.AppendFormat("FF_Left proximity detected obstacle at {0}m; ", mffl);
}
//cs.canTurnLeft &= mffl > proxiTresholdTurn2;
if (mffl < canMoveForwardDistanceM)
{
canMoveForwardDistanceM = mffl;
}
}
double mffr = MostRecentProximity.mffr;
if (mffr < proxiTreshold)
{
if (isMovingForward)
{
msb.AppendFormat("FF_Right proximity detected obstacle at {0}m; ", mffr);
}
//cs.canTurnRight &= mffr > proxiTresholdTurn2;
if (mffr < canMoveForwardDistanceM)
{
canMoveForwardDistanceM = mffr;
}
}
double mfr = MostRecentProximity.mfr;
if (mfr < proxiTreshold)
{
if (isMovingForward)
{
msb.AppendFormat("Front Right Side proximity detected obstacle at {0}m; ", mfr);
}
cs.canTurnRight &= mfr > proxiTresholdTurn1;
if (mfr < canMoveForwardDistanceM)
{
canMoveForwardDistanceM = mfr;
}
}
}
if (MostRecentParkingSensor != null && MostRecentParkingSensor.TimeStamp > sensorInvalidateTime)
{
double parkingSensorMetersLF = MostRecentParkingSensor.parkingSensorMetersLF;
if (parkingSensorMetersLF < psiTreshold)
{
if (isMovingForward)
{
msb.AppendFormat("Front Left parking sensor detected obstacle at {0}m; ", parkingSensorMetersLF);
}
if (parkingSensorMetersLF < canMoveForwardDistanceM)
{
canMoveForwardDistanceM = parkingSensorMetersLF;
}
}
double parkingSensorMetersRF = MostRecentParkingSensor.parkingSensorMetersRF;
if (parkingSensorMetersRF < psiTreshold)
{
if (isMovingForward)
{
msb.AppendFormat("Front Right parking sensor detected obstacle at {0}m; ", parkingSensorMetersRF);
}
if (parkingSensorMetersRF < canMoveForwardDistanceM)
{
canMoveForwardDistanceM = parkingSensorMetersRF;
}
}
}
// backward sensors: -------------------------------------------------------------------------------------------
if (MostRecentProximity != null && MostRecentProximity.TimeStamp > sensorInvalidateTime)
{
double mbl = MostRecentProximity.mbl;
if (mbl < proxiTreshold)
{
if (isMovingBackwards)
{
msb.AppendFormat("Back Left Side proximity detected obstacle at {0}m; ", mbl);
}
cs.canTurnRight &= mbl > proxiTresholdTurn1;
if (mbl < canMoveBackwardsDistanceM)
{
canMoveBackwardsDistanceM = mbl;
}
}
double mbbl = MostRecentProximity.mbbl;
if (mbbl < proxiTreshold)
{
if (isMovingBackwards)
{
msb.AppendFormat("BB_Left proximity detected obstacle at {0}m; ", mbbl);
}
//cs.canTurnRight &= mbbl > proxiTresholdTurn2;
if (mbbl < canMoveBackwardsDistanceM)
{
canMoveBackwardsDistanceM = mbbl;
}
}
double mbbr = MostRecentProximity.mbbr;
if (mbbr < proxiTreshold)
{
if (isMovingBackwards)
{
msb.AppendFormat("BB_Right proximity detected obstacle at {0}m; ", mbbr);
}
//cs.canTurnLeft &= mbbr > proxiTresholdTurn2;
if (mbbr < canMoveBackwardsDistanceM)
{
canMoveBackwardsDistanceM = mbbr;
}
}
double mbr = MostRecentProximity.mbr;
if (mbr < proxiTreshold)
{
if (isMovingBackwards)
{
msb.AppendFormat("Back Right Side proximity detected obstacle at {0}m; ", mbr);
}
cs.canTurnLeft &= mbr > proxiTresholdTurn1;
if (mbr < canMoveBackwardsDistanceM)
{
canMoveBackwardsDistanceM = mbr;
}
}
}
if (MostRecentParkingSensor != null && MostRecentParkingSensor.TimeStamp > sensorInvalidateTime)
{
double parkingSensorMetersLB = MostRecentParkingSensor.parkingSensorMetersLB;
if (parkingSensorMetersLB < psiTreshold)
{
if (isMovingBackwards)
{
msb.AppendFormat("Back Left parking sensor detected obstacle at {0}m; ", parkingSensorMetersLB);
}
if (parkingSensorMetersLB < canMoveBackwardsDistanceM)
{
canMoveBackwardsDistanceM = parkingSensorMetersLB;
}
}
double parkingSensorMetersRB = MostRecentParkingSensor.parkingSensorMetersRB;
if (parkingSensorMetersRB < psiTreshold)
{
if (isMovingBackwards)
{
msb.AppendFormat("Back Right parking sensor detected obstacle at {0}m; ", parkingSensorMetersRB);
}
if (parkingSensorMetersRB < canMoveBackwardsDistanceM)
{
canMoveBackwardsDistanceM = parkingSensorMetersRB;
}
}
}
if (_state.MostRecentWhiskerLeft)
{
msb.Append("Left Whisker; ");
canMoveForwardDistanceM = 0.0d;
cs.canMoveForward = false;
cs.canTurnRight = false;
cs.canTurnLeft = false;
}
if (_state.MostRecentWhiskerRight)
{
msb.Append("Right Whisker; ");
canMoveForwardDistanceM = 0.0d;
cs.canMoveForward = false;
cs.canTurnRight = false;
cs.canTurnLeft = false;
}
adjustNearCollisionSpeed(canMoveForwardDistanceM, true);
adjustNearCollisionSpeed(canMoveBackwardsDistanceM, false);
// compute the "mustStop" and "message":
if (isMovingForward)
{
cs.mustStop = !cs.canMoveForward;
}
else if (isMovingBackwards)
{
cs.mustStop = !cs.canMoveBackwards;
}
if (msb.Length > 0 || cs.mustStop)
{
if (isMovingForward)
{
msb.Insert(0, "While moving forward: ");
}
else if (isMovingBackwards)
{
msb.Insert(0, "While moving backwards: ");
}
if (cs.mustStop)
{
msb.Insert(0, "!! MUST STOP !! ");
}
}
else
{
if (isMovingForward)
{
msb.Append("Can continue forward: ");
}
else if (isMovingBackwards)
{
msb.Append("Can continue backwards: ");
}
else
{
msb.Append("Can move: ");
}
}
msb.AppendFormat(" FWD: {0} to {1} mm at {2:F0} mm/s BKWD: {3} to {4} mm at {5:F0} mm/s LEFT:{6} RIGHT:{7}",
cs.canMoveForward, cs.canMoveForwardDistanceMm, cs.canMoveForwardSpeedMms,
cs.canMoveBackwards, cs.canMoveBackwardsDistanceMm, cs.canMoveBackwardsSpeedMms,
cs.canTurnLeft, cs.canTurnRight);
if (cs.canMoveByPlan.HasValue && !cs.canMoveByPlan.Value)
{
msb.AppendFormat("; best plan obstacle too close at {0} mm; ", distance);
}
string message = msb.ToString();
//if (!string.Equals(message, _lastMessage))
//{
// Tracer.Trace(message);
// _lastMessage = message;
//}
cs.message = message;
}
