SuccessFailurePort OpenCamera(CameraInstance camera) { SuccessFailurePort resultPort = new SuccessFailurePort(); new OpenRequest(camera.DevicePath).Send(_client); Activate( Arbiter.Receive <HresultResponse>(false, _pipeDataPort, delegate(HresultResponse result) { if (result.hr == 0) { resultPort.Post(SuccessResult.Instance); } else { resultPort.Post(Marshal.GetExceptionForHR(result.hr)); } }, test => test.type == WebcamResponse.OpenDevices ) ); return(resultPort); }
SuccessFailurePort StopCapture() { SuccessFailurePort resultPort = new SuccessFailurePort(); new StopRequest().Send(_client); Activate( Arbiter.Receive <HresultResponse>(false, _pipeDataPort, delegate(HresultResponse result) { if (result.hr == 0) { resultPort.Post(SuccessResult.Instance); } else { resultPort.Post(Marshal.GetExceptionForHR(result.hr)); } }, test => test.type == WebcamResponse.StopCapture ) ); return(resultPort); }
/// <summary> /// Helper function to initialize Kinect camera. /// </summary> /// <returns>CCR Iterator</returns> protected IEnumerator <ITask> InitializeKinect() { var finishedInitialization = new SuccessFailurePort(); ThreadPool.QueueUserWorkItem( param => { try { this.InitializeKinectDevice(); finishedInitialization.Post(SuccessResult.Instance); } catch (Exception e) { LogError(e); finishedInitialization.Post(new Exception(Resources.InitializeFailed)); } }); yield return(finishedInitialization.Choice( success => { base.Start(); this.StartPollingBehavior(); // Merge the internal update ports into the main interleave MainPortInterleave.CombineWith( new Interleave( new ExclusiveReceiverGroup( Arbiter.Receive( true, this.alternateContractUpdatePort, this.OnUpdateAlternates)), new ConcurrentReceiverGroup( Arbiter.Receive( true, this.scheduleNextFrameReadPort, this.OnScheduleNextFrameRead)))); LogInfo(Resources.Initialized); }, fault => { LogError(fault); base.StartFailed(); })); }
/// <summary> /// Shutdown or kill process /// </summary> /// <returns>ITask enumerator</returns> private IEnumerator <ITask> ShutdownSuccessfullyOrKillProcess() { var shutdownOrTimedOut = new SuccessFailurePort(); Activate(TimeoutPort(DefaultProcessTimeout).Receive(dateTime => shutdownOrTimedOut.Post(new Exception()))); Activate(shutdownOrTimedOut.Choice( success => { }, /*cleanly shutdown*/ failure => /*timed out*/ System.Diagnostics.Process.GetCurrentProcess().Kill())); ControlPanelPort.Post(new DsspDefaultDrop()); Activate(Arbiter.ReceiveWithIterator <DsspDefaultDrop>(false, this.mainPort, this.DropHandler)); shutdownOrTimedOut.Post(SuccessResult.Instance); yield break; }
/// <summary> /// Try finding the hid. May return failure if not found. /// </summary> public SuccessFailurePort TryFindTheHid() { Tracer.Trace("ProximityBoardCcrServiceCommander::TryFindTheHid()"); SuccessFailurePort port = new SuccessFailurePort(); if (_pmManager.PicPxMod.FindTheHid()) { port.Post(new SuccessResult()); } else { port.Post(new Exception("HID not found")); } return(port); }
/// <summary> /// Sets the Baud rate used to communicate with the LRF. /// Acceptable values are 38400, 19200 and 9600 /// </summary> public SuccessFailurePort SetDataRate(int rate) { Packet packet; switch (rate) { case 38400: packet = Packet.MonitoringMode(0x40); break; case 19200: packet = Packet.MonitoringMode(0x41); break; case 9600: packet = Packet.MonitoringMode(0x42); break; default: SuccessFailurePort port = new SuccessFailurePort(); port.Post(new ArgumentException("Baud Rate (only 9600, 19200 and 38400 supported)")); return(port); } _rate = rate; return(Send(packet)); }
public SuccessFailurePort MoveTo( float FRShoulderFB, float FRShoulderUD, float FRElbow, float MRShoulderFB, float MRShoulderUD, float MRElbow, float RRShoulderFB, float RRShoulderUD, float RRElbow, float FLShoulderFB, float FLShoulderUD, float FLElbow, float time) { SuccessFailurePort responsePort = new SuccessFailurePort(); if (_moveToActive) { responsePort.Post(new Exception("Previous MoveTo still active.")); return(responsePort); } // check bounds. If the target is invalid, post an exception message to the response port with a helpful error. // set the target values on the joint descriptors _joints[0].Target = FRShoulderFB; _joints[1].Target = FRShoulderUD; _joints[2].Target = FRElbow; _joints[3].Target = MRShoulderFB; _joints[4].Target = MRShoulderUD; _joints[5].Target = MRElbow; _joints[6].Target = RRShoulderFB; _joints[7].Target = RRShoulderUD; _joints[8].Target = RRElbow; _joints[9].Target = FLShoulderFB; _joints[10].Target = FLShoulderUD; _joints[11].Target = FLElbow; // calculate a speed value for each joint that will cause it to complete its motion in the specified time for (int i = 0; i < _joints.Length; i++) { _joints[i].Speed = Math.Abs(_joints[i].Target - _joints[i].Current) / time; } // set this flag so that the motion is evaluated in the update method _moveToActive = true; // keep a pointer to the response port so we can post a result message to it. _moveToResponsePort = responsePort; return(responsePort); }
public override void Update(FrameUpdate update) { // The joint member in each joint description needs to be set after all of the joints // have been created. If this has not been done yet, do it now. if (_joints[0].Joint == null) { int index = 0; foreach (VisualEntity child in Children) { // no joints in the camera //if (child.GetType() == typeof(AttachedCameraEntity)) // continue; _joints[index + 0].Joint = (PhysicsJoint)child.ParentJoint; // _joints[index + 1].Joint = (PhysicsJoint)child.Children[0].ParentJoint; // _joints[index + 2].Joint = (PhysicsJoint)child.Children[0].Children[0].ParentJoint; // _joints[index + 3].Joint = (PhysicsJoint)child.Children[0].Children[0].Children[0].ParentJoint; // _joints[index + 4].Joint = (PhysicsJoint)child.Children[0].Children[0].Children[0].Children[0].ParentJoint; // _joints[index + 5].Joint = (PhysicsJoint)child.Children[0].Children[0].Children[0].Children[0].Children[0].ParentJoint; index += 6; } } base.Update(update); // update joints if necessary if (_moveToActive) { bool done = true; // Check each joint and update it if necessary. for (int index = 0; index < _joints.Length; index++) { if (_joints[index].NeedToMove(_epsilon)) { done = false; Vector3 normal = _joints[index].Joint.State.Connectors[0].JointAxis; _joints[index].UpdateCurrent(_prevTime); _joints[index].Joint.SetAngularDriveOrientation( Quaternion.FromAxisAngle(1, 0, 0, DegreesToRadians(_joints[index].Current))); } } if (done) { // no joints needed to be updated, the movement is finished _moveToActive = false; _moveToResponsePort.Post(new SuccessResult()); } } _prevTime = update.ElapsedTime; }
SuccessFailurePort SetFormat(Format format) { SuccessFailurePort resultPort = new SuccessFailurePort(); var setFormat = new SetFormatRequest { Width = format.Width, Height = format.Height, MinFrameRate = format.MinFramesPerSecond, MaxFrameRate = format.MaxFramesPerSecond, Compression = format.FourCcCompression }; setFormat.Send(_client); Activate( Arbiter.Receive <HresultResponse>(false, _pipeDataPort, delegate(HresultResponse result) { if (result.hr == 0) { resultPort.Post(SuccessResult.Instance); } else { resultPort.Post(Marshal.GetExceptionForHR(result.hr)); } }, test => test.type == WebcamResponse.SetFormat ) ); return(resultPort); }
public virtual IEnumerator <ITask> AlertHandler(Alert alert) { SuccessFailurePort result = new SuccessFailurePort(); AlertForm form = null; RunForm runForm = new RunForm( delegate() { form = new AlertForm(result); form.Message = alert.Body.Message; form.Countdown = _defaultTimeout; return(form); } ); WinFormsServicePort.Post(runForm); yield return(Arbiter.Choice( runForm.pResult, delegate(SuccessResult success){}, delegate(Exception e) { result.Post(e); } )); yield return(Arbiter.Choice( result, delegate(SuccessResult success) { alert.ResponsePort.Post(DefaultSubmitResponseType.Instance); if (form.Timeout) { LogWarning("Alert dialog timed out."); } }, delegate(Exception e) { Fault fault = Fault.FromException(e); LogError(null, "Error in Alert Handler", fault); alert.ResponsePort.Post(fault); } )); }
public SuccessFailurePort SetRate() { SuccessFailurePort port; if (_rate != 0) { SerialIOManager.SetRate setRate = new SerialIOManager.SetRate(_rate); _serial.OperationsPort.Post(setRate); port = setRate.ResponsePort; } else { port = new SuccessFailurePort(); port.Post(new Exception("Rate not set")); } return(port); }
// This is the basic method used to move the arm. The target position for each joint is specified along // with a time for the movement to be completed. A port is returned which will receive a success message when the // movement is completed or an exception message if an error is encountered. public SuccessFailurePort MoveTo( float baseVal, float shoulderVal, float elbowVal, float wristVal, float rotateVal, float gripperVal, float time) { var responsePort = new SuccessFailurePort(); if (_moveToActive) { responsePort.Post(new Exception("Previous MoveTo still active.")); return responsePort; } var values = new[] { baseVal, shoulderVal, elbowVal, wristVal, rotateVal, gripperVal}; // check bounds. If the target is invalid, post an exception message to the response port with a helpful error. for (int i = 0; i < _joints.Count; i++) { var val = values[i]; if (!_joints[i].ValidTarget(val)) { responsePort.Post(new Exception(_joints[i].Name + "Joint set to invalid value: " + val)); return responsePort; } } /* if ((_joints[5].Target > gripperVal) && (Payload == null)) { _attachPayload = true; } else if ((_joints[5].Target < gripperVal) && (Payload != null)) { _dropPayload = true; } */ // set the target values on the joint descriptors for (int i = 0; i < _joints.Count; i++) _joints[i].Target = values[i]; // calculate a speed value for each joint that will cause it to complete its motion in the specified time for (int i = 0; i < _joints.Count; i++) _joints[i].Speed = Math.Abs(_joints[i].Target - _joints[i].Current) / time; // set this flag so that the motion is evaluated in the update method _moveToActive = true; // keep a pointer to the response port so we can post a result message to it. _moveToResponsePort = responsePort; return responsePort; }
/// <summary> /// Posts the appropriate response when handling the dialog being closed /// </summary> /// <param name="e"></param> protected override void OnClosed(EventArgs e) { base.OnClosed(e); _result.Post(SuccessResult.Instance); }
/// <summary> /// Loads a grammar into the speech recognition engine; MUST run exclusive /// </summary> /// <param name="request">Request that contains the new grammar to load</param> /// <param name="response">Response port</param> /// <returns></returns> private IEnumerator <ITask> LoadGrammar(LoadGrammarRequest request, SuccessFailurePort response) { #region Build grammar // Build grammar if (request.GrammarType == GrammarType.Srgs) { // Load SRGS grammar file FileReaderPort fileReaderPort = new FileReaderPort(); yield return(new IterativeTask(delegate { return ReadFileFromMountService(request.SrgsFileLocation, fileReaderPort); })); Exception fileReaderException = (Exception)fileReaderPort; if (fileReaderException != null) { LogWarning(fileReaderException); response.Post(fileReaderException); yield break; } try { grammarToLoad = GrammarUtilities.BuildSrgsGrammar((MemoryStream)fileReaderPort); } catch (Exception ex) { LogWarning(ex); response.Post(ex); yield break; } } else { // Build dictionary-style grammar try { grammarToLoad = GrammarUtilities.BuildDictionaryGrammar(request.DictionaryGrammar); } catch (Exception ex) { LogWarning(ex); response.Post(ex); yield break; } } #endregion #region Load grammar and start engine if necessary // Request chance to update recognition engine and cancel current recognition // operation state.Recognizer.RequestRecognizerUpdate(); state.Recognizer.RecognizeAsyncCancel(); yield return((Choice)loadGrammarResponsePort); Exception loadGrammarException = (Exception)loadGrammarResponsePort; if (loadGrammarException != null) { LogWarning(loadGrammarException); response.Post(loadGrammarException); yield break; } // Empty response port SuccessResult loadGrammarSuccess = (SuccessResult)loadGrammarResponsePort; // Start engine again if (state.Recognizer.Grammars.Count > 0 && !state.IgnoreAudioInput) { state.Recognizer.RecognizeAsync(RecognizeMode.Multiple); } #endregion // Store information about the new grammar in the service's state this.state.Grammar = grammarToLoad; this.state.GrammarType = request.GrammarType; if (request.GrammarType == GrammarType.Srgs) { this.state.SrgsFileLocation = request.SrgsFileLocation; this.state.DictionaryGrammar = null; } else { this.state.DictionaryGrammar = request.DictionaryGrammar; this.state.SrgsFileLocation = null; } response.Post(SuccessResult.Instance); }
// This method calculates the joint angles necessary to place the arm into the // specified position. The arm position is specified by the X,Y,Z coordinates // of the gripper tip as well as the angle of the grip, the rotation of the grip, // and the open distance of the grip. The motion is completed in the // specified time. public SuccessFailurePort MoveToPosition( float mx, // x position float my, // y position float mz, // z position float p, // angle of the grip float w, // rotation of the grip float grip, // distance the grip is open float time) // time to complete the movement { float baseAngle, shoulder, elbow, wrist; _moveTargetEntity.Position = new xna.Vector3(mx, my, mz); if (!_kinematics.InverseKinematics(mx, my, mz, p, out baseAngle, out shoulder, out elbow, out wrist)) { var s = new SuccessFailurePort(); s.Post(new Exception("Inverse Kinematics failed")); return s; } // Update the form with these parameters WinFormsServicePort.FormInvoke(() => _form.SetPositionText(mx, my, mz, p, w, grip, time)); // Update the form with these parameters WinFormsServicePort.FormInvoke(() => _form.SetJointsText(baseAngle, shoulder, elbow, wrist, w, grip, time)); var result = _robotArm.MoveTo( baseAngle, shoulder, elbow, wrist, w, grip, time); return result; }
/// <summary> /// Writes a binary file from a stream to the mount service /// </summary> /// <param name="filename">Filename and path where the file shall be store on the mount service</param> /// <param name="fileStream">Stream containing the file data</param> /// <param name="responsePort">File writer response port</param> /// <returns></returns> private IEnumerator <ITask> WriteFileToMountService( string filename, Stream fileStream, SuccessFailurePort responsePort) { // Stream needs to support seeking, otherwise we will never know when the end // is reached if (!fileStream.CanSeek) { throw new ArgumentException("File stream needs to support seeking"); } // Construct URI to file string fileUri = "http://localhost" + ServicePaths.MountPoint; if (!filename.StartsWith("/")) { fileUri += "/"; } fileUri += filename; // Establish channel with mount service mnt.MountServiceOperations mountPort = ServiceForwarder <mnt.MountServiceOperations>(fileUri); // Set up byte update message mnt.UpdateBytesRequest updateBytesRequest = new mnt.UpdateBytesRequest(); mnt.UpdateBytes updateBytes = new mnt.UpdateBytes(updateBytesRequest); updateBytesRequest.Offset = 0; updateBytesRequest.Truncate = false; // Write file in blocks to mount service updateBytesRequest.Data = new byte[MountServiceWriteBlockSize]; fileStream.Position = 0; int writeOffset = 0; while (fileStream.Position < fileStream.Length) { // Fill buffer int offset = 0; while (offset < MountServiceWriteBlockSize && fileStream.Position < fileStream.Length) { int bytesRead = fileStream.Read( updateBytesRequest.Data, offset, MountServiceWriteBlockSize - offset ); offset += bytesRead; } if (offset < MountServiceWriteBlockSize) { // Last message will most probably not contain a completely filled buffer Array.Resize <byte>(ref updateBytesRequest.Data, offset); } if (fileStream.Position >= fileStream.Length) { // End of stream reached, truncate file on mount service // to current position updateBytesRequest.Truncate = true; } updateBytesRequest.Offset = writeOffset; // Send message to mount service mountPort.Post(updateBytes); yield return((Choice)updateBytes.ResponsePort); Fault fault = (Fault)updateBytes.ResponsePort; if (fault != null) { Exception exception = fault.ToException(); LogWarning(exception); responsePort.Post(exception); yield break; } // Clear response port DefaultUpdateResponseType success = (DefaultUpdateResponseType)updateBytes.ResponsePort; writeOffset += updateBytesRequest.Data.Length; } responsePort.Post(SuccessResult.Instance); }