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