class HeadlessClient():
"""Crazyflie headless client"""
def __init__(self):
"""Initialize the headless client and libraries"""
cflib.crtp.init_drivers()
self._jr = JoystickReader(do_device_discovery=False)
self._cf = Crazyflie(ro_cache=sys.path[0] + "/cflib/cache",
rw_cache=sys.path[1] + "/cache")
signal.signal(signal.SIGINT, signal.SIG_DFL)
self._devs = []
for d in self._jr.available_devices():
self._devs.append(d.name)
def setup_controller(self, input_config, input_device=0, xmode=False):
"""Set up the device reader"""
# Set up the joystick reader
self._jr.device_error.add_callback(self._input_dev_error)
print("Client side X-mode: %s" % xmode)
if (xmode):
self._cf.commander.set_client_xmode(xmode)
devs = self._jr.available_devices()
print("Will use [%s] for input" % self._devs[input_device])
self._jr.start_input(self._devs[input_device])
self._jr.set_input_map(self._devs[input_device], input_config)
def controller_connected(self):
""" Return True if a controller is connected"""
return True if (len(self._jr.available_devices()) > 0) else False
def list_controllers(self):
"""List the available controllers and input mapping"""
print("\nAvailable controllers:")
for i, dev in enumerate(self._devs):
print(" - Controller #{}: {}".format(i, dev))
print("\nAvailable input mapping:")
for map in os.listdir(sys.path[1] + '/input'):
print(" - " + map.split(".json")[0])
def connect_crazyflie(self, link_uri):
"""Connect to a Crazyflie on the given link uri"""
self._cf.connection_failed.add_callback(self._connection_failed)
# 2014-11-25 chad: Add a callback for when we have a good connection.
self._cf.connected.add_callback(self._connected)
self._cf.param.add_update_callback(
group="imu_sensors",
name="HMC5883L",
cb=(lambda name, found: self._jr.set_alt_hold_available(eval(found)
)))
self._jr.althold_updated.add_callback(
lambda enabled: self._cf.param.set_value("flightmode.althold",
enabled))
self._cf.open_link(link_uri)
self._jr.input_updated.add_callback(self._cf.commander.send_setpoint)
def _connected(self, link):
"""Callback for a successful Crazyflie connection."""
print("Connected to {}".format(link))
def _connection_failed(self, link, message):
"""Callback for a failed Crazyflie connection"""
print("Connection failed on {}: {}".format(link, message))
sys.exit(-1)
def _input_dev_error(self, message):
"""Callback for an input device error"""
print("Error when reading device: {}".format(message))
sys.exit(-1)
class HeadlessClient():
"""Crazyflie headless client"""
def __init__(self):
"""Initialize the headless client and libraries"""
cflib.crtp.init_drivers()
self._jr = JoystickReader(do_device_discovery=False)
self._cf = Crazyflie(ro_cache=sys.path[0] + "/cflib/cache",
rw_cache=sys.path[1] + "/cache")
signal.signal(signal.SIGINT, signal.SIG_DFL)
self._devs = []
for d in self._jr.available_devices():
self._devs.append(d.name)
def setup_controller(self, input_config, input_device=0, xmode=False):
"""Set up the device reader"""
# Set up the joystick reader
self._jr.device_error.add_callback(self._input_dev_error)
print("Client side X-mode: %s" % xmode)
if (xmode):
self._cf.commander.set_client_xmode(xmode)
devs = self._jr.available_devices()
print("Will use [%s] for input" % self._devs[input_device])
self._jr.start_input(self._devs[input_device])
self._jr.set_input_map(self._devs[input_device], input_config)
def controller_connected(self):
""" Return True if a controller is connected"""
return True if (len(self._jr.available_devices()) > 0) else False
def list_controllers(self):
"""List the available controllers and input mapping"""
print("\nAvailable controllers:")
for i, dev in enumerate(self._devs):
print(" - Controller #{}: {}".format(i, dev))
print("\nAvailable input mapping:")
for map in os.listdir(sys.path[1] + '/input'):
print(" - " + map.split(".json")[0])
def connect_crazyflie(self, link_uri):
"""Connect to a Crazyflie on the given link uri"""
self._cf.connection_failed.add_callback(self._connection_failed)
# 2014-11-25 chad: Add a callback for when we have a good connection.
self._cf.connected.add_callback(self._connected)
self._cf.param.add_update_callback(
group="imu_sensors", name="HMC5883L", cb=(
lambda name, found: self._jr.set_alt_hold_available(
eval(found))))
self._jr.althold_updated.add_callback(
lambda enabled: self._cf.param.set_value("flightmode.althold",
enabled))
self._cf.open_link(link_uri)
self._jr.input_updated.add_callback(self._cf.commander.send_setpoint)
def _connected(self, link):
"""Callback for a successful Crazyflie connection."""
print("Connected to {}".format(link))
def _connection_failed(self, link, message):
"""Callback for a failed Crazyflie connection"""
print("Connection failed on {}: {}".format(link, message))
sys.exit(-1)
def _input_dev_error(self, message):
"""Callback for an input device error"""
print("Error when reading device: {}".format(message))
sys.exit(-1)
class LogFlight():
def __init__(self, args):
self.args = args
self.optitrack_enabled = False
self.console_dump_enabled = False
cflib.crtp.init_drivers(enable_debug_driver=False)
self._cf = Crazyflie(rw_cache="./cache")
self._jr = JoystickReader(do_device_discovery=False)
# Set flight mode
if self.args["trajectory"] is None or \
self.args["trajectory"][0] == "none":
self.mode = Mode.DONT_FLY
print("Mode set to [DONT_FLY]")
elif self.args["trajectory"][0] == "manual":
self.mode = Mode.MANUAL
print("Mode set to [MANUAL]")
elif self.args["safetypilot"]:
self.mode = Mode.MODE_SWITCH
print("Mode set to [MODE_SWITCH]")
else:
self.mode = Mode.AUTO
print("Mode set to [AUTO]")
# Setup for specified mode
if self.mode == Mode.AUTO:
self.is_in_manual_control = False
# Make sure drone is setup to perform autonomous flight
if args["uwb"] == "none":
assert args["optitrack"] == "state", "OptiTrack state needed in absence of UWB"
assert args["estimator"] == "kalman", "OptiTrack state needs Kalman estimator"
elif self.mode == Mode.DONT_FLY:
self.is_in_manual_control = False
else:
# Check if controller is connected
assert self.controller_connected(), "No controller detected."
self.setup_controller(map="flappy")
self.is_in_manual_control = True
# Setup the logging framework
self.setup_logger()
# Setup optitrack if required
if not args["optitrack"] == "none":
self.setup_optitrack()
def get_filename(self):
# create default fileroot if not provided
if self.args["fileroot"] is None:
date = datetime.today().strftime(r"%Y_%m_%d")
self.args["fileroot"] = "data/" + date
fileroot = self.args["fileroot"]
# create default filename if not provided
if self.args["filename"] is None:
estimator = self.args["estimator"]
uwb = self.args["uwb"]
optitrack = self.args["optitrack"]
trajectory = self.args["trajectory"]
date = datetime.today().strftime(r"%Y-%m-%d+%H:%M:%S")
traj = '_'.join(trajectory)
if optitrack == "logging":
options = "{}+{}+optitracklog+{}".format(estimator, uwb, traj)
elif optitrack == "state":
options = "{}+{}+optitrackstate+{}".format(estimator, uwb, traj)
else:
options = "{}+{}+{}".format(estimator, uwb, traj)
name = "{}+{}.csv".format(date, options)
fname = os.path.normpath(os.path.join(os.getcwd(), fileroot, name))
else:
# make sure provided filename is unique
if self.args["filename"].endswith(".csv"):
name = self.args["filename"][:-4]
else:
name = self.args["filename"]
new_name = name + ".csv"
fname = os.path.normpath(os.path.join(
os.getcwd(), fileroot, new_name))
i = 0
while os.path.isfile(fname):
i = i + 1
new_name = name + "_" + str(i) + ".csv"
fname = os.path.normpath(os.path.join(
os.getcwd(), fileroot, new_name))
return fname
def setup_logger(self):
# Create filename from options and date
self.log_file = self.get_filename()
# Create directory if not there
Path(self.args["fileroot"]).mkdir(exist_ok=True)
print("Log location: {}".format(self.log_file))
# Logger setup
logconfig = self.args["logconfig"]
self.flogger = FileLogger(self._cf, logconfig, self.log_file)
self.flogger.enableAllConfigs()
def setup_optitrack(self):
self.ot_id = self.args["optitrack_id"]
self.ot_position = np.zeros(3)
self.ot_attitude = np.zeros(3)
self.ot_quaternion = np.zeros(4)
self.filtered_pos = np.zeros(3)
self.ot_filter_sos = scipy.signal.butter(N=4, Wn=0.1, btype='low',
analog=False, output='sos')
self.pos_filter_zi = [scipy.signal.sosfilt_zi(self.ot_filter_sos),
scipy.signal.sosfilt_zi(self.ot_filter_sos),
scipy.signal.sosfilt_zi(self.ot_filter_sos)]
# Streaming client in separate thread
streaming_client = NatNetClient()
streaming_client.newFrameListener = self.ot_receive_new_frame
streaming_client.rigidBodyListener = self.ot_receive_rigidbody_frame
streaming_client.run()
self.optitrack_enabled = True
print("OptiTrack streaming client started")
# TODO: do we need to return StreamingClient?
def reset_estimator(self):
# Kalman
if self.args["estimator"] == "kalman":
self._cf.param.set_value("kalman.resetEstimation", "1")
time.sleep(1)
self._cf.param.set_value("kalman.resetEstimation", "0")
# Complementary (needs changes to firmware)
if self.args["estimator"] == "complementary":
try:
self._cf.param.set_value("complementaryFilter.reset", "1")
time.sleep(1)
self._cf.param.set_value("complementaryFilter.reset", "0")
except:
pass
def ot_receive_new_frame(self, *args, **kwargs):
pass
def ot_receive_rigidbody_frame(self, id, position, rotation):
# Check ID
if id in self.ot_id:
# get optitrack data in crazyflie global frame
pos_in_cf_frame = util.ot2control(position)
att_in_cf_frame = util.quat2euler(rotation)
quat_in_cf_frame = util.ot2control_quat(rotation)
idx = self.ot_id.index(id)
if idx==0:
# main drone
ot_dict = {
"otX0": pos_in_cf_frame[0],
"otY0": pos_in_cf_frame[1],
"otZ0": pos_in_cf_frame[2],
"otRoll0": att_in_cf_frame[0],
"otPitch0": att_in_cf_frame[1],
"otYaw0": att_in_cf_frame[2]
}
self.ot_position = pos_in_cf_frame
self.ot_attitude = att_in_cf_frame
self.ot_quaternion = quat_in_cf_frame
self.flogger.registerData("ot0", ot_dict)
(self.filtered_pos[0], self.pos_filter_zi[0]) = scipy.signal.sosfilt(
self.ot_filter_sos, [self.ot_position[0]], zi=self.pos_filter_zi[0]
)
(self.filtered_pos[1], self.pos_filter_zi[1]) = scipy.signal.sosfilt(
self.ot_filter_sos, [self.ot_position[1]], zi=self.pos_filter_zi[1]
)
(self.filtered_pos[2], self.pos_filter_zi[2]) = scipy.signal.sosfilt(
self.ot_filter_sos, [self.ot_position[2]], zi=self.pos_filter_zi[2]
)
elif idx==1:
ot_dict = {
"otX1": pos_in_cf_frame[0],
"otY1": pos_in_cf_frame[1],
"otZ1": pos_in_cf_frame[2],
"otRoll1": att_in_cf_frame[0],
"otPitch1": att_in_cf_frame[1],
"otYaw1": att_in_cf_frame[2]
}
self.flogger.registerData("ot1", ot_dict)
def do_taskdump(self):
self._cf.param.set_value("system.taskDump", "1")
def process_taskdump(self, console_log):
file = self.get_filename()
# Dataframe placeholders
label_data, load_data, stack_data = [], [], []
# Get headers
headers = []
for i, line in enumerate(console_log):
if "Task dump" in line:
headers.append(i)
# None indicates the end of the list
headers.append(None)
# Get one task dump
for i in range(len(headers) - 1):
dump = console_log[headers[i] + 2 : headers[i + 1]]
# Process strings: strip \n, \t, spaces, SYSLOAD:
loads, stacks, labels = [], [], []
for line in dump:
entries = line.strip("SYSLOAD: ").split("\t")
loads.append(entries[0].strip()) # no sep means strip all space, \n, \t
stacks.append(entries[1].strip())
labels.append(entries[2].strip())
# Store labels
if not label_data:
label_data = labels
# Append to placeholders
load_data.append(loads)
stack_data.append(stacks)
# Check if we have data at all
if headers[0] is not None and label_data:
# Put in dataframe
load_data = pd.DataFrame(load_data, columns=label_data)
stack_data = pd.DataFrame(stack_data, columns=label_data)
# Save dataframes
load_data.to_csv(file.strip(".csv") + "+load.csv", sep=",", index=False)
stack_data.to_csv(file.strip(".csv") + "+stackleft.csv", sep=",", index=False)
else:
print("No task dump data found")
def controller_connected(self):
""" Return True if a controller is connected """
return len(self._jr.available_devices()) > 0
def setup_controller(self, map="PS3_Mode_1"):
devs = []
for d in self._jr.available_devices():
devs.append(d.name)
if len(devs)==1:
input_device = 0
else:
print("Multiple controllers detected:")
for i, dev in enumerate(devs):
print(" - Controller #{}: {}".format(i, dev))
input_device = int(input("Select controller: "))
if not input_device in range(len(devs)):
raise ValueError
self._jr.start_input(devs[input_device])
self._jr.set_input_map(devs[input_device], map)
def connect_crazyflie(self, uri):
"""Connect to a Crazyflie on the given link uri"""
# Connect some callbacks from the Crazyflie API
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
if self.mode == Mode.AUTO or self.mode == Mode.DONT_FLY:
self._cf.open_link(uri)
else:
# Add callbacks for manual control
self._cf.param.add_update_callback(
group="imu_sensors", name="AK8963", cb=(
lambda name, found: self._jr.set_alt_hold_available(
eval(found))))
# self._jr.assisted_control_updated.add_callback(
# lambda enabled: self._cf.param.set_value("flightmode.althold",
# enabled))
self._cf.open_link(uri)
self._jr.input_updated.add_callback(self.controller_input_cb)
if self.mode == Mode.MODE_SWITCH:
self._jr.alt1_updated.add_callback(self.mode_switch_cb)
def _connected(self, link):
"""This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
print("Connected to %s" % link)
# set estimator
if args["estimator"]=="kalman":
self._cf.param.set_value("stabilizer.estimator", "2")
self.flogger.start()
print("logging started")
def _connection_failed(self, link_uri, msg):
print("Connection to %s failed: %s" % (link_uri, msg))
self.flogger.is_connected = False
def _connection_lost(self, link_uri, msg):
print("Connection to %s lost: %s" % (link_uri, msg))
self.flogger.is_connected = False
def _disconnected(self, link_uri):
print("Disconnected from %s" % link_uri)
self.flogger.is_connected = False
def ready_to_fly(self):
# Wait for connection
timeout = 10
while not self._cf.is_connected():
print("Waiting for Crazyflie connection...")
time.sleep(2)
timeout -= 1
if timeout<=0:
return False
# Wait for optitrack
if self.optitrack_enabled:
while (self.ot_position == 0).any():
print("Waiting for OptiTrack fix...")
time.sleep(2)
timeout -= 1
if timeout <= 0:
return False
print("OptiTrack fix acquired")
print("Reset Estimator...")
self.reset_estimator()
time.sleep(2) # wait for kalman to stabilize
return True
def start_flight(self):
if self.ready_to_fly():
if self.mode == Mode.MANUAL:
print("Manual Flight - Ready to fly")
self.manual_flight()
elif self.mode == Mode.DONT_FLY:
print("Ready to not fly")
try:
while True:
pass
except KeyboardInterrupt:
print("Flight stopped")
else:
# Build trajectory
setpoints = self.build_trajectory(self.args["trajectory"], self.args["space"])
# Do flight
if self.mode == Mode.AUTO:
print("Autonomous Flight - Starting flight")
self.follow_setpoints(self._cf, setpoints, self.args["optitrack"])
print("Flight complete.")
else:
print("Ready to fly")
self.manual_flight()
print("Starting Trajectory")
self.follow_setpoints(self._cf, setpoints, self.args["optitrack"])
else:
print("Timeout while waiting for flight ready.")
def controller_input_cb(self, *data):
# only forward control in manual mode
if self.is_in_manual_control:
self._cf.commander.send_setpoint(*data)
def mode_switch_cb(self, auto_mode):
if auto_mode:
print("Switching autonomous flight")
self.is_in_manual_control = False
else:
print("Switching to manual control")
self.is_in_manual_control = True
def manual_flight(self):
self.is_in_manual_control = True
while(self.is_in_manual_control):
if self.args["optitrack"]=="state":
# self._cf.extpos.send_extpos(
# self.filtered_pos[0], self.filtered_pos[1], self.filtered_pos[2]
# )
self._cf.extpos.send_extpos(
self.ot_position[0], self.ot_position[1], self.ot_position[2]
)
# self._cf.extpos.send_extpose(
# self.ot_position[0], self.ot_position[1], self.ot_position[2],
# self.ot_quaternion[0], self.ot_quaternion[1], self.ot_quaternion[2], self.ot_quaternion[3]
# )
time.sleep(0.01)
def build_trajectory(self, trajectories, space):
# Load yaml file with space specification
with open(space, "r") as f:
space = yaml.full_load(f)
home = space["home"]
ranges = space["range"]
# Account for height offset
altitude = home["z"] + ranges["z"]
side_length = min([ranges["x"], ranges["y"]]) * 2
radius = min([ranges["x"], ranges["y"]])
x_bound = [home["x"] - ranges["x"], home["x"] + ranges["x"]]
y_bound = [home["y"] - ranges["y"], home["y"] + ranges["y"]]
# Build trajectory
# Takeoff
setpoints = takeoff(home["x"], home["y"], altitude, 0.0)
for trajectory in trajectories:
# If nothing, only nothing
if trajectory == "nothing":
setpoints = None
return setpoints
elif trajectory == "hover":
setpoints += hover(home["x"], home["y"], altitude)
elif trajectory == "hover_fw":
setpoints += hover_fw(home["x"], home["y"], altitude)
elif trajectory == "square":
setpoints += square(home["x"], home["y"], side_length, altitude)
elif trajectory == "square_fw":
setpoints += square_fw(home["x"], home["y"], side_length, altitude)
elif trajectory == "octagon":
setpoints += octagon(home["x"], home["y"], radius, altitude)
elif trajectory == "triangle":
setpoints += triangle(home["x"], home["y"], radius, altitude)
elif trajectory == "hourglass":
setpoints += hourglass(home["x"], home["y"], side_length, altitude)
elif trajectory == "random":
setpoints += randoms(home["x"], home["y"], x_bound, y_bound, altitude)
elif trajectory == "scan":
setpoints += scan(home["x"], home["y"], x_bound, y_bound, altitude)
else:
raise ValueError("{} is an unknown trajectory".format(trajectory))
# Add landing
setpoints += landing(home["x"], home["y"], altitude, 0.0)
return setpoints
def follow_setpoints(self, cf, setpoints, optitrack):
# Counter for task dump logging
time_since_dump = 0.0
# Start
try:
print("Flight started")
# Do nothing, just sit on the ground
if setpoints is None:
while True:
time.sleep(0.05)
time_since_dump += 0.05
# Task dump
if time_since_dump > 2:
print("Do task dump")
self.do_taskdump()
time_since_dump = 0.0
# Do actual flight
else:
for i, point in enumerate(setpoints):
print("Next setpoint: {}".format(point))
# Compute time based on distance
# Take-off
if i == 0:
distance = point[2]
# No take-off
else:
distance = np.sqrt(
((np.array(point[:3]) - np.array(setpoints[i - 1][:3])) ** 2).sum()
)
# If zero distance, at least some wait time
if distance == 0.0:
wait = 5
else:
wait = distance * 2
# Send position and wait
time_passed = 0.0
while time_passed < wait:
if self.is_in_manual_control:
self.manual_flight()
# If we use OptiTrack for control, send position to Crazyflie
if optitrack == "state":
cf.extpos.send_extpos(
self.filtered_pos[0], self.filtered_pos[1], self.filtered_pos[2]
)
cf.commander.send_position_setpoint(*point)
time.sleep(0.05)
time_passed += 0.05
time_since_dump += 0.05
# Task dump
# if time_since_dump > 2:
# print("Do task dump")
# self.do_taskdump()
# time_since_dump = 0.0
# Finished
cf.commander.send_stop_setpoint()
# Prematurely break off flight / quit doing nothing
except KeyboardInterrupt:
if setpoints is None:
print("Quit doing nothing!")
else:
print("Emergency landing!")
wait = setpoints[i][2] * 2
cf.commander.send_position_setpoint(setpoints[i][0], setpoints[i][1], 0.0, 0.0)
time.sleep(wait)
cf.commander.send_stop_setpoint()
def setup_console_dump(self):
# Console dump file
self.console_log = []
console = Console(self._cf)
console.receivedChar.add_callback(self._console_cb)
self.console_dump_enabled = True
def _console_cb(self, text):
# print(text)
self.console_log.append(text)
def end(self):
self._cf.close_link()
# Process task dumps
# TODO: add timestamps / ticks (like logging) to this
if self.console_dump_enabled:
self.process_taskdump(self.console_log)
