def execute_mission(mission):
if mission_lock.acquire(0):
try:
if not vehicle_control.get().arm():
Logger.warn("Could not arm vehicle. Will not start mission",
TAG)
else:
mission.start()
except RuntimeError, ex:
Logger.critical("Cpp module threw exception: {0}. Aborting!", TAG)
vehicle_control.abort()
finally:
def __set_mode(self, mode):
operation = _Operation(
predicate=lambda: self.vehicle.mode == VehicleMode(mode),
name="Set vechicle mode to '{0}'".format(mode))
if mode not in VEHICLE_MODES:
Logger.warn(
"Operation '{0}' failed. Unknown mode '{1}'".format(
operation.name, mode), TAG)
return False
self.vehicle.mode = VehicleMode(mode)
return self.__run_in_time_window(operation)
def takeoff(self, target_altitude):
operation = _Operation(predicate=lambda: self.get_altitude() >=
target_altitude * MINIMUM_ALTITUDE_FACTOR,
name="Change altitude to: {0}".format(
str(target_altitude)),
timeout=20)
if not self.__check_armed_and_guided():
Logger.warn(
"Operation '{0}' failed. Vehicle is not armed and guided".
format(operation.name), TAG)
return False
self.vehicle.simple_takeoff(target_altitude)
return self.__run_in_time_window(operation)
def land(self):
operation = _Operation(predicate=lambda: self.vehicle.location.
global_relative_frame.alt <= LANDING_ALTITUDE,
name="Land",
timeout=60)
if not self.__check_armed_and_guided():
Logger.warn(
"Operation '{0}' failed. Vehicle is not armed and guided".
format(operation.name), TAG)
return False
self.__set_mode("LAND")
return self.__run_in_time_window(operation)
def arm(self):
if self.vehicle.armed:
return True
operation = _Operation(predicate=lambda: self.vehicle.is_armable,
name="Check if vehicle is armable")
if not self.__check_guided():
Logger.warn(
"Operation '{0}' failed. Vehicle is not in guided mode".format(
operation.name), TAG)
return False
if self.__run_in_time_window(operation):
self.vehicle.armed = True
operation = _Operation(predicate=lambda: self.vehicle.armed,
name="Arm vehicle")
return self.__run_in_time_window(operation)
return False
def abort(self):
Logger.info("Got abort signal. Trying to land vehicle", TAG)
for i in range(ABORT_ATTEMPTS):
if self.land():
Logger.info("Abort succeeded", TAG)
return
Logger.critical("Abort Failed! You should take control from the RC!",
TAG)
def __run_in_time_window(self, operation):
Logger.debug(
"Starting to execute drone operation '{0}' with timeout '{1}'".
format(operation.name, operation.timeout), TAG)
now = time()
while time() < now + operation.timeout:
if operation.predicate():
Logger.debug("Operation '{0}' succeed".format(operation.name),
TAG)
return True
sleep(operation.gap)
Logger.warn(
"Operation '{0}' failed. Timeout reached.".format(operation.name),
TAG)
return False
def to_run():
remaining_distance = self.__get_distance_metres(
self.vehicle.location.global_relative_frame, location)
Logger.debug(
"Distance to target: {0}".format(str(remaining_distance)), TAG)
return remaining_distance <= targetDistance * 0.01 or remaining_distance <= DISTANCE_FROM_TARGET_THRESHOLD
def set_to_guided(args=None):
if not vehicle_control.get().set_to_guided():
Logger.warn("Could not set vehicle mode to guided. Will not start mission", TAG)
def on_ping(args):
Logger.info("Got ping command from user", TAG)
def on_abort_command(args):
Logger.info("Got abort command from user", TAG)
vehicle_control.get().abort()
def connect_to_vehicle(vehical_connection_string):
Logger.info("Connecting to vehicle...", TAG)
vehicle = connect(vehical_connection_string, wait_ready=True)
vehicle.wait_ready('autopilot_version')
Logger.info("Autopilot Firmware version: %s" % vehicle.version, TAG)
Logger.info("Major version number: %s" % vehicle.version.major, TAG)
Logger.info("Minor version number: %s" % vehicle.version.minor, TAG)
Logger.info("Patch version number: %s" % vehicle.version.patch, TAG)
Logger.info("Release type: %s" % vehicle.version.release_type(), TAG)
Logger.info("Release version: %s" % vehicle.version.release_version(), TAG)
Logger.info("Stable release?: %s" % vehicle.version.is_stable(), TAG)
return vehicle
def on_abort_command(args):
Logger.info("Got abort command from user", TAG)
vehicle_control.get().abort()
def on_ping(args):
Logger.info("Got ping command from user", TAG)
def set_to_guided(args=None):
if not vehicle_control.get().set_to_guided():
Logger.warn("Could not set vehicle mode to guided. Will not start mission", TAG)
if __name__ == "__main__":
args = parse_arguments()
# Setup logger
Logger.set_udp_stream(args.gcs_ip, 1234)
# Connect to drone
vehicle = connect_to_vehicle(args.vehicle_connection_string)
vehicle_control.init(vehicle)
ServiceProvider.get_instance().publish("logger", Logger);
ServiceProvider.get_instance().publish("vehicle_control", vehicle_control.get());
# Setup Video provider
video_provider = VideoProvider.get_instance()
video_provider_thread = Thread(target=video_provider.start_listen_to_camera)
video_provider_thread.setDaemon(True)
video_provider_thread.start()
# Setup Video streamer
from common.python.logger import Logger
import vehicle_control
from threading import Lock
TAG = "MissionExecuter"
mission_lock = Lock()
# mission should have a 'start' method
def execute_mission(mission):
if mission_lock.acquire(0):
try:
if not vehicle_control.get().arm():
Logger.warn("Could not arm vehicle. Will not start mission",
TAG)
else:
mission.start()
except RuntimeError, ex:
Logger.critical("Cpp module threw exception: {0}. Aborting!", TAG)
vehicle_control.abort()
finally:
vehicle_control.get().disarm()
mission_lock.release()
else:
Logger.warn("Already executing a mission", TAG)