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