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