public void UpdateTiltHandler(UpdateTilt update) { var tilt = update.Body.Tilt; // need constants if (tilt > (short)kinectSensor.MaxElevationAngle || tilt < (short)kinectSensor.MinElevationAngle) { update.ResponsePort.Post( Fault.FromCodeSubcodeReason( FaultCodes.Sender, DsspFaultCodes.OperationFailed, Resources.InvalidTiltAngle)); } else { this.SetKinectTiltAngle((int)tilt); this.UpdateTiltAngle(this.state.TiltDegrees); update.ResponsePort.Post(DefaultUpdateResponseType.Instance); SendNotification(this.submgrPort, update); } }
public IEnumerator <ITask> OnRotateSingle(pantilt.Rotate rotate) { // Just convert this to a kinect tilt operation if (rotate.Body.RotatePanRequest != null && rotate.Body.RotatePanRequest.TargetRotationAngleInRadians != 0) { rotate.ResponsePort.Post(Fault.FromCodeSubcodeReason( FaultCodes.Receiver, DsspFaultCodes.ActionNotSupported, Resources.PanNotSupported)); yield break; } if (rotate.Body.RotateTiltRequest == null) { // no-op rotate.ResponsePort.Post(DefaultUpdateResponseType.Instance); yield break; } if (rotate.Body.RotateTiltRequest.IsMotionCompletionRequiredForResponse != false) { rotate.ResponsePort.Post(Fault.FromCodeSubcodeReason( FaultCodes.Receiver, DsspFaultCodes.ActionNotSupported, Resources.ResponseOnMotionCompletedNotSupported)); yield break; } if (rotate.Body.RotateTiltRequest.TargetAccelerationInRadiansPerSecondSecond != 0) { LogWarning(Resources.AccelerationIgnored); } if (rotate.Body.RotateTiltRequest.MaxSpeedInRadiansPerSecond != 0) { LogWarning(Resources.SpeedIgnored); } double targetTiltInDegrees; if (rotate.Body.RotateTiltRequest.IsRelative) { targetTiltInDegrees = this.state.TiltDegrees + (rotate.Body.RotateTiltRequest.TargetRotationAngleInRadians * DegreesPerRadian); } else { targetTiltInDegrees = rotate.Body.RotateTiltRequest.TargetRotationAngleInRadians * DegreesPerRadian; } this.panTiltState.TiltState.JointCommand.TargetAngleInRadians = targetTiltInDegrees / DegreesPerRadian; var updateTilt = new UpdateTilt { Body = { Tilt = targetTiltInDegrees }, ResponsePort = rotate.ResponsePort }; this.mainPort.Post(updateTilt); yield break; }