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)
{
}
}
}
