/// <summary>
/// Called on a rotate single axis.
/// </summary>
/// <param name="rotate">The rotation.</param>
private void OnRotateSingleAxis(pantilt.Rotate rotate)
{
var degrees = rotate.Body.RotateTiltRequest.TargetRotationAngleInRadians * 180 / Math.PI;
this.state.TiltAngle = Math.Round(degrees);
var angle = (int)this.state.TiltAngle;
var update = new FormInvoke(() =>
{
this.dashboardForm.TiltTextbox.Text = angle.ToString();
});
WinFormsServicePort.Post(update);
}
/// <summary>
/// Updates the tilt angle.
/// </summary>
/// <param name="rotationAngleDegrees">Angle to rotate to</param>
private void UpdateTiltAngle(double rotationAngleDegrees)
{
this.panTiltState.TiltState.JointFeedback.RotationAngleInRadians
= rotationAngleDegrees / DegreesPerRadian;
var rotate = new pantilt.Rotate();
rotate.Body.RotateTiltRequest = new saj.RotateSingleAxisRequest();
rotate.Body.RotateTiltRequest.TargetRotationAngleInRadians =
this.panTiltState.TiltState.JointFeedback.RotationAngleInRadians;
rotate.Body.RotateTiltRequest.IsRelative = false;
SendNotification(this.submgrPort, rotate);
}
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;
}
