private void Vertical(NormalizedHands hands, MoveCommand moveCommand)
{
double deltaY = this.neutralHands.Center.Y - hands.Left.Y;
double deltaX = this.neutralHands.Center.X - hands.Left.X;
double gradient = deltaY / deltaX;
double alpha = this.ConvertRadiansToDegrees(Math.Atan(gradient));
deltaY = hands.Right.Y - this.neutralHands.Center.Y;
deltaX = hands.Right.X - this.neutralHands.Center.X;
gradient = deltaY / deltaX;
double beta = this.ConvertRadiansToDegrees(Math.Atan(gradient));
double deltaAngle = alpha - beta;
bool areBothHandsUp = this.neutralHands.Left.Y > hands.Left.Y && this.neutralHands.Right.Y > hands.Right.Y;
bool areBothHandsDown = this.neutralHands.Left.Y < hands.Left.Y && this.neutralHands.Right.Y < hands.Right.Y;
double angle = 180 - Math.Abs(deltaAngle);
if (angle < VERTICAL_ANGLE_THRESHOLD)
{
if (deltaAngle < 0 && areBothHandsDown)
{
moveCommand.Vertical = MOVE_DOWN;
}
else if (deltaAngle > 0 && areBothHandsUp)
{
moveCommand.Vertical = MOVE_UP;
}
}
}
private void Lateral(NormalizedHands hands, MoveCommand moveCommand)
{
double deltaY = this.neutralHands.Right.Y - this.neutralHands.Left.Y;
double deltaX = this.neutralHands.Right.X - this.neutralHands.Left.X;
double neutralGradient = deltaY / deltaX;
double neutralAngle = this.ConvertRadiansToDegrees(Math.Atan(neutralGradient));
deltaY = hands.Right.Y - hands.Left.Y;
deltaX = hands.Right.X - hands.Left.X;
double actualGradient = deltaY / deltaX;
double actualAngle = this.ConvertRadiansToDegrees(Math.Atan(actualGradient));
double deltaAngle = neutralAngle - actualAngle;
double absoluteDeltaAngle = Math.Abs(deltaAngle);
if (LATERAL_ANGLE_THRESHOLD < absoluteDeltaAngle)
{
if (deltaAngle < 0)
{
moveCommand.Lateral = MOVE_RIGHT;
}
else if (0 < deltaAngle)
{
moveCommand.Lateral = MOVE_LEFT;
}
}
}
/// <summary>
/// Processes a hands input.
/// </summary>
/// <param name="hands">The key input to evaluate.</param>
/// <returns>The evaluation result of the <paramref name="hands"/>.</returns>
public InputProcessResult ProcessHandsInput(NormalizedHands hands)
{
this.LatestHandsInputEvaluated = this.handsInputEvaluator.EvaluateHands(hands);
DroneControllerHandsInputProcessResult result =
new DroneControllerHandsInputProcessResult(this.LatestHandsInputEvaluated.Copy() as MoveCommand);
this.Control();
return(result);
}
public void KeyInputIsSentAfterHandsInput_KeyInputIsNotIgnored()
{
var skinColorHandsDetectorMock = new Mock <ISkinColorHandsDetector>();
skinColorHandsDetectorMock.Setup(schd => schd.Tuned).Returns(true);
var left = new NormalizedHand(0.15, 0.2, 0.18);
var right = new NormalizedHand(0.85, 0.2, 0.18);
var hands = new NormalizedHands(left, right);
var handsDetectorResult = new HandsDetectorResult(hands, null);
skinColorHandsDetectorMock.Setup(schd => schd.DetectHands(It.IsAny <BgrImage>()))
.Returns(handsDetectorResult);
var keyInputEvaluator = new GeneralDroneKeyInputEvaluator();
var handsInputEvaluator = new DroneControllerHandsInputEvaluator();
var neutralLeft = new NormalizedHand(0.15, 0.5, 0.18);
var neutralRight = new NormalizedHand(0.85, 0.5, 0.18);
var neutralHands = new NormalizedHands(neutralLeft, neutralRight);
handsInputEvaluator.Tune(neutralHands);
this.caduhdApp = new CaduhdApp(null, skinColorHandsDetectorMock.Object, this.tello, keyInputEvaluator, handsInputEvaluator);
this.caduhdApp.Input(BgrImage.GetBlank(1, 1, Color.Black));
this.udpClient.Client.ReceiveTimeout = 1000;
string response = string.Empty;
bool handsInputIgnored = false;
try
{
response = this.udpClient.Receive(ref this.anyEndPoint).AsString();
Assert.Equal($"rc 0 0 {this.tello.Speed} 0", response);
}
catch (SocketException)
{
}
this.caduhdApp.Input(new KeyInfo(Key.D, KeyState.Down));
try
{
response = this.udpClient.Receive(ref this.anyEndPoint).AsString();
}
catch (SocketException e)
{
if (e.SocketErrorCode == SocketError.TimedOut)
{
handsInputIgnored = true;
}
}
this.tello.Dispose();
Assert.False(handsInputIgnored);
Assert.Equal($"rc 0 0 0 {this.tello.Speed}", response);
}
/// <summary>
/// This is the entry point of the hands into the Caduhd App.
/// </summary>
/// <param name="image">The image, possibly containing hands.</param>
public void Input(BgrImage image)
{
if (Interlocked.CompareExchange(ref this.isWebCameraFrameProcessorBusy, YES, NO) == NO)
{
if (this.handsAnalyzer != null)
{
this.uiConnector.SetHandsAnalyzerState(this.handsAnalyzer.State);
}
BgrImage frame = image;
MoveCommand moveCommand = null;
if (this.skinColorHandsDetector.Tuned)
{
HandsDetectorResult result = this.skinColorHandsDetector.DetectHands(frame);
frame = result.Image;
var hands = result.Hands;
moveCommand =
(this.handsDroneController.ProcessHandsInput(hands) as DroneControllerHandsInputProcessResult)?.Result;
}
else
{
if (this.handsAnalyzer.State == HandsAnalyzerState.ReadyToAnalyzeLeft || this.handsAnalyzer.State == HandsAnalyzerState.AnalyzingLeft)
{
if (this.handsAnalyzer.State == HandsAnalyzerState.AnalyzingLeft)
{
this.handsAnalyzer.AnalyzeLeft(frame, this.handsDroneController.HandsInputEvaluator.TunerHands["left"]["poi"]);
this.handsAnalyzer.AdvanceState();
}
frame.MarkPoints(this.handsDroneController.HandsInputEvaluator.TunerHands["left"]["outline"], Color.Yellow);
}
else if (this.handsAnalyzer.State == HandsAnalyzerState.ReadyToAnalyzeRight || this.handsAnalyzer.State == HandsAnalyzerState.AnalyzingRight)
{
if (this.handsAnalyzer.State == HandsAnalyzerState.AnalyzingRight)
{
this.handsAnalyzer.AnalyzeRight(frame, this.handsDroneController.HandsInputEvaluator.TunerHands["right"]["poi"]);
this.handsAnalyzer.AdvanceState();
}
frame.MarkPoints(this.handsDroneController.HandsInputEvaluator.TunerHands["right"]["outline"], Color.Yellow);
}
else if (this.handsAnalyzer.State == HandsAnalyzerState.Tuning)
{
HandsAnalyzerResult result = this.handsAnalyzer.Result;
NormalizedHands neutralHands = this.skinColorHandsDetector.Tune(result);
this.handsDroneController.HandsInputEvaluator.Tune(neutralHands);
}
}
this.uiConnector?.SetComputerCameraImage(frame);
this.uiConnector?.SetEvaluatedHandsInput(moveCommand);
Interlocked.Exchange(ref this.isWebCameraFrameProcessorBusy, NO);
}
}
/// <summary>
/// Evaluates hands to <see cref="MoveCommand"/>.
/// </summary>
/// <param name="hands">Hands to evaluate.</param>
/// <returns>The evaluated hands as a <see cref="MoveCommand"/>.</returns>
public MoveCommand EvaluateHands(NormalizedHands hands)
{
MoveCommand moveCommand = new MoveCommand();
this.Lateral(hands, moveCommand);
this.Longitudinal(hands, moveCommand);
this.Vertical(hands, moveCommand);
this.Yaw(hands, moveCommand);
return(moveCommand);
}
public HandsDroneControllerTests()
{
this.controllableDroneMock = new Mock <AbstractDrone>();
this.keyInputEvaluatorMock = new Mock <IDroneControllerKeyInputEvaluator>();
this.handsInputEvaluatorMock = new Mock <IDroneControllerHandsInputEvaluator>();
this.droneController = new HandsDroneController(this.controllableDroneMock.Object,
this.handsInputEvaluatorMock.Object,
this.keyInputEvaluatorMock.Object);
this.hands = new NormalizedHands(new NormalizedHand(), new NormalizedHand());
}
public void DifferentHandsInputsAfterEachOther_BothSent()
{
var skinColorHandsDetectorMock = new Mock <ISkinColorHandsDetector>();
skinColorHandsDetectorMock.Setup(schd => schd.Tuned).Returns(true);
var firstLeft = new NormalizedHand(0.15, 0.2, 0.18);
var firstRight = new NormalizedHand(0.85, 0.2, 0.18);
var firstHands = new NormalizedHands(firstLeft, firstRight);
var firstHandsDetectorResult = new HandsDetectorResult(firstHands, null);
var secondLeft = new NormalizedHand(0.15, 0.8, 0.18);
var secondRight = new NormalizedHand(0.85, 0.7, 0.18);
var secondHands = new NormalizedHands(secondLeft, secondRight);
var secondandsDetectorResult = new HandsDetectorResult(secondHands, null);
skinColorHandsDetectorMock.SetupSequence(schd => schd.DetectHands(It.IsAny <BgrImage>()))
.Returns(firstHandsDetectorResult)
.Returns(secondandsDetectorResult);
var keyInputEvaluator = new GeneralDroneKeyInputEvaluator();
var handsInputEvaluator = new DroneControllerHandsInputEvaluator();
var neutralLeft = new NormalizedHand(0.15, 0.5, 0.18);
var neutralRight = new NormalizedHand(0.85, 0.5, 0.18);
var neutralHands = new NormalizedHands(neutralLeft, neutralRight);
handsInputEvaluator.Tune(neutralHands);
this.caduhdApp = new CaduhdApp(null, skinColorHandsDetectorMock.Object, this.tello, keyInputEvaluator, handsInputEvaluator);
this.caduhdApp.Input(BgrImage.GetBlank(1, 1, Color.Black));
this.caduhdApp.Input(BgrImage.GetBlank(1, 1, Color.Black));
this.udpClient.Client.ReceiveTimeout = 1000;
int counter = 0;
try
{
string response = this.udpClient.Receive(ref this.anyEndPoint).AsString();
Assert.Equal($"rc 0 0 {this.tello.Speed} 0", response);
counter++;
response = this.udpClient.Receive(ref this.anyEndPoint).AsString();
Assert.Equal($"rc 0 0 -{this.tello.Speed} 0", response);
counter++;
}
catch (SocketException)
{
}
this.tello.Dispose();
Assert.Equal(2, counter);
}
private void Yaw(NormalizedHands hands, MoveCommand moveCommand)
{
double handsWeightRatio = hands.Left.Weight / (this.neutralHands.RatioOfLeftWeightToRightWeight * hands.Right.Weight);
if (handsWeightRatio < YAW_LOWER_THRESHOLD)
{
moveCommand.Yaw = YAW_LEFT;
}
else if (handsWeightRatio > YAW_UPPDER_THRESHOLD)
{
moveCommand.Yaw = YAW_RIGHT;
}
}
private void Longitudinal(NormalizedHands hands, MoveCommand moveCommand)
{
double leftRatio = hands.Left.Weight / this.neutralHands.Left.Weight;
double rightRatio = hands.Right.Weight / this.neutralHands.Right.Weight;
if (LONGITUDINAL_UPPER_THRESHOLD < leftRatio && LONGITUDINAL_UPPER_THRESHOLD < rightRatio)
{
moveCommand.Longitudinal = MOVE_FORWARD;
}
else if (leftRatio < LONGITUDINAL_LOWER_THRESHOLD && rightRatio < LONGITUDINAL_LOWER_THRESHOLD)
{
moveCommand.Longitudinal = MOVE_BACKWARD;
}
}
public void HandsInputAnalyzerTuned_HandsInput_MoveDownwards_HandsInputEvaluated_CorrectMoveCommandSent()
{
var skinColorHandsDetectorMock = new Mock <ISkinColorHandsDetector>();
skinColorHandsDetectorMock.Setup(schd => schd.Tuned).Returns(true);
var neutralLeft = new NormalizedHand(0.15, 0.5, 0.18);
var neutralRight = new NormalizedHand(0.85, 0.5, 0.18);
var neutralHands = new NormalizedHands(neutralLeft, neutralRight);
var left = new NormalizedHand(0.15, 0.8, 0.18);
var right = new NormalizedHand(0.85, 0.8, 0.18);
var hands = new NormalizedHands(left, right);
var handsDetectorResult = new HandsDetectorResult(hands, null);
skinColorHandsDetectorMock.Setup(schd => schd.DetectHands(It.IsAny <BgrImage>()))
.Returns(handsDetectorResult);
var keyInputEvaluator = new GeneralDroneKeyInputEvaluator();
var handsInputEvaluator = new DroneControllerHandsInputEvaluator();
handsInputEvaluator.Tune(neutralHands);
this.caduhdApp = new CaduhdApp(null, skinColorHandsDetectorMock.Object, this.tello, keyInputEvaluator, handsInputEvaluator);
string response = string.Empty;
this.caduhdApp.Input(BgrImage.GetBlank(1, 1, Color.Black));
try
{
response = this.udpClient.Receive(ref this.anyEndPoint).AsString();
}
catch (SocketException)
{
}
// it needs to be disposed of here
this.tello.Dispose();
Assert.Equal($"rc 0 0 -{this.tello.Speed} 0", response);
}
public HandsDetectorResultTests()
{
this.hands = new NormalizedHands(new NormalizedHand(), new NormalizedHand());
this.image = BgrImage.GetBlank(100, 100, Color.White);
this.handsDetectorResult = new HandsDetectorResult(this.hands, this.image);
}
public NormalizedHandsTests()
{
this.left = new NormalizedHand(LEFT_X, LEFT_Y, LEFT_WEIGHT);
this.right = new NormalizedHand(RIGHT_X, RIGHT_Y, RIGHT_WEIGHT);
this.hands = new NormalizedHands(this.left, this.right);
}
/// <summary> /// Tuner method to set the neutral hands as a reference for future hands input evaluation. /// </summary> /// <param name="neutralHands">Hands detected in their neutral position.</param> public void Tune(NormalizedHands neutralHands) => this.neutralHands = neutralHands;
