public async Task navigate(Twist twist) { if (md != null) { if (navigatePub == null) { navigatePub = new Publisher("/bebop/cmd_vel", "geometry_msgs/Twist", md); await navigatePub.AdvertiseAsync(); } //Publisher publisher = new Publisher("/bebop/cmd_vel", "geometry_msgs/Twist", md); //await publisher.AdvertiseAsync(); await navigatePub.PublishAsync(twist); //await navigatePub.UnadvertiseAsync(); //publisher = null; } }
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) { } } }