private async void TranslateGestureForUAV1(GestureRecongizedArgs args)
{
FlightOperationMode _mode = _uav.mode;
if (_mode == FlightOperationMode.landingOrTakingOff)
{
return;
}
// Switch Mode
if (args.rightHandGesture.leftRightGestureArgs == LeftRightGesture.Right)
{
if (_mode == FlightOperationMode.idle)
{
await SwitchMode(FlightOperationMode.navigate, _uav);
}
else
{
await SwitchMode(FlightOperationMode.idle, _uav);
}
}
// Control landing and takeoff
if (_mode == FlightOperationMode.idle)
{
if (args.rightHandGesture.upwardDownwardGestureArgs == UpwardDownwardGesture.Down && !_uav.isLanded)
{
await SwitchMode(FlightOperationMode.landingOrTakingOff, _uav);
await _uav.landing();
await SwitchMode(FlightOperationMode.idle, _uav);
}
else if (args.rightHandGesture.upwardDownwardGestureArgs == UpwardDownwardGesture.Up && _uav.isLanded)
{
await SwitchMode(FlightOperationMode.landingOrTakingOff, _uav);
await _uav.takeOff();
await SwitchMode(FlightOperationMode.idle, _uav);
}
}
}
public void detect(Body body)
{
if (body != null)
{
GestureRecongizedArgs args = new GestureRecongizedArgs();
args.leftHandGesture = new Gesture
{
forwardBackwardGestureArgs = compareForwardBackward(body.Joints[JointType.ShoulderLeft], body.Joints[JointType.ElbowLeft]),
upwardDownwardGestureArgs = compareUpDown(body.Joints[JointType.ElbowLeft], body.Joints[JointType.WristLeft]),
leftRightGestureArgs = compareLeftRight(body.Joints[JointType.ElbowLeft], body.Joints[JointType.WristLeft])
};
args.rightHandGesture = new Gesture
{
forwardBackwardGestureArgs = compareForwardBackward(body.Joints[JointType.ShoulderRight], body.Joints[JointType.ElbowRight]),
upwardDownwardGestureArgs = compareUpDown(body.Joints[JointType.ElbowRight], body.Joints[JointType.WristRight]),
leftRightGestureArgs = compareLeftRight(body.Joints[JointType.ElbowRight], body.Joints[JointType.WristRight])
};
GestureRecongized.Invoke(this, args);
}
}
private async void TranslateGestureForUAV2(GestureRecongizedArgs args)
{
FlightOperationMode _mode = _uav2.mode;
if (_uav2.mode == FlightOperationMode.landingOrTakingOff)
{
return;
}
if (args.leftHandGesture.leftRightGestureArgs == LeftRightGesture.Left)
{
if (_mode == FlightOperationMode.idle)
{
await SwitchMode(FlightOperationMode.navigate, _uav, 3000);
}
else
{
await SwitchMode(FlightOperationMode.idle, _uav, 3000);
}
}
if (_mode == FlightOperationMode.idle)
{
if (args.leftHandGesture.upwardDownwardGestureArgs == UpwardDownwardGesture.Down && !_uav2.isLanded)
{
await SwitchMode(FlightOperationMode.landingOrTakingOff, _uav2);
await _uav2.landing();
await SwitchMode(FlightOperationMode.idle, _uav2);
}
else if (args.leftHandGesture.upwardDownwardGestureArgs == UpwardDownwardGesture.Up && _uav2.isLanded)
{
await SwitchMode(FlightOperationMode.landingOrTakingOff, _uav2);
await _uav2.takeOff();
await SwitchMode(FlightOperationMode.idle, _uav2);
}
}
}
private void _gestureDetector_GestureRecongized(object sender, GestureRecongizedArgs e)
{
lhForwardBackwardGestureStatus.Text = e.leftHandGesture.forwardBackwardGestureArgs.ToString();
lhLeftRightGestureStatus.Text = e.leftHandGesture.leftRightGestureArgs.ToString();
lhUpDownGestureStatus.Text = e.leftHandGesture.upwardDownwardGestureArgs.ToString();
rhForwardBackwardGestureStatus.Text = e.rightHandGesture.forwardBackwardGestureArgs.ToString();
rhLeftRightGestureStatus.Text = e.rightHandGesture.leftRightGestureArgs.ToString();
rhUpDownGestureStatus.Text = e.rightHandGesture.upwardDownwardGestureArgs.ToString();
if (_flightController != null)
{
try
{
_flightController.TranslateGestureToFlightOperation(e);
}catch (Exception ex)
{
_flightController.emergency();
log("Failed to translate gesture to command");
log(ex.Message);
}
}
}
public async virtual void TranslateGestureToFlightOperation(GestureRecongizedArgs args)
{
if (_uav == null || !_uav.isConnected)
{
return;
}
if (_uav.mode == FlightOperationMode.landingOrTakingOff)
{
return;
}
if (_uav.mode == FlightOperationMode.idle)
{
if (args.rightHandGesture.upwardDownwardGestureArgs == UpwardDownwardGesture.Down && !_uav.isLanded)
{
_mw.log("islanding");
await SwitchMode(FlightOperationMode.landingOrTakingOff, _uav);
try
{
await _uav.landing();
}catch (Exception ex)
{
}
_uav.isLanded = true;
await SwitchMode(FlightOperationMode.idle, _uav);
}
else if (args.rightHandGesture.upwardDownwardGestureArgs == UpwardDownwardGesture.Up && _uav.isLanded)
{
_mw.log("is takeoff");
await SwitchMode(FlightOperationMode.landingOrTakingOff, _uav);
try
{
await _uav.takeOff();
}catch (Exception ex)
{
}
_uav.isLanded = false;
await SwitchMode(FlightOperationMode.idle, _uav);
}
}
if (args.rightHandGesture.forwardBackwardGestureArgs == ForwardBackwardGesture.Forward && !_uav.isLanded)
{
if (_uav.mode == FlightOperationMode.idle)
{
await SwitchMode(FlightOperationMode.navigate, _uav, 1000);
}
else
{
await SwitchMode(FlightOperationMode.idle, _uav, 1000);
}
}
if (_uav.mode == FlightOperationMode.navigate && !_uav.isLanded)
{
Twist twist = new Twist
{
angular = new Vector
{
x = 0,
y = 0,
z = 0
},
linear = new Vector
{
x = 0,
y = 0,
z = 0
}
};
switch (args.leftHandGesture.forwardBackwardGestureArgs)
{
case ForwardBackwardGesture.Forward:
twist.linear.x = 0.2f;
break;
case ForwardBackwardGesture.Backward:
twist.linear.x = -0.2f;
break;
case ForwardBackwardGesture.None:
twist.linear.x = 0;
break;
}
switch (args.leftHandGesture.upwardDownwardGestureArgs)
{
case UpwardDownwardGesture.Up:
twist.linear.z = 0.2f;
break;
case UpwardDownwardGesture.Down:
twist.linear.z = -0.2f;
break;
case UpwardDownwardGesture.None:
twist.linear.z = 0;
break;
}
switch (args.leftHandGesture.leftRightGestureArgs)
{
case LeftRightGesture.Left:
twist.linear.y = 0.2f;
break;
case LeftRightGesture.Right:
twist.linear.y = -0.2f;
break;
case LeftRightGesture.None:
twist.linear.y = 0;
break;
}
switch (args.rightHandGesture.leftRightGestureArgs)
{
case LeftRightGesture.Left:
twist.angular.z = 0.3f;
break;
case LeftRightGesture.Right:
twist.angular.z = -0.3f;
break;
case LeftRightGesture.None:
twist.linear.y = 0;
break;
}
try
{
await _uav.navigate(twist);
}catch (Exception ex)
{
}
}
}
public override async void TranslateGestureToFlightOperation(GestureRecongizedArgs args)
{
if (_uav != null && _uav.isConnected)
{
TranslateGestureForUAV1(args);
}
if (_uav2 != null && _uav2.isConnected)
{
TranslateGestureForUAV2(args);
}
//if (_mode == FlightOperationMode.navigate)
//{
// Twist twist = new Twist
// {
// angular = new Vector
// {
// x = 0,
// y = 0,
// z = 0
// },
// linear = new Vector
// {
// x = 0,
// y = 0,
// z = 0
// }
// };
// switch (args.leftHandGesture.forwardBackwardGestureArgs)
// {
// case ForwardBackwardGesture.Backward:
// twist.linear.x = -1;
// break;
// case ForwardBackwardGesture.Forward:
// twist.linear.x = -1;
// break;
// case ForwardBackwardGesture.None:
// twist.linear.x = 0;
// break;
// }
// switch (args.leftHandGesture.upwardDownwardGestureArgs)
// {
// case UpwardDownwardGesture.Up:
// twist.linear.z = 1;
// break;
// case UpwardDownwardGesture.Down:
// twist.linear.z = -1;
// break;
// case UpwardDownwardGesture.None:
// twist.linear.z = 0;
// break;
// }
// switch (args.leftHandGesture.leftRightGestureArgs)
// {
// case LeftRightGesture.Left:
// twist.linear.y = 1;
// break;
// case LeftRightGesture.Right:
// twist.linear.y = -1;
// break;
// case LeftRightGesture.None:
// twist.linear.y = 0;
// break;
// }
// switch (args.rightHandGesture.leftRightGestureArgs)
// {
// case LeftRightGesture.Left:
// twist.angular.z = 1;
// break;
// case LeftRightGesture.Right:
// twist.angular.z = -1;
// break;
// case LeftRightGesture.None:
// twist.linear.y = 0;
// break;
// }
// await navigate(twist);
//}
}