def read_mission(file_name):
write_info_blue("Reading mission from file: %s" % file_name)
mission_list = []
with open(file_name) as file:
for i, line in enumerate(file):
if i == 0:
if not line.startswith('QGC WPL 110'):
write_error("Given file is not supported version")
return None
else:
line_array = line.split('\t')
ln_index = int(line_array[0])
ln_currentwp = int(line_array[1])
ln_frame = int(line_array[2])
ln_command = int(line_array[3])
ln_param1 = float(line_array[4])
ln_param2 = float(line_array[5])
ln_param3 = float(line_array[6])
ln_param4 = float(line_array[7])
ln_param5 = float(line_array[8])
ln_param6 = float(line_array[9])
ln_param7 = float(line_array[10])
ln_autocontinue = int(line_array[11].strip())
cmd = dronekit.Command( 0, 0, 0, ln_frame, ln_command, ln_currentwp, ln_autocontinue, ln_param1, ln_param2, ln_param3, ln_param4, ln_param5, ln_param6, ln_param7)
mission_list.append(cmd)
return mission_list
def make_mavlink_command(cmd,
trg_sys=0,
trg_component=0,
seq=0,
frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
param1=0,
param2=0,
param3=0,
param4=0,
lat_or_param5=0,
lon_or_param6=0,
alt_or_param7=0):
"""
Make a new mavlink command.
:param cmd:
:param trg_sys:
:param trg_component:
:param seq:
:param frame:
:param param1:
:param param2:
:param param3:
:param param4:
:param lat_or_param5:
:param lon_or_param6:
:param alt_or_param7:
:return:
"""
cmd_args = [
trg_sys, trg_component, seq, frame, cmd, 0, 0, param1, param2, param3,
param4, lat_or_param5, lon_or_param6, alt_or_param7
]
return dronekit.Command(*cmd_args)
def command_rtl(alt):
"""
Wrapper to hide mavlink parameters.
Returns command to return to launch.
"""
return dronekit.Command(0, 0, 0,
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0,
0, 0, 0, 0, 0, 0, alt)
def command_land():
"""
Wrapper to hide mavlink parameters.
Returns command to land.
"""
return dronekit.Command(0, 0, 0,
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0, 0,
0, 0, 0)
def command_waypoint(lat, lon, alt):
"""
Wrapper to hide mavlink parameters.
Returns command to go to specified location.
"""
return dronekit.Command(0, 0, 0,
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0,
0, 0, lat, lon, alt)
def command_takeoff(alt):
"""
Wrapper to hide mavlink parameters.
Returns command to takeoff to specified altitude.
"""
return dronekit.Command(0, 0, 0,
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0,
0, 0, 0, alt)
def command_unlock():
"""
Wrapper to hide mavlink parameters.
Returns command to unlock the solenoid.
Depends on solenoid configuration.
"""
return dronekit.Command(0, 0, 0,
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_DO_SET_RELAY, 0, 0, 0, 1,
0, 0, 0, 0, 0)
def line_to_command(s: str) -> dronekit.Command:
"""Parses a WPL line to a Dronekit command."""
logger.debug("parsing command string: %s", s)
args = s.split()
arg_index = int(args[0])
arg_currentwp = 0 # int(args[1])
arg_frame = int(args[2])
arg_cmd = int(args[3])
arg_autocontinue = 0 # not supported by dronekit
(p1, p2, p3, p4, x, y, z) = [float(x) for x in args[4:11]]
cmd = dronekit.Command(0, 0, 0, arg_frame, arg_cmd, arg_currentwp,
arg_autocontinue, p1, p2, p3, p4, x, y, z)
return cmd
def to_dronekit_command(
self,
frame: int = mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT
) -> dronekit.Command:
cmd = dronekit.Command(
self.target_system,
self.target_component,
0, # Sequence number automatically saved
frame,
self.cmd_id,
0, # current (not supported)
0, # autocontinue (not supported)
self.param_1,
self.param_2,
self.param_3,
self.param_4,
self.param_5,
self.param_6,
self.param_7)
return cmd
def command_dummy():
return dronekit.Command(0, 0, 0,
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0, 0,
0, 0, 0)
def _mission_sender(self, cmd_list):
cmds = self._vehicle.commands
cmds.clear()
last_command_location = None
for element in cmd_list:
self._logger.info("Parsing mission element {0}".format(element))
if 'cmd' not in element:
self._logger.error(
"Rejecting Mission - Malformed mission element (cmd not found): {0}"
.format(element))
cmds.clear()
break
elif element['cmd'] == 'WAYPOINT':
self._logger.debug("Parsing WAYPOINT mission element")
lat = None
lon = None
alt = None
if 'latitude' in element:
lat = element['latitude']
if 'longitude' in element:
lon = element['longitude']
if 'altitude' in element:
alt = element['altitude']
elif last_command_location is not None:
alt = last_command_location[2]
if lat is None or lon is None or alt is None:
self._logger.error(
"Rejecting Mission - Malformed mission element WAYPOINT (missing lat, long, or alt): {0}"
.format(element))
cmds.clear()
break
last_command_location = (lat, lon, alt)
cmd = dronekit.Command(
0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
MAV_CMD['WAYPOINT'], 0, 1, DEFAULT_HOLD_TIME,
DEFAULT_RADIUS, 0, 0, lat, lon, alt)
cmds.add(cmd)
elif element['cmd'] == 'TAKEOFF':
self._logger.debug("Parsing TAKEOFF mission element")
target_alt = DEFAULT_TAKEOFF_ALT
if target_alt in element:
target_alt = element['target_altitude']
current_pos = self._vehicle.location.global_relative_frame
last_command_location = (current_pos.lat, current_pos.lon,
current_pos.alt + target_alt)
cmd = dronekit.Command(
0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
MAV_CMD['TAKEOFF'], 0, 1, 0, 0, 0, 0, current_pos.lat,
current_pos.lon, current_pos.alt + target_alt)
cmds.add(cmd)
elif element['cmd'] == 'LAND':
self._logger.debug("Parsing LAND mission element")
lat = None
lon = None
if 'latitude' in element:
lat = element['latitude']
elif last_command_location is not None:
lat = last_command_location[0]
if 'longitude' in element:
lon = element['longitude']
elif last_command_location is not None:
lon = last_command_location[1]
if lat is None or lon is None:
current_pos = self._vehicle.location.global_relative_frame
lat = current_pos.lat
lon = current_pos.lon
cmd = dronekit.Command(
0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
MAV_CMD['LAND'], 0, 1, 0, 0, 0, 0, lat, lon, 0.0)
cmds.add(cmd)
self._logger.info(
"Mission parsed, sending mission to flight controller")
upload_successful = cmds.upload()
return upload_successful
def command_rtl(alt):
return dronekit.Command(0, 0, 0,
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0,
0, 0, 0, 0, 0, 0, alt)
def command_waypoint(lat, lon, alt):
return dronekit.Command(0, 0, 0,
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0,
0, 0, lat, lon, alt)
def command_takeoff(alt):
return dronekit.Command(0, 0, 0,
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0,
0, 0, 0, alt)
def create_servo_set_command(channel, pwm): return dronekit.Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_DO_SET_SERVO, 0, 0, channel, pwm, 0, 0, 0, 0, 0)
def run_and_trace(self,
commands: Sequence[Command],
collect_coverage: bool = False) -> 'MissionTrace':
"""
Executes a mission, represented as a sequence of commands, and
returns a description of the outcome.
Parameters:
commands: the list of commands to be sent to the robot as
a mission.
collect_coverage: indicates whether or not coverage information
should be incorporated into the trace. If True (i.e., coverage
collection is enabled), this function expects the sandbox to be
properly instrumented.
Returns:
a trace describing the execution of a sequence of commands.
"""
config = self.configuration
env = self.environment
speedup = config.speedup
timeout_command = 300 / speedup + 5
timeout_arm = 10 / speedup + 5
timeout_mission_upload = 20
# the number of seconds for the delay added after DO commands
do_delay = max(4, int(20 / speedup))
with self.__lock:
outcomes = [] # type: List[CommandOutcome]
passed = True
connection_lost = threading.Event()
# FIXME The initial command should be based on initial state
initial = dronekit.Command(0, 0, 0, 0, 16, 0, 0, 0.0, 0.0, 0.0,
0.0, -35.3632607, 149.1652351, 584)
# delay to allow the robot to reach its stable state
delay = dronekit.Command(0, 0, 0, 3, 93, 0, 0, do_delay, -1, -1,
-1, 0, 0, 0)
# converting from Houston commands to dronekit commands
dronekitcmd_to_cmd_mapping = {}
cmds = [initial]
for i, cmd in enumerate(commands):
dronekitcmd_to_cmd_mapping[len(cmds)] = i
cmds.append(cmd.to_message().to_dronekit_command())
# DO commands trigger some action and return.
# we add a delay after them to see how they affect the state.
if 'MAV_CMD_DO_' in cmd.__class__.uid:
dronekitcmd_to_cmd_mapping[len(cmds)] = i
cmds.append(delay)
logger.debug("Final mission commands len: %d, mapping: %s",
len(cmds), dronekitcmd_to_cmd_mapping)
# uploading the mission to the vehicle
vcmds = self.vehicle.commands
vcmds.clear()
for cmd in cmds:
vcmds.add(cmd)
vcmds.upload(timeout=timeout_mission_upload)
logger.debug("Mission uploaded")
vcmds.wait_ready()
# maps each wp to the final state and time when wp was reached
wp_to_state = {} # Dict[int, Tuple[State, float]]
# [wp that has last been reached, wp running at the moment]
last_wp = [0, 0]
# used to synchronize rw to last_wp
wp_lock = threading.Lock()
# is set whenever a command in mission is done
wp_event = threading.Event()
# NOTE dronekit connection must not use its own heartbeat checking
def heartbeat_listener(_, __, value):
if value > TIME_LOST_CONNECTION:
connection_lost.set()
wp_event.set()
self.vehicle.add_attribute_listener('last_heartbeat',
heartbeat_listener)
def check_for_reached(m):
name = m.name
message = m.message
if name == 'MISSION_ITEM_REACHED':
logger.debug("**MISSION_ITEM_REACHED: %d", message.seq)
if message.seq == len(cmds) - 1:
logger.info("Last item reached")
with wp_lock:
last_wp[1] = int(message.seq) + 1
wp_event.set()
elif name == 'MISSION_CURRENT':
logger.debug("**MISSION_CURRENT: %d", message.seq)
logger.debug("STATE: {}".format(self.state))
if message.seq > last_wp[1]:
with wp_lock:
if message.seq > last_wp[0]:
last_wp[1] = message.seq
logger.debug("SET EVENT")
wp_event.set()
elif name == 'MISSION_ACK':
logger.debug("**MISSION_ACK: %s", message.type)
self.connection.add_hooks({'check_for_reached': check_for_reached})
stopwatch = Stopwatch()
stopwatch.start()
self.vehicle.armed = True
while not self.vehicle.armed:
if stopwatch.duration >= timeout_arm:
raise VehicleNotReadyError
logger.debug("waiting for the rover to be armed...")
self.vehicle.armed = True
time.sleep(0.1)
# starting the mission
self.vehicle.mode = dronekit.VehicleMode("AUTO")
initial_state = self.state
start_message = CommandLong(0, 0, 300, 0, 1,
len(cmds) + 1, 0, 0, 0, 0, 4)
self.connection.send(start_message)
logger.debug("sent mission start message to vehicle")
time_start = timer()
wp_to_traces = {}
with self.record() as recorder:
while last_wp[0] <= len(cmds) - 1:
logger.debug("waiting for command")
not_reached_timeout = wp_event.wait(timeout_command)
logger.debug("Event set %s", last_wp)
if not not_reached_timeout:
logger.error("Timeout occured %d", last_wp[0])
break
if connection_lost.is_set():
logger.error("Connection to vehicle was lost.")
raise ConnectionLostError
with wp_lock:
# self.observe()
logger.info("last_wp: %s len: %d", str(last_wp),
len(cmds))
logger.debug("STATE: {}".format(self.state))
current_time = timer()
time_passed = current_time - time_start
time_start = current_time
states, messages = recorder.flush()
if last_wp[0] > 0:
cmd_index = dronekitcmd_to_cmd_mapping[last_wp[0]]
wp_to_state[cmd_index] = (self.state, time_passed)
cmd = commands[cmd_index]
trace = CommandTrace(cmd, states)
wp_to_traces[cmd_index] = trace
# if appropriate, store coverage files
if collect_coverage:
cm_directory = "command{}".format(cmd_index)
self.__copy_coverage_files(cm_directory)
last_wp[0] = last_wp[1]
wp_event.clear()
self.connection.remove_hook('check_for_reached')
logger.debug("Removed hook")
if collect_coverage:
for cmd_index, command in enumerate(commands):
if cmd_index in wp_to_traces:
directory = 'command{}'.format(cmd_index)
coverage = self.__get_coverage(directory=directory)
wp_to_traces[cmd_index].add_coverage(coverage)
traces = [wp_to_traces[k] for k in sorted(wp_to_traces.keys())]
return MissionTrace(tuple(traces))
def command_unlock():
return dronekit.Command(0, 0, 0,
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_DO_SET_RELAY, 0, 0, 0, 1,
0, 0, 0, 0, 0)
