/// <summary>
/// We need to initialize Kinect port - since we'll be talking to the service
/// </summary>
/// <param name="kinectPort"></param>
public FramePreProcessor(kinectProxy.KinectOperations kinectPort, TrackRoamerBehaviorsState state, bool useKinectNykoGlasses)
{
this.kinectPort = kinectPort;
_state = state;
if (useKinectNykoGlasses)
{
KinectNykoGlassesFactor = 1.25d;
}
}
protected IEnumerator <ITask> BackUpTurnIterator(Port <bool> butwCompletionPort, TurnAndMoveParameters tamp)
{
bool lastOpSuccess = true;
Fault fault = null;
LogInfo("[[[[[[[[[[[[[[[[[[[[ BackUpTurnIterator() starting ");
// First backup a little.
yield return(Arbiter.Choice(
Translate(-tamp.distance, tamp.speed),
delegate(DefaultUpdateResponseType response) { lastOpSuccess = true; },
delegate(Fault f) { lastOpSuccess = false; fault = f; }
));
// If the DriveDistance was accepted, then wait for it to complete, and start turn.
if (!lastOpSuccess)
{
LogError("[[[[[[[[[[[[[[[[[[[[ BackUpTurnIterator() - Translate FAULT : " + fault);
}
else
{
DriveStageContainer driveStage = new DriveStageContainer();
yield return(WaitForCompletion(driveStage));
LogInfo("WaitForCompletion() returned: " + driveStage.DriveStage);
lastOpSuccess = driveStage.DriveStage == drive.DriveStage.Completed;
if (lastOpSuccess)
{
LogInfo("[[[[[[[[[[[[[[[[[[[[ BackUpTurnIterator() - BackUp portion completed");
if (Math.Abs(tamp.rotateAngle) > 5)
{
// Wait for settling time
//yield return Timeout(settlingTime);
// now we can Turn:
yield return(Arbiter.Choice(
TurnByAngle(tamp.rotateAngle, ModerateTurnPower),
delegate(DefaultUpdateResponseType response) { lastOpSuccess = true; },
delegate(Fault f) { lastOpSuccess = false; fault = f; }
));
// If the RotateDegrees was accepted, then wait for it to complete.
// It is important not to wait if the request failed.
if (lastOpSuccess)
{
TrackRoamerBehaviorsState state = _state;
state.IsTurning = true;
state.LastTurnStarted = state.LastTurnCompleted = DateTime.Now; // reset watchdog
yield return(WaitForCompletion(driveStage));
LogInfo("WaitForCompletion() returned: " + driveStage.DriveStage);
state.IsTurning = false;
state.LastTurnCompleted = DateTime.Now;
if (_mapperVicinity.turnState != null)
{
_mapperVicinity.turnState.finished = state.LastTurnCompleted;
_mapperVicinity.turnState.hasFinished = true;
}
}
else
{
if (_mapperVicinity.turnState != null)
{
_mapperVicinity.turnState.finished = DateTime.Now;
_mapperVicinity.turnState.hasFinished = true;
_mapperVicinity.turnState.wasCanceled = true;
}
LogError("[[[[[[[[[[[[[[[[[[[[ BackUpTurnIterator() - Turn FAULT : " + fault);
}
}
}
else
{
LogInfo("[[[[[[[[[[[[[[[[[[[[ BackUpTurnIterator() - BackUp portion canceled");
}
}
LogInfo("[[[[[[[[[[[[[[[[[[[[ BackUpTurnIterator() completed");
butwCompletionPort.Post(true);
// done
yield break;
}
protected IEnumerator <ITask> KataRunner(Kata kata, Handler onComplete)
{
LogInfo("DriveBehaviorServiceBase: KataRunner(" + kata.name + ") Started" + currentCompass);
kata.success = false;
kata.successfulStepsCount = 0;
bool lastOpSuccess = false;
_state.MovingState = MovingState.InTransition;
// onComplete handler may set MovingState to whatever appropriate. We can set MovingState.Unknown on any error or interruption, and at the end to tamp.desiredMovingState
lastInTransitionStarted = DateTime.Now;
foreach (KataStep kataStep in kata)
{
LogInfo("IP: KataRunner(" + kata.name + ") started step " + (kata.successfulStepsCount + 1) + " " + kataStep.name + currentCompass);
kataStep.success = false;
Fault fault = null;
int rotateAngle = kataStep.rotateAngle;
int distance = kataStep.distance;
if (Math.Abs(rotateAngle) > 1) // "rotate" step
{
CollisionState collisionState = _state.collisionState;
if (collisionState == null || !kataStep.CanPerform(collisionState))
{
LogInfo("Error: KataRunner cannot perform due to CollisionState - on turn");
break; // kata interrupted
}
LogInfo("IP: KataRunner Turn " + rotateAngle);
yield return(Arbiter.Choice(
TurnByAngle(rotateAngle, kataStep.rotatePower * PowerScale),
delegate(DefaultUpdateResponseType response)
{
LogInfo("IP: KataRunner TurnByAngle accepted" + currentCompass);
lastOpSuccess = true;
},
delegate(Fault f)
{
LogInfo("Error: KataRunner TurnByAngle rejected: " + fault);
lastOpSuccess = false;
_state.MovingState = MovingState.Unknown;
fault = f;
}
));
// If the RotateDegrees was accepted, then wait for it to complete.
// It is important not to wait if the request failed.
if (lastOpSuccess)
{
TrackRoamerBehaviorsState state = _state;
state.IsTurning = true; // can't trust laser while turning
state.LastTurnStarted = state.LastTurnCompleted = DateTime.Now; // reset watchdog
DriveStageContainer driveStage = new DriveStageContainer();
yield return(WaitForCompletion(driveStage));
LogInfo("OK: WaitForCompletion() returned: " + driveStage.DriveStage);
lastOpSuccess = driveStage.DriveStage == drive.DriveStage.Completed;
if (lastOpSuccess)
{
if (_mapperVicinity.turnState != null)
{
_mapperVicinity.turnState.finished = DateTime.Now;
_mapperVicinity.turnState.hasFinished = true;
}
}
else
{
if (_mapperVicinity.turnState != null)
{
_mapperVicinity.turnState.finished = DateTime.Now;
_mapperVicinity.turnState.hasFinished = true;
_mapperVicinity.turnState.wasCanceled = true;
}
LogInfo("op failure");
state.MovingState = MovingState.Unknown;
}
state.IsTurning = false;
state.LastTurnCompleted = DateTime.Now;
// make sure we display zero power:
_mapperVicinity.robotState.leftPower = 0.0d;
_mapperVicinity.robotState.rightPower = 0.0d;
}
else
{
break;
}
}
if (Math.Abs(distance) > 1) // "translate" step
{
// while we were rotating, collisionState might have changed
CollisionState collisionState = _state.collisionState;
if (collisionState == null || !kataStep.CanPerform(collisionState))
{
LogInfo("Error: KataRunner cannot perform due to CollisionState - on translate");
break; // kata interrupted
}
LogInfo("IP: KataRunner Translate " + distance);
yield return(Arbiter.Choice(
Translate(distance, kataStep.speed * PowerScale),
delegate(DefaultUpdateResponseType response) { lastOpSuccess = true; },
delegate(Fault f)
{
lastOpSuccess = false;
_state.MovingState = MovingState.Unknown;
fault = f;
}
));
// If the Translate was accepted, then wait for it to complete.
// It is important not to wait if the request failed.
if (lastOpSuccess)
{
DriveStageContainer driveStage = new DriveStageContainer();
yield return(WaitForCompletion(driveStage));
LogInfo("WaitForCompletion() returned: " + driveStage.DriveStage);
lastOpSuccess = driveStage.DriveStage == drive.DriveStage.Completed;
// make sure we display zero power:
_mapperVicinity.robotState.leftPower = 0.0d;
_mapperVicinity.robotState.rightPower = 0.0d;
}
if (!lastOpSuccess)
{
break;
}
}
LogInfo("KataRunner(" + kata.name + ") finished step=" + kataStep.name + currentCompass);
kata.successfulStepsCount++;
}
kata.success = kata.Count == kata.successfulStepsCount;
_state.MovingState = MovingState.Unknown; // that's for now, onComplete may set it to whatever appropriate
LogInfo("KataRunner - calling onComplete()");
onComplete(); // check kata.success, will be false if DriveStage.Cancel or other interruption occured.
// done
LogInfo("DriveBehaviorServiceBase: KataRunner(" + kata.name + ") finished" + currentCompass);
yield break;
}
/// <summary>
/// We need to initialize Kinect port - since we'll be talking to the service
/// </summary>
/// <param name="kinectPort"></param>
public FramePreProcessor(kinectProxy.KinectOperations kinectPort, TrackRoamerBehaviorsState state)
{
this.kinectPort = kinectPort;
_state = state;
}
protected override void Start()
{
LogInfo("DriveBehaviorServiceBase:Start() _doUnitTest=" + _doUnitTest + " _doSimulatedLaser=" + _doSimulatedLaser + " _testBumpMode=" + _testBumpMode);
base.Start(); // start MainPortInterleave; wireup [ServiceHandler] methods
InitGunTurrets();
// HeadlightsOff(); -- this will be done by SafePositions in PololuMaestroService
if (_state == null)
{
// no partner-supplied initial state found - create default one:
LogInfo("DriveBehaviorServiceBase:: initial _state null");
_state = new TrackRoamerBehaviorsState();
SaveState(_state);
}
else
{
LogInfo("DriveBehaviorServiceBase:: initial _state supplied by partner");
_state.Init(); // clear and re-create some members that should not be taken from the saved state
}
_httpUtilities = DsspHttpUtilitiesService.Create(Environment);
if (_doUnitTest)
{
Talker.Say(9, "unit test mode");
}
if (_testBumpMode)
{
Talker.Say(9, "bumper test mode");
}
if (_doSimulatedLaser)
{
Talker.Say(9, "laser will be simulated");
WatchdogInterval = _simulatedLaserWatchdogInterval; // about the time ultrasonic sensor makes a sweep
}
if (!_doUnitTest || _unitTestSensorsOn)
{
// Handlers that need write or exclusive access to state go under
// the exclusive group. Handlers that need read or shared access, and can be
// concurrent to other readers, go to the concurrent group.
// Other internal ports can be included in interleave so you can coordinate
// intermediate computation with top level handlers.
#region request handler setup
MainPortInterleave.CombineWith(new Interleave(
new TeardownReceiverGroup(),
new ExclusiveReceiverGroup(
Arbiter.Receive<LaserRangeFinderResetUpdate>(true, _mainPort, LaserRangeFinderResetUpdateHandler),
Arbiter.Receive<LaserRangeFinderUpdate>(true, _mainPort, LaserRangeFinderUpdateHandler),
Arbiter.Receive<BumpersArrayUpdate>(true, _mainPort, BumpersArrayUpdateHandler),
Arbiter.Receive<BumperUpdate>(true, _mainPort, BumperUpdateHandler),
Arbiter.Receive<DriveUpdate>(true, _mainPort, DriveUpdateHandler),
//Arbiter.Receive<EncoderUpdate>(true, _mainPort, EncoderUpdateHandler),
Arbiter.Receive<WatchDogUpdate>(true, _mainPort, WatchDogUpdateHandler)
),
new ConcurrentReceiverGroup(
Arbiter.Receive<Get>(true, _mainPort, GetHandler),
//Arbiter.Receive<HttpGet>(true, _mainPort, HttpGetHandler),
Arbiter.Receive<dssp.DsspDefaultLookup>(true, _mainPort, DefaultLookupHandler)
)
)
);
#endregion
#region notification handler setup
MainPortInterleave.CombineWith(new Interleave(
new TeardownReceiverGroup(),
new ExclusiveReceiverGroup(),
new ConcurrentReceiverGroup(
Arbiter.Receive<sicklrf.Reset>(true, _laserNotify, LaserResetNotification),
Arbiter.Receive<proxibrick.UpdateAccelerometerData>(true, _trackRoamerBrickProximityBoardServiceNotify, trpbUpdateAccelerometerNotification),
Arbiter.Receive<proxibrick.UpdateDirectionData>(true, _trackRoamerBrickProximityBoardServiceNotify, trpbUpdateDirectionNotification),
Arbiter.Receive<proxibrick.UpdateProximityData>(true, _trackRoamerBrickProximityBoardServiceNotify, trpbUpdateProximityNotification),
Arbiter.Receive<proxibrick.UpdateParkingSensorData>(true, _trackRoamerBrickProximityBoardServiceNotify, trpbUpdateParkingSensorNotification),
Arbiter.Receive<proxibrick.UpdateAnalogData>(true, _trackRoamerBrickProximityBoardServiceNotify, trpbUpdateAnalogNotification),
Arbiter.Receive<drive.Update>(true, _driveNotify, DriveUpdateNotification),
Arbiter.ReceiveWithIterator<drive.DriveDistance>(true, _driveNotify, DriveDistanceUpdateHandler),
Arbiter.ReceiveWithIterator<drive.RotateDegrees>(true, _driveNotify, RotateDegreesUpdateHandler),
//Arbiter.Receive<encoder.UpdateTickCount>(true, _encoderNotifyLeft, EncoderUpdateTickCountNotificationLeft),
//Arbiter.Receive<encoder.UpdateTickCount>(true, _encoderNotifyRight, EncoderUpdateTickCountNotificationRight),
Arbiter.Receive<powerbrick.UpdateMotorEncoder>(true, notificationPortEncoder, EncoderNotificationHandler),
Arbiter.Receive<powerbrick.UpdateMotorEncoderSpeed>(true, notificationPortEncoderSpeed, EncoderSpeedNotificationHandler),
Arbiter.Receive<bumper.Replace>(true, _bumperNotify, BumperReplaceNotification),
Arbiter.Receive<bumper.Update>(true, _bumperNotify, BumperUpdateNotification)
)
)
);
#endregion
// We cannot replicate the activation of laser notifications because the
// handler uses Test() to skip old laser notifications.
// Activate the handler once, it will re-activate itself:
Activate(
Arbiter.ReceiveWithIterator<sicklrf.Replace>(false, _laserNotify, LaserReplaceNotificationHandler)
);
if (USE_ORIENTATION_UM6 || USE_DIRECTION_UM6_QUATERNION || USE_DIRECTION_UM6_EULER)
{
// Start listening to CH Robotics UM6 Orientation Sensor:
SubscribeToChrUm6OrientationSensor();
}
// Start listening to GPS:
SubscribeToGps();
// Start watchdog timer - we need it to detect that the laser does not return current picture and stop:
_mainPort.Post(new WatchDogUpdate(new WatchDogUpdateRequest(DateTime.Now)));
}
else
{
// for unit tests - drive only operation:
#region request handler setup
MainPortInterleave.CombineWith(new Interleave(
new TeardownReceiverGroup(),
new ExclusiveReceiverGroup(
Arbiter.Receive<BumpersArrayUpdate>(true, _mainPort, BumpersArrayUpdateHandler),
Arbiter.Receive<BumperUpdate>(true, _mainPort, BumperUpdateHandler),
//Arbiter.Receive<EncoderUpdate>(true, _mainPort, EncoderUpdateHandler),
Arbiter.Receive<DriveUpdate>(true, _mainPort, DriveUpdateHandler)
),
new ConcurrentReceiverGroup(
Arbiter.Receive<Get>(true, _mainPort, GetHandler),
//Arbiter.Receive<HttpGet>(true, _mainPort, HttpGetHandler),
Arbiter.Receive<dssp.DsspDefaultLookup>(true, _mainPort, DefaultLookupHandler)
)
)
);
#endregion
#region notification handler setup
MainPortInterleave.CombineWith(new Interleave(
new TeardownReceiverGroup(),
new ExclusiveReceiverGroup(),
new ConcurrentReceiverGroup(
Arbiter.Receive<drive.Update>(true, _driveNotify, DriveUpdateNotification),
Arbiter.ReceiveWithIterator<drive.DriveDistance>(true, _driveNotify, DriveDistanceUpdateHandler),
Arbiter.ReceiveWithIterator<drive.RotateDegrees>(true, _driveNotify, RotateDegreesUpdateHandler),
//Arbiter.Receive<encoder.UpdateTickCount>(true, _encoderNotifyLeft, EncoderUpdateTickCountNotificationLeft),
//Arbiter.Receive<encoder.UpdateTickCount>(true, _encoderNotifyRight, EncoderUpdateTickCountNotificationRight),
Arbiter.Receive<powerbrick.UpdateMotorEncoder>(true, notificationPortEncoder, EncoderNotificationHandler),
Arbiter.Receive<powerbrick.UpdateMotorEncoderSpeed>(true, notificationPortEncoderSpeed, EncoderSpeedNotificationHandler),
Arbiter.Receive<bumper.Replace>(true, _bumperNotify, BumperReplaceNotification),
Arbiter.Receive<bumper.Update>(true, _bumperNotify, BumperUpdateNotification)
)
)
);
#endregion
}
// Subscribe to the direct drive for notification messages:
_drivePort.Subscribe(_driveNotify);
//_encoderPortLeft.Subscribe(_encoderNotifyLeft);
//_encoderPortRight.Subscribe(_encoderNotifyRight);
SubscribeToPowerBrick(); // for Encoders distance and speed
_bumperPort.Subscribe(_bumperNotify);
SpawnIterator(this.AhrsZeroGyrosAndAccelerometer);
SpawnIterator(this.InitializeSpeechRecognizer);
// create WPF adapter
this.wpfServicePort = ccrwpf.WpfAdapter.Create(TaskQueue);
SpawnIterator(InitializeGui);
if (!_doUnitTest || _unitTestSensorsOn)
{
Tracer.Trace("DriveBehaviorServiceBase:Start(): calling Subscribe() for laser range, proximity, accelerometer, compass partners");
_laserPort.Subscribe(_laserNotify);
Type[] notifyMeOf = new Type[] {
typeof(proxibrick.UpdateAccelerometerData),
typeof(proxibrick.UpdateDirectionData),
typeof(proxibrick.UpdateProximityData),
typeof(proxibrick.UpdateParkingSensorData),
typeof(proxibrick.UpdateAnalogData)
};
_trackRoamerBrickProximityBoardServicePort.Subscribe(_trackRoamerBrickProximityBoardServiceNotify, notifyMeOf);
SpawnIterator(this.InitializeKinectUI);
}
SafePosture();
SetLightsTest(); // SetLightsNormal() is called at the end of CalibrateKinectPlatform iteration
SpawnIterator(this.LightsControlLoop);
SpawnIterator(this.ConnectObstacleAvoidanceDrive);
SpawnIterator(this.CalibrateKinectPlatform);
SpawnIterator(this.TestAnimatedHead);
if (_doUnitTest)
{
performUnitTest();
}
else
{
SpawnIterator(this.InitializeDecisionMainLoop);
}
// we will need this later:
robotCornerDistanceMeters = Math.Sqrt(_mapperVicinity.robotState.robotLengthMeters * _mapperVicinity.robotState.robotLengthMeters + _mapperVicinity.robotState.robotWidthMeters * _mapperVicinity.robotState.robotWidthMeters) / 2.0d;
}
