class Node:
def __init__(self):
self.vx = 0
self.vy = 0
self.vz = 0
self.va = 0
self.vel = Twist()
self.vel.linear.x = self.vx
self.vel.linear.y = self.vy
self.vel.linear.z = self.vz
self.vel.angular.z = self.va
self.vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
cflib.crtp.init_drivers(enable_debug_driver=False)
self.crazyflie = SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache'))
self.commander = MotionCommander(self.crazyflie)
self.cf = Crazyflie()
self.crazyflie.open_link()
self.commander.take_off()
def shut_down(self):
try:
self.pitch_log.stop()
finally:
self.crazyflie.close_link()
def run_node(self):
self.vel.linear.x = 0
self.vel.linear.y = 0
self.vel.linear.z = 0.2
self.vel.angular.z = 0
self.vel_pub.publish(self.vel)
self.commander._set_vel_setpoint(0, 0, 0.2, 0)
time.sleep(3)
self.vel.linear.x = 0
self.vel.linear.y = 0
self.vel.linear.z = 0
self.vel.angular.z = 72
self.vel_pub.publish(self.vel)
self.commander._set_vel_setpoint(0, 0, 0, 72)
time.sleep(3)
self.vel.linear.x = 0
self.vel.linear.y = 0
self.vel.linear.z = -0.2
self.vel.angular.z = 0
self.vel_pub.publish(self.vel)
self.commander._set_vel_setpoint(0, 0, -0.2, 0)
time.sleep(3)
class CrazyflieThread(threading.Thread):
def __init__(self, ctrl_message, state_message, message_lock, barrier):
threading.Thread.__init__(self)
self.ctrl = ctrl_message
self.state = state_message
self.lock = message_lock
self.cf = None
self.mc = None
self.scf = None
self.runSM = True
self.cmd_height_old = uglyConst.TAKEOFF_HEIGHT
self.b = barrier
self.descCounter = 0
def raise_exception(self):
self.runSM = False
#-- FSM condition funcitons
def isCloseXYP(self, r):
"""Determines if drone is within radius r and aligned."""
x = self.ctrl.errorx
y = self.ctrl.errory
if (np.sqrt(x*x+y*y) > r) or (np.abs(self.ctrl.erroryaw) > uglyConst.FAR_ANGL):
return False
else:
return True
def isCloseCone(self):
"""Determines if drone within an inverted cone (defined in constants)."""
x = self.ctrl.errorx
y = self.ctrl.errory
r = (self.mc._thread.get_height()-uglyConst.DIST_IGE)*uglyConst.FAR_CONE
if (np.sqrt(x*x+y*y) > r): # or (np.abs(self.ctrl.erroryaw) > uglyConst.FAR_ANGL):
return False
else:
return True
def isClosePix(self):
"""Determines if drone is within radius (in pixels, defined in constants)."""
x = self.ctrl.errorpixx
y = self.ctrl.errorpixy
if (np.sqrt(x*x+y*y) > uglyConst.CLOSE_PIX):
return False
else:
return True
def limOutputVel(self, vx, vy, vz):
"""Limits output of velocity controller."""
return min(vx, uglyConst.MAX_XVEL), min(vy, uglyConst.MAX_YVEL), min(vz, uglyConst.MAX_ZVEL)
#-- FSM state functions
def stateInit(self):
"""Initialize CF (scan, open link) and logging framework"""
#--- Scan for cf
cflib.crtp.init_drivers(enable_debug_driver=False)
print('Scanning interfaces for Crazyflies...')
available = cflib.crtp.scan_interfaces()
if len(available) == 0:
print("No cf found, aborting cf code.")
self.cf = None
else:
print('Crazyflies found:')
for i in available:
print(str(i[0]))
self.URI = 'radio://0/80/2M' #available[0][0]
self.cf = Crazyflie(rw_cache='./cache')
if self.cf is None:
self.b.wait()
print('Not running cf code.')
return None
self.scf = SyncCrazyflie(self.URI,cf=Crazyflie(rw_cache='./cache'))
self.scf.open_link()
self.mc = MotionCommander(self.scf)
self.file = open("ugly_log.txt","a")
#-- Barrier to wait for CV thread
self.b.wait()
self.lgr = UglyLogger(self.URI, self.scf, self.file)
self.enterTakingOff()
return self.stateTakingOff
def enterTakingOff(self):
"""Entry to state: Take off"""
print("enterTakingOff")
self.mc.take_off(uglyConst.TAKEOFF_HEIGHT,uglyConst.TAKEOFF_ZVEL)
def stateTakingOff(self):
"""State: Taking off"""
print("stateTakingOff")
if self.mc._thread.get_height() >= uglyConst.TAKEOFF_HEIGHT:
return self.stateSeeking
else:
time.sleep(0.05)
return self.stateTakingOff
def stateSeeking(self):
"""State: Seeking. Slowly ascend until marker is detected. TODO: Implement some kind of search algorithm (circle outward?)"""
self.mc._set_vel_setpoint(0.0, 0.0, 0.01, 0.0)
print("stateSeeking")
if self.state.isMarkerDetected:
return self.stateNearing
else:
time.sleep(0.05)
return self.stateSeeking
def stateNearing(self):
"""
State: Nearing
Control in pixel frame. Takes in error in pixels (note: camera coordinates), outputs velocity command in x,y,z. Z vel inversely proportional to radius.
"""
x = self.ctrl.errorpixx
y = self.ctrl.errorpixy
cmdx = y*uglyConst.PIX_Kx
cmdy = x*uglyConst.PIX_Ky
r = np.sqrt(x*x+y*y)
if r > 0.0:
cmdz = (1/r)*uglyConst.PIX_Kz
else:
cmdz = 1
cmdx, cmdy, cmdz = self.limOutputVel(cmdx, cmdy, cmdz)
self.mc._set_vel_setpoint(cmdx, cmdy, -cmdz, 0.0)
print("stateNearing")
if self.isClosePix() and self.mc._thread.get_height() < uglyConst.NEARING2APPROACH_HEIGHT:
self.state.cv_mode = uglyConst.CVMODE_POSE
return self.stateApproachin
else:
time.sleep(0.05)
return self.stateNearing
def stateApproachingXY(self):
"""
State: Approaching (XY plane)
Control in the horizontal XY plane and yaw angle.
"""
#self.mc._set_vel_setpoint(self.ctrl.errorx*(uglyConst.Kx+self.ctrl.K), self.ctrl.errory*(uglyConst.Ky+self.ctrl.K), 0.0, 30.0)
self.mc._set_vel_setpoint(self.ctrl.errorx*uglyConst.Kx, self.ctrl.errory*uglyConst.Ky, 0.0, -self.ctrl.erroryaw*uglyConst.Kyaw)
print("stateApproachingXY")
if not self.isClosePix:
return self.stateNearing
if self.isCloseCone() and np.abs(self.ctrl.erroryaw) < uglyConst.FAR_ANGL:
return self.stateApproachingXYZ
else:
time.sleep(0.05)
return self.stateApproachingXY
def stateApproachingXYZ(self):
"""
State: Approaching
Control in world frame. Takes in error in meters, outputs velocity command in x,y. Constant vel cmd in z.
"""
self.mc._set_vel_setpoint(self.ctrl.errorx*uglyConst.Kx, self.ctrl.errory*uglyConst.Ky, -uglyConst.APPROACH_ZVEL, -self.ctrl.erroryaw*uglyConst.Kyaw)
print("stateApproachingXYZ")
if not self.isCloseCone:
return self.stateApproachingXY
elif self.mc._thread.get_height() < uglyConst.APPROACH2LANDING_HEIGHT:
return self.stateDescending
else:
time.sleep(0.05)
return self.stateApproachingXYZ
def stateDescending(self):
"""
State: Landing
Procedure: Descend to a set height, then stop and land.
"""
self.mc._set_vel_setpoint(self.ctrl.errorx*uglyConst.Kx*2.0, self.ctrl.errory*uglyConst.Ky*2.0, -uglyConst.LANDING_ZVEL, -self.ctrl.erroryaw*uglyConst.Kyaw)
print("stateDescending")
if self.mc._thread.get_height() > uglyConst.APPROACH2LANDING_HEIGHT:
return self.stateApproaching
elif self.mc._thread.get_height() < uglyConst.LANDING2LANDED_HEIGHT:
#self.exitLanding()
#return self.stateTerminating
return self.stateLanding
else:
time.sleep(0.01)
return self.stateDescending
def stateLanding(self):
""""""
self.mc._set_vel_setpoint(self.ctrl.errorx*uglyConst.Kx*4.0, self.ctrl.errory*uglyConst.Ky*4.0, -uglyConst.MAX_ZVEL, -self.ctrl.erroryaw*uglyConst.Kyaw*2.0)
self.descCounter += 1
print("stateLanding")
if self.descCounter > 10:
self.mc.land()
return self.stateTerminating
else:
time.sleep(0.01)
return self.stateLanding
def exitLanding(self):
"""
Exit from state: Landing
Stop movement (vel cmds=0), then descend.
"""
self.mc.land()
print("exitLandning")
def stateTerminating(self):
"""
State: Landed
Dummy state.
"""
print("stateTerminating")
return None
def run(self):
"""Main loop, state machine"""
try:
state = self.stateInit # initial state
self.stateNum = uglyConst.S0_INIT
while state and self.runSM:
state = state()
finally:
#-- Clean up
print('Stopping cf_thread')
if self.cf is not None:
if self.mc._is_flying: # if cf has not landed, do so
self.mc.stop()
print('Finally landing')
self.mc.land()
#-- Close link and logging
self.scf.close_link()
self.file.close()
class Node:
def __init__(self):
self.vx = 0
self.vy = 0
self.vz = 0
self.va = 0
self.vel = Twist()
self.pose = Pose()
self.vel.linear.x = self.vx
self.vel.linear.y = self.vy
self.vel.linear.z = self.vz
self.vel.angular.z = self.va
cflib.crtp.init_drivers(enable_debug_driver=False)
self.crazyflie = SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache'))
self.commander = MotionCommander(self.crazyflie)
self.cf = Crazyflie()
self.crazyflie.open_link()
self.commander.take_off()
def write_to_file(self, data):
# Two loggers should yield an even number of rows of data
# being collected in the end.
# There is one packet missing if the array only contains
# an even number of rows of data.
if len(data) % 2 is not 0:
data = data[:len(data) - 1]
temp_df = pd.DataFrame(data)
m, _ = temp_df.shape
even_rows = temp_df.iloc[np.arange(0, m, 2), :]
odd_rows = temp_df.iloc[np.arange(1, m, 2), :]
columns = [
'timestamp_start', 'timestamp_end', 'acc.x', 'acc.y', 'acc.z',
'gyro.x', 'gyro.y', 'gyro.z', 'stateEstimate.x', 'stateEstimate.y',
'stateEstimate.z', 'stabilizer.pitch', 'stabilizer.roll',
'stabilizer.yaw'
]
df = pd.DataFrame(data=np.zeros((int(m / 2), 14)), columns=columns)
df[['gyro.x', 'gyro.y',
'gyro.z']] = np.array(even_rows[['gyro.x', 'gyro.y', 'gyro.z']])
df[['stabilizer.pitch', 'stabilizer.roll',
'stabilizer.yaw']] = np.array(even_rows[[
'stabilizer.pitch', 'stabilizer.roll', 'stabilizer.yaw'
]])
df[["acc.x", "acc.y",
"acc.z"]] = np.array(odd_rows[["acc.x", "acc.y", "acc.z"]])
df[["stateEstimate.x", "stateEstimate.y", "stateEstimate.z"]] = \
np.array(odd_rows[["stateEstimate.x", "stateEstimate.y", "stateEstimate.z"]])
df['timestamp_start'] = np.array(even_rows.timestamp)
df['timestamp_end'] = np.array(odd_rows.timestamp)
# df.to_csv("data/project2/drone2/data_set_label_"
# +class_label+"_packet_"+packet_num+".csv")
df.to_csv("test.csv")
def write(self, data):
print(data)
def log1(self):
lg1 = LogConfig(name='pose_acc', period_in_ms=10)
lg1.add_variable('stateEstimate.x', 'float')
lg1.add_variable('stateEstimate.y', 'float')
lg1.add_variable('stateEstimate.z', 'float')
lg1.add_variable('acc.x', 'float')
lg1.add_variable('acc.y', 'float')
lg1.add_variable('acc.z', 'float')
return lg1
def log2(self):
lg2 = LogConfig(name='stabilizer_gyro', period_in_ms=10)
lg2.add_variable('stabilizer.roll', 'float')
lg2.add_variable('stabilizer.pitch', 'float')
lg2.add_variable('stabilizer.yaw', 'float')
lg2.add_variable('gyro.x', 'float')
lg2.add_variable('gyro.y', 'float')
lg2.add_variable('gyro.z', 'float')
return lg2
def sync(self, position_pub, data):
switch = 0
with SyncLogger(self.crazyflie, self.log1()) as logger1, \
SyncLogger(self.crazyflie, self.log2()) as logger2:
end_time = time.time() + 24
while time.time() < end_time:
if switch == 0:
logger = logger2
elif switch == 1:
logger = logger1
for log_entry in logger:
row = log_entry[1]
row["timestamp"] = log_entry[0]
if switch == 1:
x = row['stateEstimate.x']
y = row['stateEstimate.y']
z = row['stateEstimate.z']
self.pose.position.x = x
self.pose.position.y = y
self.pose.position.z = z
position_pub.publish(self.pose)
print('x:', x, ' y:', y, ' z:', z)
print()
data.append(row)
switch += 1
break
if switch == 2:
switch = 0
return None
def shut_down(self):
self.crazyflie.close_link()
def cmd_vel(self, msg):
self.vx = msg.linear.x
self.vy = msg.linear.y
self.vz = msg.linear.z
self.va = msg.angular.z
self.commander._set_vel_setpoint(self.vx, self.vy, self.vz, self.va)
print(self.vx, self.vy, self.vz, self.va)
class CrazyflieControl:
def __init__(self, name, crazyflie):
# Instantiate motion commander
self._cf = crazyflie
self._name = name
self._mc = MotionCommander(self._cf)
# Topic Publishers
self._velocity_setpoint_pub = rospy.Publisher(self._name +
'/velocity_setpoint',
Vector3,
queue_size=10)
"""
Services hosted for this crazyflie controller
"""
self._reset_position_estimator_srv = rospy.Service(
self._name + '/reset_position_estimator', ResetPositionEstimator,
self._reset_position_estimator_cb)
self._send_hover_setpoint_srv = rospy.Service(
self._name + '/send_hover_setpoint', SendHoverSetpoint,
self._send_hover_setpoint_cb)
self._set_param_srv = rospy.Service(self._name + '/set_param',
SetParam, self._set_param_cb)
self._velocity_control_srv = rospy.Service(
self._name + '/velocity_control', VelocityControl,
self._velocity_control_cb)
"""
Action servers for this crazyflie controller
"""
self._position_control_as = actionlib.SimpleActionServer(
self._name + '/position_control', PositionControlAction,
self._position_control_cb, False)
self._position_control_as.start()
"""
Service Callbacks
"""
def _reset_position_estimator_cb(self, req):
pass
def _send_hover_setpoint_cb(self, req):
vx = req.vx
vy = req.vy
z = req.z
yaw_rate = req.yaw_rate
self._cf.commander.send_hover_setpoint(vx, vy, yaw_rate, z)
return []
def _set_param_cb(self, req):
self._cf.param.set_value(req.param, req.value)
print("set %s to %s" % (req.param, req.value))
return SetParamResponse()
def _velocity_control_cb(self, req):
try:
obj = pickle.loads(req.pickle)
print(self._mc)
if isinstance(obj, SetVelSetpoint):
self._mc._set_vel_setpoint(obj.vx, obj.vy, obj.vz,
obj.rate_yaw)
elif isinstance(obj, StartBack):
self._mc.start_back(velocity=obj.velocity)
elif isinstance(obj, StartCircleLeft):
self._mc.start_circle_left(obj.radius_m, velocity=obj.velocity)
elif isinstance(obj, StartCircleRight):
self._mc.start_turn_right(obj.radius_m, velocity=obj.velocity)
elif isinstance(obj, StartDown):
self._mc.start_down(velocity=obj.velocity)
elif isinstance(obj, StartForward):
self._mc.start_forward(velocity=obj.velocity)
elif isinstance(obj, StartLeft):
self._mc.start_left(velocity=obj.velocity)
elif isinstance(obj, StartLinearMotion):
self._mc.start_linear_motion(obj.vx, obj.vy, obj.vz)
elif isinstance(obj, StartRight):
self._mc.start_right(velocity=obj.velocity)
elif isinstance(obj, StartTurnLeft):
self._mc.start_turn_left(rate=obj.rate)
elif isinstance(obj, StartTurnRight):
self._mc.start_turn_right(rate=obj.rate)
elif isinstance(obj, StartUp):
self._mc.start_up(velocity=obj.velocity)
elif isinstance(obj, Stop):
self._mc.stop()
else:
return 'Object is not a valid velocity command'
except Exception as e:
print(str(e))
raise e
return 'ok'
"""
Action Implementations
"""
def _position_control_cb(self, goal):
try:
obj = pickle.loads(goal.pickle)
if isinstance(obj, Back):
self._mc.back(obj.distance_m, velocity=obj.velocity)
elif isinstance(obj, CircleLeft):
self._mc.circle_left(obj.radius_m,
velocity=obj.velocity,
angle_degrees=obj.angle_degrees)
elif isinstance(obj, CircleRight):
self._mc.circle_right(obj.radius_m,
velocity=obj.velocity,
angle_degrees=obj.angle_degrees)
elif isinstance(obj, Down):
self._mc.down(obj.distance_m, velocity=obj.velocity)
elif isinstance(obj, Forward):
self._mc.forward(obj.distance_m, velocity=obj.velocity)
elif isinstance(obj, Land):
self._mc.land(velocity=obj.velocity)
elif isinstance(obj, Left):
self._mc.left(obj.distance_m, velocity=obj.velocity)
elif isinstance(obj, MoveDistance):
self._mc.move_distance(obj.x,
obj.y,
obj.z,
velocity=obj.velocity)
elif isinstance(obj, Right):
self._mc.right(obj.distance_m, velocity=obj.velocity)
elif isinstance(obj, TakeOff):
self._mc.take_off(height=obj.height, velocity=obj.velocity)
elif isinstance(obj, TurnLeft):
self._mc.turn_left(obj.angle_degrees, rate=obj.rate)
elif isinstance(obj, TurnRight):
self._mc.turn_right(obj.angle_degrees, rate=obj.rate)
elif isinstance(obj, Up):
self._mc.up(obj.distance_m, velocity=obj.velocity)
except Exception as e:
print('Exception in action server %s' % self._name +
'/position_control')
print(str(e))
print('Action aborted')
self._position_control_as.set_aborted()
return
self._position_control_as.set_succeeded()
def _takeoff(self, goal):
try:
self._mc.take_off(height=goal.height)
time.sleep(5)
except BaseException as e:
self._takeoff_as.set_aborted()
print(e)
return
self._takeoff_as.set_succeeded(TakeOffResult(True))
def _land(self, goal):
try:
self._mc.land(velocity=goal.velocity)
except BaseException as e:
self._land_as.set_aborted()
print(e)
return
self._land_as.set_succeeded(LandResult(True))
def _move_distance(self, goal):
try:
x = goal.x
y = goal.y
z = goal.z
velocity = goal.velocity
dist = np.sqrt(x**2 + y**2 + z**2)
vx = x / dist * velocity
vy = y / dist * velocity
vz = z / dist * velocity
# self._velocity_setpoint_pub.publish(Vector3(vx, vy, vz))
self._mc.move_distance(x, y, z, velocity=velocity)
# self._velocity_setpoint_pub.publish(Vector3(vx, vy, vz))
except BaseException as e:
self._move_distance_as.set_aborted()
print(e)
return
self._move_distance_as.set_succeeded()
def key_ctrl(self, scf):
mc = MotionCommander(scf)
mc._reset_position_estimator()
print 'Spacebar to start'
raw_input()
pygame.display.set_mode((400, 300))
print 'WASD for throttle & yaw; arrow keys for left/right/forward/backward'
print 'Spacebar to land'
vel_x = 0
vel_y = 0
vel_z = 0
yaw_rate = 0
try:
mc.take_off()
#set inital Yaw value
with open('SensorMaster.txt', 'r') as stbFile:
stbLines = stbFile.readlines()
while len(stbLines) == 0:
with open('SensorMaster.txt', 'r') as stbFile:
stbLines = stbFile.readlines()
currAttitude = stbLines[len(stbLines) - 1]
need, to, currentYaw, test = currAttitude.split(',')
self.yawCurr = float(currentYaw)
Thread(target=self._updateYaw, args=(scf, )).start()
while True:
for event in pygame.event.get():
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_w:
vel_z = 0.3
if event.key == pygame.K_SPACE:
print 'Space pressed, landing'
mc.land()
time.sleep(2)
print 'Closing link...'
scf.close_link()
print 'Link closed'
pygame.quit()
sys.exit(0)
if event.key == pygame.K_a:
yaw_rate = -45
if event.key == pygame.K_s:
vel_z = -0.3
if event.key == pygame.K_d:
yaw_rate = 45
if event.key == pygame.K_UP:
#vel_x = 0.3
vel_x = 0.2 * self.fwCoef[0]
vel_y = 0.2 * self.fwCoef[1]
print('vel_x = %.2f, vel_y = %.2f' %
(vel_x * self.fwCoef[0],
vel_y * self.fwCoef[1]))
print("the current yaw is:")
print(self.yawCurr)
if event.key == pygame.K_DOWN:
#vel_x = -0.3
vel_x = 0.2 * self.bkCoef[0]
vel_y = 0.2 * self.bkCoef[1]
print('vel_x = %.2f, vel_y = %.2f' %
(vel_x * self.bkCoef[0],
vel_y * self.bkCoef[1]))
print("the current yaw is:")
print(self.yawCurr)
if event.key == pygame.K_LEFT:
#vel_y = 0.3
vel_x = 0.2 * self.lfCoef[0]
vel_y = 0.2 * self.lfCoef[1]
print('vel_x = %.2f, vel_y = %.2f' %
(vel_x * self.bkCoef[0],
vel_y * self.bkCoef[1]))
print("the current yaw is:")
print(self.yawCurr)
if event.key == pygame.K_RIGHT:
#vel_y = -0.3
vel_x = 0.2 * self.rtCoef[0]
vel_y = 0.2 * self.rtCoef[1]
print('vel_x = %.2f, vel_y = %.2f' %
(vel_x * self.bkCoef[0],
vel_y * self.bkCoef[1]))
print("the current yaw is:")
print(self.yawCurr)
if event.key == pygame.K_r:
#set recalibrate initial Yaw value
with open('SensorMaster.txt', 'r') as stbFile:
stbLines = stbFile.readlines()
while len(stbLines) == 0:
with open('SensorMaster.txt', 'r') as stbFile:
stbLines = stbFile.readlines()
print("yaw angle recalibrated :")
newInitAttitude = stbLines[len(stbLines) - 1]
need, to, initYaw, test = newInitAttitude.split(
',')
print(initYaw)
self.yawInit = float(initYaw)
if event.key == pygame.K_RCTRL:
mc.stop()
if event.type == pygame.KEYUP:
if event.key == pygame.K_w or event.key == pygame.K_s:
vel_z = 0
if event.key == pygame.K_a or event.key == pygame.K_d:
yaw_rate = 0
if event.key == pygame.K_UP or event.key == pygame.K_DOWN:
vel_x = 0
if event.key == pygame.K_LEFT or event.key == pygame.K_RIGHT:
vel_y = 0
mc._set_vel_setpoint(vel_x, vel_y, vel_z, yaw_rate)
except KeyboardInterrupt:
print('\nShutting down...')
scf.close_link()
except Exception, e:
print str(e)
scf.close_link()
class CrazyDrone(Drone):
def __init__(self, link_uri):
super().__init__()
cache = "./cache"
if getattr(sys, 'frozen', False):
cache = sys._MEIPASS + os.path.sep + "cache"
self._cf = Crazyflie(rw_cache=cache)
self.motion_commander = None
self.multiranger = None
# maximum speeds
self.max_vert_speed = 1
self.max_horiz_speed = 1
self.max_rotation_speed = 90
self.logger = None
# 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)
print('Connecting to %s' % link_uri)
# Try to connect to the Crazyflie
self._cf.open_link(link_uri)
# Variable used to keep main loop occupied until disconnect
self.is_connected = True
def _connected(self, link_uri):
""" 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_uri)
self.connection.emit("progress")
# The definition of the logconfig can be made before connecting
self.logger = LogConfig("Battery", 1000) # delay
self.logger.add_variable("pm.vbat", "float")
try:
self._cf.log.add_config(self.logger)
self.logger.data_received_cb.add_callback(
lambda e, f, g: self.batteryValue.emit(float(f['pm.vbat'])))
# self.logger.error_cb.add_callback(lambda: print('error'))
self.logger.start()
except KeyError as e:
print(e)
self.connection.emit("on")
self.motion_commander = MotionCommander(self._cf, 0.5)
self.multiranger = Multiranger(self._cf, rate_ms=50)
self.multiranger.start()
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the speficied address)"""
print('Connection to %s failed: %s' % (link_uri, msg))
self.is_connected = False
self.connection.emit("off")
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
print('Connection to %s lost: %s' % (link_uri, msg))
self.connection.emit("off")
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
print('Disconnected from %s' % link_uri)
self.is_connected = False
self.connection.emit("off")
def take_off(self):
if self._cf.is_connected(
) and self.motion_commander and not self.motion_commander._is_flying:
self.motion_commander.take_off()
self.is_flying_signal.emit(True)
def land(self):
if self._cf.is_connected(
) and self.motion_commander and self.motion_commander._is_flying:
self.motion_commander.land()
self.is_flying_signal.emit(False)
def stop(self):
if not (self.logger is None):
self.logger.stop()
if self.motion_commander:
self.motion_commander.land()
if self.multiranger:
self.multiranger.stop()
self._cf.close_link()
def is_flying(self):
if self._cf.is_connected() and self.motion_commander:
return self.motion_commander._is_flying
return False
def process_motion(self, _up, _rotate, _front, _right):
if self.motion_commander:
# WARNING FOR CRAZYFLY
# positive X is forward, # positive Y is left # positive Z is up
velocity_z = _up * self.max_vert_speed
velocity_yaw = _rotate * self.max_rotation_speed
velocity_x = _front * self.max_horiz_speed
velocity_y = -_right * self.max_horiz_speed
# print("PRE", velocity_x, velocity_y, velocity_z, velocity_yaw)
# protect against collision by reducing speed depending on distance
ranger = self.multiranger
if ranger.front and ranger.front < ANTI_COLLISION_DISTANCE and velocity_x > 0:
velocity_x = velocity_x * (
ranger.front -
ANTI_COLLISION_MIN_DIST) / ANTI_COLLISION_DISTANCE
velocity_x = max(0, velocity_x)
if ranger.back and ranger.back < ANTI_COLLISION_DISTANCE and velocity_x < 0:
velocity_x = velocity_x * (
ranger.back -
ANTI_COLLISION_MIN_DIST) / ANTI_COLLISION_DISTANCE
velocity_x = min(0, velocity_x)
if ranger.left and ranger.left < ANTI_COLLISION_DISTANCE and velocity_y > 0:
velocity_y = velocity_y * (
ranger.left -
ANTI_COLLISION_MIN_DIST) / ANTI_COLLISION_DISTANCE
velocity_y = max(0, velocity_y)
if ranger.right and ranger.right < ANTI_COLLISION_DISTANCE and velocity_y < 0:
velocity_y = velocity_y * (
ranger.left -
ANTI_COLLISION_MIN_DIST) / ANTI_COLLISION_DISTANCE
velocity_y = min(0, velocity_y)
if ranger.up and ranger.up < ANTI_COLLISION_DISTANCE and velocity_z > 0:
velocity_z = velocity_z * (ranger.up - ANTI_COLLISION_MIN_DIST
) / ANTI_COLLISION_DISTANCE
velocity_z = max(0, velocity_z)
# print("POST", velocity_x, velocity_y, velocity_z, velocity_yaw)
if self.motion_commander._is_flying:
self.motion_commander._set_vel_setpoint(
velocity_x, velocity_y, velocity_z, velocity_yaw)
class Aruco_tracker():
def __init__(self, distance):
"""
Inicialização dos drivers, parâmetros do controle PID e decolagem do drone.
"""
self.distance = distance
self.pid_foward = PID(distance, 0.01, 0.0001, 0.01, 500, -500, 0.7,
-0.7)
self.pid_yaw = PID(0, 0.33, 0.0, 0.33, 500, -500, 100, -100)
self.pid_angle = PID(0.0, 0.01, 0.0, 0.01, 500, -500, 0.3, -0.3)
self.pid_height = PID(0.0, 0.002, 0.0002, 0.002, 500, -500, 0.3, -0.2)
cflib.crtp.init_drivers(enable_debug_driver=False)
self.factory = CachedCfFactory(rw_cache='./cache')
self.URI4 = 'radio://0/40/2M/E7E7E7E7E4'
self.cf = Crazyflie(rw_cache='./cache')
self.sync = SyncCrazyflie(self.URI4, cf=self.cf)
self.sync.open_link()
self.mc = MotionCommander(self.sync)
self.cont = 0
self.notFoundCount = 0
self.calculator = DistanceCalculator()
self.cap = cv2.VideoCapture(1)
self.mc.take_off()
time.sleep(1)
def search_marker(self):
"""
Interrompe o movimento se nao encontrar o marcador por tres frames
consecutivos. Após 4 segundos, inicia movimento de rotação para
procurar pelo marcador.
"""
if ((3 <= self.notFoundCount <= 20) and self.mc.isRunning):
self.mc.stop()
self.mc.setIsRunning(False)
elif (self.notFoundCount > 20):
self.mc._set_vel_setpoint(0.0, 0.0, 0.0, 80)
self.mc.setIsRunning(True)
return
def update_motion(self):
"""
Atualiza as velocidades utilizando controlador PID.
"""
horizontal_distance = self.calculator.horizontal_distance
vertical_distance = self.calculator.vertical_distance
x_distance = self.calculator.distance_x
alpha = self.calculator.alpha
velocity_x = self.pid_foward.update(x_distance)
Velocity_z = self.pid_height.update(vertical_distance)
if (x_distance < (self.distance + 10)):
velocity_y = self.pid_angle.update(alpha)
else:
velocity_y = 0
velocity_z = 0
yaw = self.pid_yaw.update(horizontal_distance)
self.mc._set_vel_setpoint(velocity_x, velocity_y, 0, yaw)
self.mc.setIsRunning(True)
return
def track_marker(self):
"""
Programa principal, drone segue um marcador aruco.
"""
while (self.cap.isOpened()):
ret, frame = self.cap.read()
if (frame is None):
break
if (self.cont % 6 == 0):
thread = threading.Thread(
target=self.calculator.calculateDistance, args=(frame, ))
thread.setDaemon(True)
thread.start()
if (self.calculator.markerFound):
self.notFoundCount = 0
self.update_motion()
else:
self.notFoundCount += 1
self.search_marker()
self.calculator.writeDistance(frame)
cv2.imshow('frame', frame)
self.cont += 1
if cv2.waitKey(10) & 0xFF == ord('q'):
self.mc.land()
cv2.destroyAllWindows()
break
cv2.waitKey()
self.sync.close_link()
else:
velocity_x = -0.5
# velocity_z = pid_height.update(vertical_distance)
if (x_distance < 50):
if calculator.alpha > 0:
velocity_y = 0.3
else:
velocity_y = -0.3
if (calculator.horizontal_distance > 0):
yaw = 30
else:
yaw = -30
mc._set_vel_setpoint(velocity_x, velocity_y, 0, yaw)
mc.setIsRunning(True)
else:
# Drone interrompe o movimento somente se não encontrar o marcador
# em 3 frames consecutivos
notFoundCount += 1
if ((3 <= notFoundCount <= 20) and mc.isRunning):
mc.stop()
mc.setIsRunning(False)
elif (notFoundCount > 20):
mc._set_vel_setpoint(0.0, 0.0, 0.0, 100)
mc.setIsRunning(True)
calculator.writeDistance(frame)
cv2.imshow('frame', frame)
