def __call__(self,
by=None,
to=None,
method=NotSet,
tolerance=NotSet,
timeout=NotSet):
"""Block until the vehicle is within tolerance of the target altitude.
Args:
by (Real): Change altitude by this amount. May be negative. Cannot
be set at the same time as `to`.
to (Real): Ascend or descend to this altitude. Cannot be set at the
same time as `by`.
method (Callable[[Real, Real], Real]): Take two arguments, the
current altitude and the target altitude, and return a z-axis
control value [-1, 1].
tolerance (Real): The allowable error in altitude before the method
sets the z-axis to 0 and returns.
timeout (Optional[timedelta]): If not None, the amount of time the
method has to complete an operation before raising
CommandTimeout. If None the timeout is datetime.max.
"""
# NotSet resolution
method = self.resolve(method, 'method')
timeout = self.resolve(timeout, 'timeout')
tolerance = self.resolve(tolerance, 'tolerance')
# Sanity checks
if not by and not to:
raise ValueError('Must set either by or to.')
elif by and to:
raise ValueError('Must not set both by and to.')
if not method or not callable(method):
raise ValueError('Must have a callable navigation method.')
if not isinstance(tolerance, Real) or tolerance <= 0:
raise ValueError(
'Tolerance must be set to a positive real number.')
# Timeout
# TODO Use vehicle time not local
timeout = datetime.max if timeout is None else timeout + datetime.now()
target_altitude = (self.nav.altitude + by) if by else to
while datetime.now() < timeout:
difference = abs(target_altitude - self.nav.altitude)
if difference <= tolerance:
self.info('vnav expected %s actual %s (%s < %s m)',
target_altitude, self.nav.altitude, difference,
tolerance)
self.broadcast(Intent(action=ACTION_AXIS_SET, data=('z', 0.0)))
return self
control = method(self.nav.altitude, target_altitude)
self.debug('vnav toward %.3f actual %.3f (%.3f < %.3f m) %.3f',
target_altitude, self.nav.altitude, difference,
tolerance, control)
self.broadcast(Intent(action=ACTION_AXIS_SET, data=('z', control)))
time.sleep(self.nav.sleep_time)
raise CommandTimeout('vnav exceeded timeout')
def arm(self):
"""Arm vehicle."""
print("Arming vehicle")
self.vehicle.info("Arming vehicle")
self.vehicle.broadcast(
Intent(action=ACTION_SEND_COMMAND,
data=ManualSetpoint(ArmSwitch=3))).first()
self.vehicle.broadcast(
Intent(action=ACTION_SEND_COMMAND,
data=ManualSetpoint(ArmSwitch=1))).first()
def _mode_posctl(self):
print("Position control")
self.vehicle.info("Position control")
self.vehicle.broadcast(
Intent(action=ACTION_SEND_COMMAND,
data=ManualSetpoint(Z=0.5, PosctlSwitch=1,
GearSwitch=1))).first()
def __exit__(self, exc_type, exc_val, exc_tb):
# If the context manager exits without error, all good. Otherwise...
if exc_type:
if callable(self.failure_callback):
self.failure_callback(self, (exc_type, exc_val, exc_tb))
else:
print('Error in execution. Returning to Launch.')
self._vehicle.broadcast(Intent(action=ACTION_RTL))
def _send_telemetry(self):
mod_z = self.z / 2.0 + 0.5 # [-1, 1] -> [0, 1]
self.vehicle.broadcast(Intent(
action=ACTION_SEND_COMMAND,
data=ManualSetpoint(
X=self._x, Y=self._y, Z=mod_z, R=self._r,
PosctlSwitch=1, GearSwitch=1, ArmSwitch=1)
))
def _check_fence(self):
old = self.fence_violation
self.fence_violation = self.fence_violation or \
(self.enabled and self.position not in self)
if not old and self.fence_violation:
self.vehicle.error('Encountered Fence Violation at %s',
self.position)
self.vehicle.broadcast(Intent(action=ACTION_RTL))
print('Encountered fence violation. Press Ctrl-C exit.')
def takeoff(self):
"""Takeoff"""
print("Auto takeoff")
self.vehicle.info("Auto takeoff")
self.vehicle.broadcast(
Intent(action=ACTION_SEND_COMMAND,
data=ManualSetpoint(TransitionSwitch=1,
ArmSwitch=1))).first()
time.sleep(5)
def rtl(self):
"""Return to launch."""
print("RTL")
self.vehicle.info("RTL")
self.vehicle.broadcast(
Intent(action=ACTION_SEND_COMMAND,
data=ManualSetpoint(ReturnSwitch=1,
GearSwitch=3,
ArmSwitch=1))).first()
def __call__(self,
distance,
axis=NotSet,
method=NotSet,
tolerance=NotSet,
timeout=NotSet):
"""Block until the distance traveled is within the tolerance of the
target distance.
Args:
distance (Real): Distance in meters to move.
axis (str): Must be one of the lateral movement axes (x, y, or z),
and may be prefixed by an optional "-" for negative movement.
method (Callable[[Real, Real], Real]): Callable takes two arguments,
the distance traveled and the target distance, and returns an
axis control value within [-1, 1].
tolerance (Real): The allowable error in movement before the method
sets the control to 0 and returns.
"""
# NotSet resolution
axis = self.resolve(axis, 'axis')
method = self.resolve(method, 'method')
tolerance = self.resolve(tolerance, 'tolerance')
timeout = self.resolve(timeout, 'timeout')
# Sanity checks
if not method or not callable(method):
raise ValueError('Must have a callable navigation method.')
if not isinstance(tolerance, Real) or tolerance <= 0:
raise ValueError(
'Tolerance must be set to a positive real number.')
# Match axis value
valid = '(-)?([xyz])'
axis_match = re.match(valid, axis)
if not axis_match:
raise ValueError('Axis must match "{}".'.format(valid))
neg, axis = axis_match.groups()
# Timeout
# TODO Use vehicle time not local
timeout = datetime.max if timeout is None else timeout + datetime.now()
original = self.nav.position
while datetime.now() < timeout:
delta = self.broadcast(
Intent(action=ACTION_CALC_DISTANCE,
data=(original, self.nav.position))).first_result()
if (distance - delta) < tolerance:
self.info('lnav expected %s actual %s (%s < %s m)', distance,
delta, distance - delta, tolerance)
self.broadcast(Intent(action=ACTION_AXIS_SET,
data=(axis, 0.0)))
return self
control = method(delta, distance)
control = -control if neg else control
self.debug('lnav toward %.3f actual %.3f (%.3f < %.3f m) %.3f',
distance, delta, distance - delta, tolerance, control)
self.broadcast(Intent(action=ACTION_AXIS_SET,
data=(axis, control)))
time.sleep(self.nav.sleep_time)
raise CommandTimeout('lnav exceeded timeout')
def __call__(self, waypoints, tolerance=NotSet, timeout=NotSet):
"""Block until the vehicle is within tolerance of the final waypoint.
Args:
waypoints: A single waypoint or an iterable of waypoints for the
vehicle to follow.
tolerance (Real): The allowable error in altitude before the method
sets the z-axis to 0 and returns.
timeout (Optional[timedelta]): If not None, the amount of time the
method has to complete an operation before raising
CommandTimeout. If None the timeout is datetime.max.
"""
# NotSet resolution
timeout = self.resolve(timeout, 'timeout')
tolerance = self.resolve(tolerance, 'tolerance')
# Sanity checks
if not isinstance(tolerance, Real) or tolerance <= 0:
raise ValueError('Tolerance must be set to a positive real number.')
# Timeout
# TODO Use vehicle time not local
timeout = datetime.max if timeout is None else timeout + datetime.now()
# Iterate through all given waypoints
if not isinstance(waypoints, Iterable):
waypoints = (waypoints,)
for prv, cur, nxt in shifter(3, (None,) + tuple(waypoints) + (None,)):
palt = 0 if not prv else prv.altitude \
if prv.altitude is not None else self.nav.altitude
calt = 0 if not cur else cur.altitude \
if cur.altitude is not None else self.nav.altitude
nalt = 0 if not nxt else nxt.altitude \
if nxt.altitude is not None else self.nav.altitude
pyaw = 0 if not prv else prv.yaw \
if prv.yaw is not None else self.nav.yaw
cyaw = 0 if not cur else cur.yaw \
if cur.yaw is not None else self.nav.yaw
nyaw = 0 if not nxt else nxt.yaw \
if nxt.yaw is not None else self.nav.yaw
self.broadcast(Intent(
action=ACTION_SEND_COMMAND,
data=SetpointTriplet(
Prev_Lat=prv.latitude if prv is not None else 0,
Prev_Lon=prv.longitude if prv is not None else 0,
Prev_Alt=palt, Prev_Yaw=pyaw,
Prev_Valid=prv is not None,
Prev_PositionValid=prv is not None,
Cur_Lat=cur.latitude if cur is not None else 0,
Cur_Lon=cur.longitude if cur is not None else 0,
Cur_Alt=calt, Cur_Yaw=cyaw,
Cur_Valid=cur is not None,
Cur_PositionValid=cur is not None,
Next_Lat=nxt.latitude if nxt is not None else 0,
Next_Lon=nxt.longitude if nxt is not None else 0,
Next_Alt=nalt, Next_Yaw=nyaw,
Next_Valid=nxt is not None,
Next_PositionValid=nxt is not None
)))
while True:
if datetime.now() > timeout:
raise CommandTimeout('goto exceeded timeout')
distance = self.broadcast(Intent(
action=ACTION_CALC_DISTANCE,
data=(cur, self.nav.position),
)).first_result(0.5)
if distance < tolerance:
self.nav.vehicle.info(
'goto expected %s actual %s (%s < %s m)',
cur, self.nav.position, distance, tolerance)
break
time.sleep(self.nav.sleep_time)
import time
from pyliner.action import ACTION_RTL
from pyliner.app.communication import Communication
from pyliner.intent import Intent
from pyliner.util import read_json
from pyliner.vehicle import Vehicle
vehicle = Vehicle(
vehicle_id='rocky',
communications=Communication(
airliner_map=read_json('airliner.json'),
ci_port=5009, to_port=5012
)
)
vehicle.broadcast(Intent(action=ACTION_RTL))
time.sleep(1)
def __call__(self, by=None, to=None, method=NotSet, tolerance=NotSet,
direction=Direction.NEAREST, timeout=NotSet, underflow=NotSet):
"""Block until the vehicle is within tolerance of the target heading.
Args:
by (Real): Change heading by this amount. May be negative. Cannot
be set at the same time as `to`.
to (Real): Rotate to this heading. Cannot be set at the same
time as `by`.
method (Callable[[Real, Real], Real]): Take two arguments, the
current heading and the target heading, and return an
axis control value within [-1, 1].
tolerance (Real): The allowable error in altitude before the method
sets the z-axis to 0 and returns.
direction (Direction): The direction to rotate.
timeout (Optional[timedelta]): If not None, the amount of time the
method has to complete an operation before raising
CommandTimeout. If None the timeout is datetime.max.
underflow (Real): If the vehicle over-rotates and is outside of
tolerance this is how far the vehicle is allowed to correct
the error before resetting to follow the set direction.
"""
# NotSet resolution
method = self.resolve(method, 'method')
tolerance = self.resolve(tolerance, 'tolerance')
direction = self.resolve(direction, 'direction')
timeout = self.resolve(timeout, 'timeout')
underflow = self.resolve(underflow, 'underflow')
# Sanity checks
if not by and not to:
raise ValueError('Must set one of by or to.')
elif by and to:
raise ValueError('Must not set both by and to.')
if not method or not callable(method):
raise ValueError('Must have a callable navigation method.')
if not isinstance(tolerance, Real) or tolerance < 0.0:
raise ValueError('Tolerance must be a non-negative real number.')
if direction not in Direction:
raise TypeError('Direction must be a valid Direction.')
if timeout is not None and not isinstance(timeout, timedelta):
raise ValueError('Timeout must be None or a timedelta.')
if not isinstance(underflow, Real) or underflow < 0.0:
raise ValueError('Underflow must be a non-negative real number.')
# Timeout
# TODO Use vehicle time not local
timeout = datetime.max if timeout is None else datetime.now() + timeout
original = self.nav.heading
target = original + by if by else Heading(to)
tol_range = target.range(tolerance)
while datetime.now() < timeout:
current = self.nav.heading
if current in tol_range:
self.info('rotate expected %s actual %s (in %s)',
target, current, tol_range)
self.broadcast(Intent(action=ACTION_AXIS_SET, data=('r', 0.0)))
return self
distance = Heading.distance(current, target, direction, underflow)
control = method(0.0, distance)
self.debug('rotate toward %.3f current %.3f (%.3f < %.3f) %.3f',
target, current, abs(distance), tolerance, control)
self.broadcast(Intent(action=ACTION_AXIS_SET, data=('r', control)))
time.sleep(self.nav.sleep_time)
raise CommandTimeout('rotate exceeded timeout')