class Drone: """Drone instance. Starts and flies drone""" logging.basicConfig(level=logging.ERROR) URI = 'radio://0/80/250K' def __init__(self, URI='radio://0/80/250K'): """ Init commad connects to drone :param URI: URI """ self._cf = Crazyflie(rw_cache='./cache') cflib.crtp.init_drivers(enable_debug_driver=False) self.scf = SyncCrazyflie(URI, cf=self._cf) self.scf.__enter__() self.motionCommander = MotionCommander(self.scf) #self.multiranger = Multiranger(self.scf) def take_off(self, height=1.0, velocity=0.2): """has the drone take off""" # drone takes off self.motionCommander.take_off(height=height, velocity=velocity) def land(self): self.motionCommander.land() def move(self, vector): """moves the drone at a constant speed in one direction""" if vector['y'] > 0.1: print "move up by {}m".format(vector['y']) self.motionCommander.up(vector['y'], velocity=0.4) elif vector['y'] < -0.1: print "move down by {}m".format(abs(vector['y'])) self.motionCommander.down(abs(vector['y']), velocity=0.4) if vector['x'] > 0.1: print "move left by {}m".format(vector['x']) self.motionCommander.left(vector['x'], velocity=0.4) elif vector['x'] < -0.1: print "move right by {}m".format(abs(vector['x'])) self.motionCommander.right(abs(vector['x']), velocity=0.4) if vector['z'] > 2.1: print "move fowards by {}m".format(vector['z'] - 2.0) self.motionCommander.forward(vector['z'] - 2.0, velocity=0.4) self elif vector['z'] < 1.9: print "move backwards by {}m".format(2.0 - vector['z']) self.motionCommander.back(2.0 - vector['z'], velocity=0.4) def disconnect(self): self.scf.close_link() def is_close(range): MIN_DISTANCE = 0.4 # m if range is None: return False else: return range < MIN_DISTANCE
class UAVController(): def __init__(self, targetURI=None): """ Function: __init__ Purpose: Initialize all necessary UAV functionality Inputs: none Outputs: none """ cflib.crtp.init_drivers() #self.FD = open("logFile.txt", "w") self.timeout = True self.available = [] self.UAV = Crazyflie() self.param = None self.airborne = False self._recentDataPacket = None self._receivingDataPacket = False #Attempt to locate UAV by scanning available interface for _ in range(0, 500): if (len(self.available) > 0): self.timeout = False break #If a UAV is found via scanning, break out of this loop else: self.available = cflib.crtp.scan_interfaces() if (len(self.available) > 0): if (targetURI != None): for i in range(len(self.available)): if (self.available[i][0] == targetURI): self.UAV.open_link(self.available[i][0]) self.connectedToTargetUAV = True else: self.connectedToTargetUAV = False else: self.UAV.open_link(self.available[0][0]) while (self.UAV.is_connected() == False): time.sleep(0.1) self.MC = MotionCommander(self.UAV) #Create desired logging parameters self.UAVLogConfig = LogConfig(name="UAVLog", period_in_ms=100) #self.UAVLogConfig.add_variable('pm.batteryLevel', 'float') self.UAVLogConfig.add_variable('pm.vbat', 'float') self.UAVLogConfig.add_variable('pm.batteryLevel', 'float') #self.UAVLogConfig.add_variable('stateEstimate.x', 'float') #self.UAVLogConfig.add_variable('stateEstimate.y', 'float') self.UAVLogConfig.add_variable('stateEstimate.z', 'float') self.UAVLogConfig.add_variable('pm.chargeCurrent', 'float') self.UAVLogConfig.add_variable('pm.extCurr', 'float') self.UAVLogConfig.add_variable('pwm.m1_pwm', 'float') #self.UAVLogConfig.add_variable('baro.pressure', 'float') #self.UAVLogConfig.add_variable('extrx.thrust', 'float') #Add more variables here for logging as desired self.UAV.log.add_config(self.UAVLogConfig) if (self.UAVLogConfig.valid): self.UAVLogConfig.data_received_cb.add_callback( self._getUAVDataPacket) self.UAVLogConfig.start() else: logger.warning("Could not setup log configuration") self._startTime = time.time() #For testing purposes self._trialRun = 0 #End of function def done(self): """ Function: done Purpose: Close connection to UAV to terminate all threads running Inputs: none Outputs: none """ self.UAVLogConfig.stop() self.UAV.close_link() self.airborne = False return def launch(self): """ Function: launch Purpose: Instruct the UAV to takeoff from current position to the default height Inputs: none Outputs: none """ self.airborne = True self.MC.take_off() #End of function return def land(self): """ Function: launch Purpose: Instruct the UAV to land on the ground at current position Inputs: none Outputs: none """ self.MC.land() return def move(self, distanceX, distanceY, distanceZ, velocity): """ Function: move Purpose: A wrapper function to instruct an UAV to move <x, y, z> distance from current point Inputs: distance - a floating point value distance in meters velocity - a floating point value velocity in meters per second Outputs: none """ if (self.airborne == False): self.launch() self.MC.move_distance(distanceX, distanceY, distanceZ, velocity) #End of function return def rotate(self, degree): """ Function: rotate Purpose: A wrapper function to instruct an UAV to rotate Inputs: degree - a floating point value in degrees Outputs: none """ if (self.airborne == False): self.launch() if (degree < 0): print("UC: rotate - Going Right") locDeg = 0 #self.MC.turn_right(abs(degree)) for _ in range(1, int(abs(degree) / 1)): locDeg += 1 self.MC.turn_right(1) self.MC.turn_right(abs(degree) - locDeg) else: print("UC: rotate - Going Left") self.MC.turn_left(abs(degree)) #Delay by 1 seclond, to allow for total rotation time time.sleep(1) return def getBatteryLevel(self): """ Function: getBatteryLevel Purpose: A function that reads the UAV battery level from a IOStream Inputs: none Outputs: none Description: """ retVal = None if (self._recentDataPacket != None and self._receivingDataPacket == False): retVal = self._recentDataPacket["pm.vbat"] return retVal def getHeight(self): """ Function: getCurrentHeight Purpose: A function that reads the UAV height from a IOStream Inputs: none Outputs: none Description: """ retVal = None if (self._recentDataPacket != None and self._receivingDataPacket == False): retVal = self._recentDataPacket["stateEstimate.z"] return retVal def isCharging(self): """ Function: getCurrentHeight Purpose: A function that reads the UAV height from a IOStream Inputs: none Outputs: none Description: """ retVal = None if (self._recentDataPacket != None and self._receivingDataPacket == False): retVal = self._recentDataPacket["pm.chargeCurrent"] return retVal def _getUAVDataPacket(self, ident, data, logconfig): """ Function: getUAVDataPacket Purpose: Process a data packet received from the UAV Inputs: ident - data - logconfig - Outputs: None Description: A user should never call this function. """ self._receivingDataPacket = True self._recentDataPacket = data self._receivingDataPacket = False def appendToLogFile(self): """ Function: appendToLogFile Purpose: append log variables to log file 'log.txt.' Inputs: none Outputs: none Description: """ #retVal = None if (self._recentDataPacket != None and self._receivingDataPacket == False): current = self._recentDataPacket["pm.chargeCurrent"] extcurrent = self._recentDataPacket["pm.extCurr"] voltage = self._recentDataPacket["pm.vbat"] bat_level = self._recentDataPacket["pm.batteryLevel"] height = self._recentDataPacket["stateEstimate.z"] #x = self._recentDataPacket["stateEstimate.y"] #y = self._recentDataPacket["stateEstimate.x"] m1_pwm = self._recentDataPacket["pwm.m1_pwm"] #extrx_thrust = self._recentDataPacket["extrx.thrust"] #baro_pressure = self._recentDataPacket["baro.pressure"] with open('log2.txt', 'a') as file: #print(batlevel) file.write(str(datetime.now()) + '\n') file.write('bat_voltage: ' + str(voltage) + '\n') file.write('bat_level: ' + str(bat_level) + '\n') file.write('crg_current: ' + str(current) + '\n') file.write('ext_current: ' + str(extcurrent) + '\n') file.write('height: ' + str(height) + '\n') #file.write('x: ' + str(x) + '\n') #file.write('y: ' + str(y) + '\n') file.write('m1_pwm: ' + str(m1_pwm) + '\n')
class TestMotionCommander(unittest.TestCase): def setUp(self): self.commander_mock = MagicMock(spec=Commander) self.param_mock = MagicMock(spec=Param) self.cf_mock = MagicMock(spec=Crazyflie) self.cf_mock.commander = self.commander_mock self.cf_mock.param = self.param_mock self.cf_mock.is_connected.return_value = True self.sut = MotionCommander(self.cf_mock) def test_that_the_estimator_is_reset_on_take_off( self, _SetPointThread_mock, sleep_mock): # Fixture # Test self.sut.take_off() # Assert self.param_mock.set_value.assert_has_calls([ call('kalman.resetEstimation', '1'), call('kalman.resetEstimation', '0') ]) def test_that_take_off_raises_exception_if_not_connected( self, _SetPointThread_mock, sleep_mock): # Fixture self.cf_mock.is_connected.return_value = False # Test # Assert with self.assertRaises(Exception): self.sut.take_off() def test_that_take_off_raises_exception_when_already_flying( self, _SetPointThread_mock, sleep_mock): # Fixture self.sut.take_off() # Test # Assert with self.assertRaises(Exception): self.sut.take_off() def test_that_it_goes_up_on_take_off( self, _SetPointThread_mock, sleep_mock): # Fixture thread_mock = _SetPointThread_mock() # Test self.sut.take_off(height=0.4, velocity=0.5) # Assert thread_mock.set_vel_setpoint.assert_has_calls([ call(0.0, 0.0, 0.5, 0.0), call(0.0, 0.0, 0.0, 0.0) ]) sleep_mock.assert_called_with(0.4 / 0.5) def test_that_it_goes_up_to_default_height( self, _SetPointThread_mock, sleep_mock): # Fixture thread_mock = _SetPointThread_mock() sut = MotionCommander(self.cf_mock, default_height=0.4) # Test sut.take_off(velocity=0.6) # Assert thread_mock.set_vel_setpoint.assert_has_calls([ call(0.0, 0.0, 0.6, 0.0), call(0.0, 0.0, 0.0, 0.0) ]) sleep_mock.assert_called_with(0.4 / 0.6) def test_that_the_thread_is_started_on_takeoff( self, _SetPointThread_mock, sleep_mock): # Fixture thread_mock = _SetPointThread_mock() # Test self.sut.take_off() # Assert thread_mock.start.assert_called_with() def test_that_it_goes_down_on_landing( self, _SetPointThread_mock, sleep_mock): # Fixture thread_mock = _SetPointThread_mock() thread_mock.get_height.return_value = 0.4 self.sut.take_off() thread_mock.reset_mock() # Test self.sut.land(velocity=0.5) # Assert thread_mock.set_vel_setpoint.assert_has_calls([ call(0.0, 0.0, -0.5, 0.0), call(0.0, 0.0, 0.0, 0.0) ]) sleep_mock.assert_called_with(0.4 / 0.5) def test_that_it_takes_off_and_lands_as_context_manager( self, _SetPointThread_mock, sleep_mock): # Fixture thread_mock = _SetPointThread_mock() thread_mock.reset_mock() thread_mock.get_height.return_value = 0.3 # Test with self.sut: pass # Assert thread_mock.set_vel_setpoint.assert_has_calls([ call(0.0, 0.0, 0.2, 0.0), call(0.0, 0.0, 0.0, 0.0), call(0.0, 0.0, -0.2, 0.0), call(0.0, 0.0, 0.0, 0.0) ]) sleep_mock.assert_called_with(0.3 / 0.2) sleep_mock.assert_called_with(0.3 / 0.2) def test_that_it_starts_moving_multi_dimensional( self, _SetPointThread_mock, sleep_mock): # Fixture thread_mock = _SetPointThread_mock() self.sut.take_off() thread_mock.reset_mock() # Test self.sut.start_linear_motion(0.1, 0.2, 0.3) # Assert thread_mock.set_vel_setpoint.assert_has_calls([ call(0.1, 0.2, 0.3, 0.0), ]) def test_that_it_starts_moving( self, _SetPointThread_mock, sleep_mock): # Fixture self.sut.take_off() vel = 0.3 data = [ [self.sut.start_forward, (vel, 0.0, 0.0, 0.0)], [self.sut.start_back, (-vel, 0.0, 0.0, 0.0)], [self.sut.start_left, (0.0, vel, 0.0, 0.0)], [self.sut.start_right, (0.0, -vel, 0.0, 0.0)], [self.sut.start_up, (0.0, 0.0, vel, 0.0)], [self.sut.start_down, (0.0, 0.0, -vel, 0.0)], ] # Test # Assert for line in data: self._verify_start_motion(line[0], vel, line[1], _SetPointThread_mock) def test_that_it_moves_multi_dimensional_distance( self, _SetPointThread_mock, sleep_mock): # Fixture thread_mock = _SetPointThread_mock() x = 0.1 y = 0.2 z = 0.3 vel = 0.2 distance = math.sqrt(x * x + y * y + z * z) expected_time = distance / vel expected_vel_x = vel * x / distance expected_vel_y = vel * y / distance expected_vel_z = vel * z / distance self.sut.take_off() thread_mock.reset_mock() sleep_mock.reset_mock() # Test self.sut.move_distance(x, y, z) # Assert thread_mock.set_vel_setpoint.assert_has_calls([ call(expected_vel_x, expected_vel_y, expected_vel_z, 0.0), ]) sleep_mock.assert_called_with(expected_time) def test_that_it_moves(self, _SetPointThread_mock, sleep_mock): # Fixture vel = 0.3 self.sut.take_off() data = [ [self.sut.forward, (vel, 0.0, 0.0, 0.0)], [self.sut.back, (-vel, 0.0, 0.0, 0.0)], [self.sut.left, (0.0, vel, 0.0, 0.0)], [self.sut.right, (0.0, -vel, 0.0, 0.0)], [self.sut.up, (0.0, 0.0, vel, 0.0)], [self.sut.down, (0.0, 0.0, -vel, 0.0)], ] # Test # Assert for test in data: self._verify_move(test[0], vel, test[1], _SetPointThread_mock, sleep_mock) def test_that_it_starts_turn_right(self, _SetPointThread_mock, sleep_mock): # Fixture rate = 20 thread_mock = _SetPointThread_mock() self.sut.take_off() thread_mock.reset_mock() # Test self.sut.start_turn_right(rate) # Assert thread_mock.set_vel_setpoint.assert_has_calls([ call(0.0, 0.0, 0.0, rate), ]) def test_that_it_starts_turn_left(self, _SetPointThread_mock, sleep_mock): # Fixture rate = 20 thread_mock = _SetPointThread_mock() self.sut.take_off() thread_mock.reset_mock() # Test self.sut.start_turn_left(rate) # Assert thread_mock.set_vel_setpoint.assert_has_calls([ call(0.0, 0.0, 0.0, -rate), ]) def test_that_it_starts_circle_right( self, _SetPointThread_mock, sleep_mock): # Fixture velocity = 0.5 radius = 0.9 expected_rate = 360 * velocity / (2 * radius * math.pi) thread_mock = _SetPointThread_mock() self.sut.take_off() thread_mock.reset_mock() # Test self.sut.start_circle_right(radius, velocity=velocity) # Assert thread_mock.set_vel_setpoint.assert_has_calls([ call(velocity, 0.0, 0.0, expected_rate), ]) def test_that_it_starts_circle_left( self, _SetPointThread_mock, sleep_mock): # Fixture velocity = 0.5 radius = 0.9 expected_rate = 360 * velocity / (2 * radius * math.pi) thread_mock = _SetPointThread_mock() self.sut.take_off() thread_mock.reset_mock() # Test self.sut.start_circle_left(radius, velocity=velocity) # Assert thread_mock.set_vel_setpoint.assert_has_calls([ call(velocity, 0.0, 0.0, -expected_rate), ]) def test_that_it_turns_right(self, _SetPointThread_mock, sleep_mock): # Fixture rate = 20 angle = 45 turn_time = angle / rate thread_mock = _SetPointThread_mock() self.sut.take_off() thread_mock.reset_mock() # Test self.sut.turn_right(angle, rate=rate) # Assert thread_mock.set_vel_setpoint.assert_has_calls([ call(0.0, 0.0, 0.0, rate), call(0.0, 0.0, 0.0, 0.0) ]) sleep_mock.assert_called_with(turn_time) def test_that_it_turns_left(self, _SetPointThread_mock, sleep_mock): # Fixture rate = 20 angle = 45 turn_time = angle / rate thread_mock = _SetPointThread_mock() self.sut.take_off() thread_mock.reset_mock() # Test self.sut.turn_left(angle, rate=rate) # Assert thread_mock.set_vel_setpoint.assert_has_calls([ call(0.0, 0.0, 0.0, -rate), call(0.0, 0.0, 0.0, 0.0) ]) sleep_mock.assert_called_with(turn_time) def test_that_it_circles_right(self, _SetPointThread_mock, sleep_mock): # Fixture radius = 0.7 velocity = 0.3 angle = 45 distance = 2 * radius * math.pi * angle / 360.0 turn_time = distance / velocity rate = angle / turn_time thread_mock = _SetPointThread_mock() self.sut.take_off() thread_mock.reset_mock() # Test self.sut.circle_right(radius, velocity, angle) # Assert thread_mock.set_vel_setpoint.assert_has_calls([ call(velocity, 0.0, 0.0, rate), call(0.0, 0.0, 0.0, 0.0) ]) sleep_mock.assert_called_with(turn_time) def test_that_it_circles_left(self, _SetPointThread_mock, sleep_mock): # Fixture radius = 0.7 velocity = 0.3 angle = 45 distance = 2 * radius * math.pi * angle / 360.0 turn_time = distance / velocity rate = angle / turn_time thread_mock = _SetPointThread_mock() self.sut.take_off() thread_mock.reset_mock() # Test self.sut.circle_left(radius, velocity, angle) # Assert thread_mock.set_vel_setpoint.assert_has_calls([ call(velocity, 0.0, 0.0, -rate), call(0.0, 0.0, 0.0, 0.0) ]) sleep_mock.assert_called_with(turn_time) ###################################################################### def _verify_start_motion(self, function_under_test, velocity, expected, _SetPointThread_mock): # Fixture thread_mock = _SetPointThread_mock() thread_mock.reset_mock() # Test function_under_test(velocity=velocity) # Assert try: thread_mock.set_vel_setpoint.assert_has_calls([ call(*expected), ]) except AssertionError as e: self._eprint('Failed when testing function ' + function_under_test.__name__) raise e def _verify_move(self, function_under_test, velocity, expected, _SetPointThread_mock, sleep_mock): # Fixture thread_mock = _SetPointThread_mock() distance = 1.2 expected_time = distance / velocity thread_mock.reset_mock() sleep_mock.reset_mock() # Test function_under_test(distance, velocity=velocity) # Assert try: thread_mock.set_vel_setpoint.assert_has_calls([ call(*expected), call(0.0, 0.0, 0.0, 0.0), ]) sleep_mock.assert_called_with(expected_time) except AssertionError as e: print('Failed when testing function ' + function_under_test.__name__) raise e
from cflib.positioning.motion_commander import MotionCommander URI = 'radio://0/80/2M' if __name__ == "__main__": mp = MotionCommander(URI) mp.take_off(height=0.3, velocity=0.2) mp.land(velocity=0.2)
print("zero") elif (calculator.distance_x > 30): if (mc.isRunning): mc.stop() mc.start_linear_motion(0.1, 0.0, 0.0) mc.setIsRunning(True) print("para frente") elif (calculator.distance_x < 25): if (mc.isRunning): mc.stop() mc.start_linear_motion(-0.1, 0.0, 0.0) mc.setIsRunning(True) print("para tras") else: if (mc.isRunning): mc.stop() cont += 1 if cv2.waitKey(10) & 0xFF == ord('q'): break soma = 0 for t in exeTime: soma += t print("A media é ", soma / len(exeTime)) cv2.waitKey() cv2.destroyAllWindows() mc.land() sync.close_link()
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()
class UAVController(): def __init__(self, targetURI=None): """ Function: __init__ Purpose: Initialize all necessary UAV functionality Inputs: none Outputs: none Description: An initializer function that finds a Crazyflie, can be particular target or not, and sets up all necessary data values for logging data values from the Crazyflie. """ #Enable the CrazyRadio PA drivers to communicate with the UAV. cflib.crtp.init_drivers() self.timeout = True self.available = [] self.UAV = Crazyflie() self.param = None self.airborne = False self._recentDataPacket = None self._receivingDataPacket = False #Attempt to locate UAV by scanning available interface for _ in range(0, 500): if (len(self.available) > 0): self.timeout = False break #If a UAV is found via scanning, break out of this loop else: self.available = cflib.crtp.scan_interfaces() #If there are Crazyflies available, connect to one of them if (len(self.available) > 0): #If there is a specific target, parse through the returned array to locate specific Crazyflie if (targetURI != None): for i in range(len(self.available)): if (self.available[i][0] == targetURI): #If found, open a link to the Crazyflie self.UAV.open_link(self.available[i][0]) self.connectedToTargetUAV = True else: self.connectedToTargetUAV = False else: #If there is not a specific target, simply connect to the first Crazyflie available self.UAV.open_link(self.available[0][0]) #While the Crazyflie is not connected, wait until the connection occurs to allow for Motion Commander initialization while (self.UAV.is_connected() == False): time.sleep(0.1) #Pass the connected Crazyflie to the Motion Commander class self.MC = MotionCommander(self.UAV) #Create desired logging parameters self.UAVLogConfig = LogConfig(name="UAVLog", period_in_ms=100) self.UAVLogConfig.add_variable('pm.vbat', 'float') self.UAVLogConfig.add_variable('stateEstimate.x', 'float') self.UAVLogConfig.add_variable('stateEstimate.y', 'float') self.UAVLogConfig.add_variable('stateEstimate.z', 'float') self.UAVLogConfig.add_variable('pm.chargeCurrent', 'float') #Add more variables here for logging as desired #Add the configured logger to the Crazyflie to begin grabbing data packets self.UAV.log.add_config(self.UAVLogConfig) if (self.UAVLogConfig.valid): self.UAVLogConfig.data_received_cb.add_callback( self._getUAVDataPacket) self.UAVLogConfig.start() else: logger.warning("Could not setup log configuration") #End of function def done(self): """ Function: done Purpose: Close connection to UAV to terminate all threads running Inputs: none Outputs: none Description: See purpose. """ self.UAVLogConfig.stop() self.UAV.close_link() self.airborne = False return def launch(self): """ Function: launch Purpose: Instruct the UAV to takeoff from current position to the default height Inputs: none Outputs: none Description: A wrapper function that calls the Crazyflie Motion Commander take_off function. See Bitcraze Crazyflie documentation for further details. """ self.airborne = True self.MC.take_off() #End of function return def land(self): """ Function: launch Purpose: Instruct the UAV to land on the ground at current position Inputs: none Outputs: none Description: A function that calls the Crazyflie Motion Commander Land function. See Bitcraze Crazyflie documentation for further details. """ self.airborne = False self.MC.land() return def move(self, distanceX, distanceY, distanceZ, velocity): """ Function: move Purpose: A wrapper function to instruct an UAV to move <x, y, z> distance from current point Inputs: distanceX - a floating point value that represents the distance to move the in the X-dimension, measured in meters. distanceY - a floating point value that represents the distance to move the in the Y-dimension, measured in meters. distanceZ - a floating point value that represents the distance to move the in the Z-dimension, measured in meters. velocity - a floating point value that represents the velocity of the UAV during movement, measured in meters per second Outputs: none Description: See purpose. """ if (self.airborne == False): self.launch() self.MC.move_distance(distanceX, distanceY, distanceZ, velocity) #End of function return def rotate(self, degree): """ Function: rotate Purpose: A wrapper function to instruct an UAV to rotate Inputs: degree - a floating point value in degrees Outputs: none Description: Currently this function is not in use as rotating the Crazyflie regularly introduces positional errors. In particular, the Crazyflie will rotate on a motor, not the center of the drone. """ if (self.airborne == False): self.launch() if (degree < 0): print("UC: rotate - Going Right") locDeg = 0 #self.MC.turn_right(abs(degree)) for _ in range(1, int(abs(degree) / 1)): locDeg += 1 self.MC.turn_right(1) self.MC.turn_right(abs(degree) - locDeg) else: print("UC: rotate - Going Left") self.MC.turn_left(abs(degree)) #Delay by 1 second, to allow for total rotation time time.sleep(1) return def getBatteryLevel(self): """ Function: getBatteryLevel Purpose: A function that reads the UAV battery level from an IOStream Inputs: none Outputs: retVal - a floating point value that represents the battery voltage of the UAV. Description: See purpose. """ retVal = None if (self._recentDataPacket != None and self._receivingDataPacket == False): retVal = self._recentDataPacket["pm.vbat"] return retVal def getHeight(self): """ Function: getCurrentHeight Purpose: A function that reads the UAV height from a IOStream. Inputs: none Outputs: retVal - a floating point value that represents the onboard height detection of the UAV. Description: See purpose. """ retVal = None if (self._recentDataPacket != None and self._receivingDataPacket == False): retVal = self._recentDataPacket["stateEstimate.z"] return retVal def isCharging(self): """ Function: getCurrentHeight Purpose: A function that reads the UAV charge current from a IOStream Inputs: none Outputs: retVal - a floating point value that represents the charge current of the UAV. Description: See purpose. """ retVal = None if (self._recentDataPacket != None and self._receivingDataPacket == False): retVal = self._recentDataPacket["pm.chargeCurrent"] return retVal def _getUAVDataPacket(self, ident, data, logconfig): """ Function: getUAVDataPacket Purpose: A callback function to process a data packet received from the UAV Inputs: ident - identifier of the UAV data - data from the UAV logconfig - log configuration from the UAV Outputs: None Description: A user should NEVER call this function. """ self._receivingDataPacket = True self._recentDataPacket = data self._receivingDataPacket = False
class TestMotionCommander(unittest.TestCase): def setUp(self): self.commander_mock = MagicMock(spec=Commander) self.param_mock = MagicMock(spec=Param) self.cf_mock = MagicMock(spec=Crazyflie) self.cf_mock.commander = self.commander_mock self.cf_mock.param = self.param_mock self.cf_mock.is_connected.return_value = True self.sut = MotionCommander(self.cf_mock) def test_that_the_estimator_is_reset_on_take_off(self, _SetPointThread_mock, sleep_mock): # Fixture # Test self.sut.take_off() # Assert self.param_mock.set_value.assert_has_calls([ call('kalman.resetEstimation', '1'), call('kalman.resetEstimation', '0') ]) def test_that_take_off_raises_exception_if_not_connected( self, _SetPointThread_mock, sleep_mock): # Fixture self.cf_mock.is_connected.return_value = False # Test # Assert with self.assertRaises(Exception): self.sut.take_off() def test_that_take_off_raises_exception_when_already_flying( self, _SetPointThread_mock, sleep_mock): # Fixture self.sut.take_off() # Test # Assert with self.assertRaises(Exception): self.sut.take_off() def test_that_it_goes_up_on_take_off(self, _SetPointThread_mock, sleep_mock): # Fixture thread_mock = _SetPointThread_mock() # Test self.sut.take_off(height=0.4, velocity=0.5) # Assert thread_mock.set_vel_setpoint.assert_has_calls( [call(0.0, 0.0, 0.5, 0.0), call(0.0, 0.0, 0.0, 0.0)]) sleep_mock.assert_called_with(0.4 / 0.5) def test_that_the_thread_is_started_on_takeoff(self, _SetPointThread_mock, sleep_mock): # Fixture thread_mock = _SetPointThread_mock() # Test self.sut.take_off() # Assert thread_mock.start.assert_called_with() def test_that_it_goes_down_on_landing(self, _SetPointThread_mock, sleep_mock): # Fixture thread_mock = _SetPointThread_mock() thread_mock.get_height.return_value = 0.4 self.sut.take_off() thread_mock = _SetPointThread_mock() thread_mock.reset_mock() # Test self.sut.land(velocity=0.5) # Assert thread_mock.set_vel_setpoint.assert_has_calls( [call(0.0, 0.0, -0.5, 0.0), call(0.0, 0.0, 0.0, 0.0)]) sleep_mock.assert_called_with(0.4 / 0.5) def test_that_it_starts_moving_multi_dimensional(self, _SetPointThread_mock, sleep_mock): # Fixture thread_mock = _SetPointThread_mock() self.sut.take_off() thread_mock.reset_mock() # Test self.sut.start_linear_motion(0.1, 0.2, 0.3) # Assert thread_mock.set_vel_setpoint.assert_has_calls([ call(0.1, 0.2, 0.3, 0.0), ]) def test_that_it_starts_moving(self, _SetPointThread_mock, sleep_mock): # Fixture self.sut.take_off() vel = 0.3 data = [ [self.sut.start_forward, (vel, 0.0, 0.0, 0.0)], [self.sut.start_back, (-vel, 0.0, 0.0, 0.0)], [self.sut.start_left, (0.0, vel, 0.0, 0.0)], [self.sut.start_right, (0.0, -vel, 0.0, 0.0)], [self.sut.start_up, (0.0, 0.0, vel, 0.0)], [self.sut.start_down, (0.0, 0.0, -vel, 0.0)], ] # Test # Assert for line in data: self._verify_start_motion(line[0], vel, line[1], _SetPointThread_mock) def test_that_it_moves_multi_dimensional_distance(self, _SetPointThread_mock, sleep_mock): # Fixture thread_mock = _SetPointThread_mock() x = 0.1 y = 0.2 z = 0.3 vel = 0.2 distance = math.sqrt(x * x + y * y + z * z) expected_time = distance / vel expected_vel_x = vel * x / distance expected_vel_y = vel * y / distance expected_vel_z = vel * z / distance self.sut.take_off() thread_mock.reset_mock() sleep_mock.reset_mock() # Test self.sut.move_distance(x, y, z) # Assert thread_mock.set_vel_setpoint.assert_has_calls([ call(expected_vel_x, expected_vel_y, expected_vel_z, 0.0), ]) sleep_mock.assert_called_with(expected_time) def test_that_it_moves(self, _SetPointThread_mock, sleep_mock): # Fixture vel = 0.3 self.sut.take_off() data = [ [self.sut.forward, (vel, 0.0, 0.0, 0.0)], [self.sut.back, (-vel, 0.0, 0.0, 0.0)], [self.sut.left, (0.0, vel, 0.0, 0.0)], [self.sut.right, (0.0, -vel, 0.0, 0.0)], [self.sut.up, (0.0, 0.0, vel, 0.0)], [self.sut.down, (0.0, 0.0, -vel, 0.0)], ] # Test # Assert for test in data: self._verify_move(test[0], vel, test[1], _SetPointThread_mock, sleep_mock) def test_that_it_starts_turn_right(self, _SetPointThread_mock, sleep_mock): # Fixture rate = 20 thread_mock = _SetPointThread_mock() self.sut.take_off() thread_mock.reset_mock() # Test self.sut.start_turn_right(rate) # Assert thread_mock.set_vel_setpoint.assert_has_calls([ call(0.0, 0.0, 0.0, rate), ]) def test_that_it_starts_turn_left(self, _SetPointThread_mock, sleep_mock): # Fixture rate = 20 thread_mock = _SetPointThread_mock() self.sut.take_off() thread_mock.reset_mock() # Test self.sut.start_turn_left(rate) # Assert thread_mock.set_vel_setpoint.assert_has_calls([ call(0.0, 0.0, 0.0, -rate), ]) def test_that_it_starts_circle_right(self, _SetPointThread_mock, sleep_mock): # Fixture velocity = 0.5 radius = 0.9 expected_rate = 360 * velocity / (2 * radius * math.pi) thread_mock = _SetPointThread_mock() self.sut.take_off() thread_mock.reset_mock() # Test self.sut.start_circle_right(radius, velocity=velocity) # Assert thread_mock.set_vel_setpoint.assert_has_calls([ call(velocity, 0.0, 0.0, expected_rate), ]) def test_that_it_starts_circle_left(self, _SetPointThread_mock, sleep_mock): # Fixture velocity = 0.5 radius = 0.9 expected_rate = 360 * velocity / (2 * radius * math.pi) thread_mock = _SetPointThread_mock() self.sut.take_off() thread_mock.reset_mock() # Test self.sut.start_circle_left(radius, velocity=velocity) # Assert thread_mock.set_vel_setpoint.assert_has_calls([ call(velocity, 0.0, 0.0, -expected_rate), ]) def test_that_it_turns_right(self, _SetPointThread_mock, sleep_mock): # Fixture rate = 20 angle = 45 turn_time = angle / rate thread_mock = _SetPointThread_mock() self.sut.take_off() thread_mock.reset_mock() # Test self.sut.turn_right(angle, rate=rate) # Assert thread_mock.set_vel_setpoint.assert_has_calls( [call(0.0, 0.0, 0.0, rate), call(0.0, 0.0, 0.0, 0.0)]) sleep_mock.assert_called_with(turn_time) def test_that_it_turns_left(self, _SetPointThread_mock, sleep_mock): # Fixture rate = 20 angle = 45 turn_time = angle / rate thread_mock = _SetPointThread_mock() self.sut.take_off() thread_mock.reset_mock() # Test self.sut.turn_left(angle, rate=rate) # Assert thread_mock.set_vel_setpoint.assert_has_calls( [call(0.0, 0.0, 0.0, -rate), call(0.0, 0.0, 0.0, 0.0)]) sleep_mock.assert_called_with(turn_time) def test_that_it_circles_right(self, _SetPointThread_mock, sleep_mock): # Fixture radius = 0.7 velocity = 0.3 angle = 45 distance = 2 * radius * math.pi * angle / 360.0 turn_time = distance / velocity rate = angle / turn_time thread_mock = _SetPointThread_mock() self.sut.take_off() thread_mock.reset_mock() # Test self.sut.circle_right(radius, velocity, angle) # Assert thread_mock.set_vel_setpoint.assert_has_calls( [call(velocity, 0.0, 0.0, rate), call(0.0, 0.0, 0.0, 0.0)]) sleep_mock.assert_called_with(turn_time) def test_that_it_circles_left(self, _SetPointThread_mock, sleep_mock): # Fixture radius = 0.7 velocity = 0.3 angle = 45 distance = 2 * radius * math.pi * angle / 360.0 turn_time = distance / velocity rate = angle / turn_time thread_mock = _SetPointThread_mock() self.sut.take_off() thread_mock.reset_mock() # Test self.sut.circle_left(radius, velocity, angle) # Assert thread_mock.set_vel_setpoint.assert_has_calls( [call(velocity, 0.0, 0.0, -rate), call(0.0, 0.0, 0.0, 0.0)]) sleep_mock.assert_called_with(turn_time) ###################################################################### def _verify_start_motion(self, function_under_test, velocity, expected, _SetPointThread_mock): # Fixture thread_mock = _SetPointThread_mock() thread_mock.reset_mock() # Test function_under_test(velocity=velocity) # Assert try: thread_mock.set_vel_setpoint.assert_has_calls([ call(*expected), ]) except AssertionError as e: self._eprint('Failed when testing function ' + function_under_test.__name__) raise e def _verify_move(self, function_under_test, velocity, expected, _SetPointThread_mock, sleep_mock): # Fixture thread_mock = _SetPointThread_mock() distance = 1.2 expected_time = distance / velocity thread_mock.reset_mock() sleep_mock.reset_mock() # Test function_under_test(distance, velocity=velocity) # Assert try: thread_mock.set_vel_setpoint.assert_has_calls([ call(*expected), call(0.0, 0.0, 0.0, 0.0), ]) sleep_mock.assert_called_with(expected_time) except AssertionError as e: print('Failed when testing function ' + function_under_test.__name__) raise e
class FlightCtrl: #initialize variables for attitude tracking yawInit = 0 yawCurr = 0 theta = 0 rtCoef = [0, -1] lfCoef = [0, 1] fwCoef = [1, 0] bkCoef = [-1, 0] lvSpeed = 0.3 def __init__(self, _scf): self.mc = MotionCommander(_scf, default_height=0.7) self.scf = _scf self.scf.open_link() def perform_gesture(self, g_id): global consecDoubleTaps global inMotion d = 0.3 if g_id[0] == DOUBLE_TAP: if self.mc._is_flying: print("Hovering...") #mc.stop() self.resetYawInit() else: consecDoubleTaps = 0 print("Taking off...") inMotion = True self.mc.take_off() inMotion = False self.resetYawInit() threadUpdate = Thread(target=self._updateYaw, args=(self.scf, )) threadUpdate.start() elif g_id[0] == NO_POSE and g_id[1] == yaw_id: print("Roll...") inMotion = True self.mc.move_distance(0, math.copysign(d, g_id[2]), 0) inMotion = False """if (g_id[2] > 0): #turn left print("turning left") inMotion = True self.mc.move_distance(self.lvSpeed * self.lfCoef[0], self.lvSpeed * self.lfCoef[1], 0) inMotion = False else: #turn right print("turning right") inMotion = True self.mc.move_distance(self.lvSpeed * self.rtCoef[0], self.lvSpeed * self.rtCoef[1], 0) inMotion = False """ elif g_id[0] == NO_POSE and g_id[1] == pitch_id: print("Pitch...") inMotion = True self.mc.move_distance(-math.copysign(d, g_id[2]), 0, 0) inMotion = False """if (g_id[2] < 0): #move forward print("moving forward") inMotion = True self.mc.move_distance(self.lvSpeed * self.fwCoef[0], self.lvSpeed * self.fwCoef[1], 0) inMotion = False else: #move backward print("moving backward") inMotion = True self.mc.move_distance(self.lvSpeed * self.bkCoef[0], self.lvSpeed * self.bkCoef[1], 0) inMotion = False """ elif g_id[0] == FINGERS_SPREAD and g_id[1] == pitch_id: if g_id[2] > 0: if self.mc._thread.get_height() + d < self.mc.max_height: print("Up...") inMotion = True self.mc.up(d) inMotion = False else: print("Max. height %.2fm reached: requested height: %.2f" ) % (self.mc.max_height, self.mc._thread.get_height() + d) else: if self.mc._thread.get_height() - d < self.mc.min_height: print("Down...") inMotion = True self.mc.down(d) inMotion = False else: print("Max. height %.2fm reached: requested height: %.2f" ) % (self.mc.max_height, self.mc._thread.get_height() + d) elif g_id[0] == FIST and g_id[1] == yaw_id: print('Yaw...') if g_id[2] > 0: inMotion = True self.mc.turn_left(30) inMotion = False else: inMotion = True self.mc.turn_right(30) inMotion = False elif g_id[0] == LAND: print("Landing...") inMotion = True self.mc.land() inMotion = False else: #rest behaviour if self.mc._is_flying: inMotion = True self.mc.stop() inMotion = False """Functions to update attitude by reading storage text file""" def updateCoef(self): try: self.theta = (self.yawCurr - self.yawInit) * (3.1415926 / 180) self.rtCoef = [-sin(self.theta), -cos(self.theta)] self.lfCoef = [sin(self.theta), cos(self.theta)] self.fwCoef = [cos(self.theta), -sin(self.theta)] self.bkCoef = [-cos(self.theta), sin(self.theta)] except Exception, e: print str(e) print("Update failed")
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()
import logging import time import cflib.crtp from cflib.crazyflie import Crazyflie from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.positioning.motion_commander import MotionCommander URI = 'radio://0/60/2M/E7E7E7E7E7' # Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) if __name__ == '__main__': # Initialize the low-level drivers (don't list the debug drivers) cflib.crtp.init_drivers(enable_debug_driver=False) with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: # We take off when the commander is create mc = MotionCommander(scf) mc.take_off(0.5, 0.3) time.sleep(1) mc.circle_right(1, velocity=0.6, angle_degrees=180) mc.up(0.5) mc.circle_right(1, velocity=0.6, angle_degrees=180) mc.land(0.2)
class MyCrazyFlieClient: def __init__(self): cflib.crtp.init_drivers(enable_debug_driver=False) cf = Crazyflie(rw_cache='./cache') self.scf = SyncCrazyflie(URI, cf=cf) self.scf.open_link() self.client = None log_config = LogConfig(name='kalman', period_in_ms=100) log_config.add_variable('kalman.stateX', 'float') log_config.add_variable('kalman.stateY', 'float') self.logger_pos = SyncLogger(self.scf, log_config) log_config_2 = LogConfig(name='stabilizer', period_in_ms=100) log_config_2.add_variable('stabilizer.yaw', 'float') self.logger_orientation = SyncLogger(self.scf, log_config_2) self.home_pos = self.get_position() print('Home = %s ' % self.home_pos) self.orientation = self.get_orientation() self.client = MotionCommander(self.scf) self.client.take_off(height=0.6) def land(self): self.client.land() self.scf.close_link() def check_if_in_target(self, goal): pos = self.get_position() distance = np.sqrt( np.power((goal[0] - pos[0]), 2) + np.power((goal[1] - pos[1]), 2)) if distance < 1: self.land() return True return False def get_position(self): self.logger_pos.connect() data = self.logger_pos.next()[1] self.logger_pos.disconnect() self.logger_pos._queue.empty() return [data['kalman.stateX'], -data['kalman.stateY']] def get_orientation(self): self.logger_orientation.connect() data = self.logger_orientation.next()[1] self.logger_orientation.disconnect() self.logger_orientation._queue.empty() return -data['stabilizer.yaw'] def straight(self, speed): self.client.forward(0.25, speed) start = time.time() return start def yaw_right(self): self.client.turn_right(30, 72) start = time.time() return start def yaw_left(self): self.client.turn_left(30, 72) start = time.time() return start def take_action(self, action): start = time.time() collided = False if action == 0: start = self.straight(0.25) if action == 1: start = self.yaw_right() if action == 2: start = self.yaw_left() # print(start) return collided def goal_direction(self, goal, pos): yaw = self.get_orientation() print('yaw now %s' % yaw) pos_angle = math.atan2(goal[1] - pos[1], goal[0] - pos[0]) pos_angle = math.degrees(pos_angle) % 360 print('pos angle %s' % pos_angle) track = pos_angle - yaw track = ((track - 180) % 360) - 180 print('Track = %s ' % track) return track def observe(self, goal): position_now = self.get_position() track = self.goal_direction(goal, position_now) distance = np.sqrt( np.power((goal[0] - position_now[0]), 2) + np.power((goal[1] - position_now[1]), 2)) distance_sensor = Multiranger(self.scf) distance_sensor.start() front_distance_sensor = distance_sensor.front while front_distance_sensor is None: time.sleep(0.01) front_distance_sensor = distance_sensor.front distance_sensor.stop() print('Position now = %s ' % position_now) # print('Track = %s ' % track) print('Distance to target: %s' % distance) print('Front distance: %s' % front_distance_sensor) return position_now[0], position_now[ 1], track, distance, front_distance_sensor
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 Mission: def __init__(self, URI): # Tool to process the data from drone's sensors self.processing = Processing(URI) cflib.crtp.init_drivers(enable_debug_driver=False) self.cf = Crazyflie(ro_cache=None, rw_cache='./cache') # Connect callbacks from the Crazyflie API self.cf.connected.add_callback(self.connected) self.cf.disconnected.add_callback(self.disconnected) # Connect to the Crazyflie self.cf.open_link(URI) time.sleep(3) self.pose_home = self.position self.pose_home[2] = 0.1 self.velocity = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'yaw': 0.0} self.goal = np.array([0.5, 0.0, 0.3]) self.mc = MotionCommander(self.cf) time.sleep(3) self.mc.take_off(0.15, 0.2) time.sleep(1) self.mc.circle_right(0.8, velocity=0.5, angle_degrees=360) time.sleep(2) self.mc.up(0.1) time.sleep(1) self.mc.circle_right(0.8, velocity=0.5, angle_degrees=360) time.sleep(2) self.mc.up(0.1) time.sleep(1) self.mc.circle_right(0.8, velocity=0.5, angle_degrees=360) time.sleep(2) self.mc.land(0.2) ''' Mission to a goal and return to home position ''' # self.mc.take_off(0.5, 0.2) # time.sleep(1) # rate = rospy.Rate(10) # while not rospy.is_shutdown(): # self.sendVelocityCommand() # rate.sleep() def coverage_mission(self, height=0.2, length=1.0, width=0.3, numiters=2): self.mc.take_off(height, 0.2) time.sleep(1) for _ in range(numiters): self.mc.forward(length) time.sleep(0.1) self.mc.turn_right(90) time.sleep(0.1) self.mc.forward(width) time.sleep(0.1) self.mc.turn_right(90) time.sleep(0.1) self.mc.forward(length) time.sleep(0.1) self.mc.turn_left(90) time.sleep(0.1) self.mc.forward(width) time.sleep(0.1) self.mc.turn_left(90) time.sleep(0.1) self.mc.land(0.2) time.sleep(1) def square_mission(self, numiters=2): ''' Square trajectory to collect data with multiranger. ''' self.mc.take_off(0.15, 0.2) time.sleep(1) self.mc.turn_left(45) time.sleep(0.1) # sequence of repeated squares for _ in range(numiters): # sqaure for _ in range(4): self.mc.forward(1.4) time.sleep(2) self.mc.turn_right(90) time.sleep(1) self.mc.up(0.1) time.sleep(1) for _ in range(numiters): # sqaure for _ in range(4): self.mc.forward(1.4) time.sleep(2) self.mc.turn_right(90) time.sleep(1) self.mc.land(0.2) time.sleep(1) def sendVelocityCommand(self): direction = normalize(self.goal - self.position) v_x = SPEED_FACTOR * direction[0] v_y = SPEED_FACTOR * direction[1] v_z = SPEED_FACTOR * direction[2] # Local movement correction from obstacles dV = 0.1 # [m/s] if is_close(self.measurement['left']): # print('Obstacle on the LEFT') v_y -= dV if is_close(self.measurement['right']): # print('Obstacle on the RIGHT') v_y += dV self.velocity['x'] = v_x self.velocity['y'] = v_y self.velocity['z'] = v_z # print('Sending velocity:', self.velocity) goal_dist = norm(self.goal - self.position) # print('Distance to goal %.2f [m]:' %goal_dist) if goal_dist < GOAL_TOLERANCE: # goal is reached # print('Goal is reached. Going home...') self.goal = self.pose_home self.cf.commander.send_velocity_world_setpoint(self.velocity['x'], self.velocity['y'], self.velocity['z'], self.velocity['yaw']) def disconnected(self, URI): print('Disconnected') def connected(self, URI): print('We are now connected to {}'.format(URI)) # The definition of the logconfig can be made before connecting lpos = LogConfig(name='Position', period_in_ms=100) lpos.add_variable('lighthouse.x') lpos.add_variable('lighthouse.y') lpos.add_variable('lighthouse.z') lpos.add_variable('stabilizer.roll') lpos.add_variable('stabilizer.pitch') lpos.add_variable('stabilizer.yaw') try: self.cf.log.add_config(lpos) lpos.data_received_cb.add_callback(self.pos_data) lpos.start() except KeyError as e: print('Could not start log configuration,' '{} not found in TOC'.format(str(e))) except AttributeError: print('Could not add Position log config, bad configuration.') lmeas = LogConfig(name='Meas', period_in_ms=100) lmeas.add_variable('range.front') lmeas.add_variable('range.back') lmeas.add_variable('range.up') lmeas.add_variable('range.left') lmeas.add_variable('range.right') lmeas.add_variable('range.zrange') lmeas.add_variable('stabilizer.roll') lmeas.add_variable('stabilizer.pitch') lmeas.add_variable('stabilizer.yaw') try: self.cf.log.add_config(lmeas) lmeas.data_received_cb.add_callback(self.meas_data) lmeas.start() except KeyError as e: print('Could not start log configuration,' '{} not found in TOC'.format(str(e))) except AttributeError: print('Could not add Measurement log config, bad configuration.') lbat = LogConfig( name='Battery', period_in_ms=500) # read battery status with 2 Hz rate lbat.add_variable('pm.vbat', 'float') try: self.cf.log.add_config(lbat) lbat.data_received_cb.add_callback(self.battery_data) lbat.start() except KeyError as e: print('Could not start log configuration,' '{} not found in TOC'.format(str(e))) except AttributeError: print('Could not add Measurement log config, bad configuration.') def pos_data(self, timestamp, data, logconf): # Transformation according to https://wiki.bitcraze.io/doc:lighthouse:setup position = [ -data['lighthouse.z'], -data['lighthouse.x'], data['lighthouse.y'] # data['stateEstimate.x'], # data['stateEstimate.y'], # data['stateEstimate.z'] ] orientation = [ data['stabilizer.roll'], data['stabilizer.pitch'], data['stabilizer.yaw'] ] self.position = np.array(position) self.orientation = np.array(orientation) self.processing.set_position(position, orientation) def meas_data(self, timestamp, data, logconf): measurement = { 'roll': data['stabilizer.roll'], 'pitch': data['stabilizer.pitch'], 'yaw': data['stabilizer.yaw'], 'front': data['range.front'], 'back': data['range.back'], 'up': data['range.up'], 'down': data['range.zrange'], 'left': data['range.left'], 'right': data['range.right'] } self.measurement = measurement self.processing.set_measurement(measurement) def battery_data(self, timestamp, data, logconf): self.V_bat = data['pm.vbat'] # print('Battery status: %.2f [V]' %self.V_bat) if self.V_bat <= V_BATTERY_TO_GO_HOME: self.battery_state = 'needs_charging' # print('Battery is not charged: %.2f' %self.V_bat) self.goal = self.pose_home elif self.V_bat >= V_BATTERY_CHARGED: self.battery_state = 'fully_charged'
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 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()
class CF: def __init__(self, scf, available): # get yaw-angle and battery level of crazyflie self.vbat, self.blevel, self.m1_pwm, self.temp = getCFparams(scf, available) self.scf = scf self.available = available self.mc = MotionCommander(scf) self.psi_limit = 0.7 # Don't cmd an angle less than this [deg] self.des_angle = 0 # Set to zero initially self.psi = 0 self.roll = 0 self.pitch = 0 self.theta = 0 self.phi = 0 def update(self, des_angle, eul, turnRate): if 'psi' in eul and abs(des_angle) > self.psi_limit: self.des_angle = des_angle if des_angle > 0: if not self.mc.turn_right(abs(self.des_angle), turnRate): pass else: if not self.mc.turn_left(abs(self.des_angle), turnRate): pass # Compute current angle (yaw) of CF self.vbat, self.blevel, self.m1_pwm, self.temp = getCFparams(self.scf, self.available) return(self.vbat, self.blevel, self.m1_pwm, self.temp) # Move cf left or right def update_x(self, dist): if dist is not None and dist != 0: if not self.move_distance(0, -dist, 0): pass elif dist is not None and dist == 0 and self.des_angle == 0: self.mc.stop() def takeoff(self): self.mc.take_off() def land(self): self.mc.land() def move_distance(self, x, y, z): ''' Move in a straight line, {CF frame}. positive X is forward [cm] positive Y is left [cm] positive Z is up [cm] ''' velocity = 0.08 x = x/100 y = y/100 z = z/100 distance = math.sqrt(x**2 + y**2 + z**2) if x != 0: flight_time = distance / velocity velocity_x = velocity * x / distance else: velocity_x = 0 if y != 0: velocity_y = velocity * y / distance else: velocity_y = 0 if z != 0: velocity_z = velocity * z / distance else: velocity_z = 0 self.mc.start_linear_motion(velocity_x, velocity_y, velocity_z) if x != 0: time.sleep(flight_time) return(False)
class Drone: def __init__(self, URI): cflib.crtp.init_drivers(enable_debug_driver=False) self.cf = Crazyflie(ro_cache=None, rw_cache='./cache') # Connect callbacks from the Crazyflie API self.cf.connected.add_callback(self.connected) self.cf.disconnected.add_callback(self.disconnected) # Connect to the Crazyflie self.cf.open_link(URI) # Tool to process the data from drone's sensors # self.processing = Processing() self.motion_commander = MotionCommander(self.cf) time.sleep(3) self.motion_commander.take_off(0.3, 0.2) time.sleep(1) self.motion_commander.start_forward(0.15) time.sleep(0.5) self.stop_recording = [] thread.start_new_thread(self.land_command, ()) self.start_mission() def land_command(self): raw_input() self.stop_recording.append(True) print('Landing...') self.motion_commander.stop() self.motion_commander.land(0.2) time.sleep(1) def start_mission(self): rate = rospy.Rate(4000) while not self.stop_recording: print('fly') self.sendHoverCommand() rate.sleep() def sendHoverCommand(self): """ Change UAV flight direction based on obstacles detected with multiranger. """ if is_close(self.measurement['front'] ) and self.measurement['left'] > self.measurement['right']: print('FRONT RIGHT') self.motion_commander.stop() self.motion_commander.turn_left(60, 70) self.motion_commander.start_forward(0.15) if is_close(self.measurement['front'] ) and self.measurement['left'] < self.measurement['right']: print('FRONT LEFT') self.motion_commander.stop() self.motion_commander.turn_right(60, 70) self.motion_commander.start_forward(0.15) if is_close(self.measurement['left']): print('LEFT') self.motion_commander.right(0.1, 0.2) self.motion_commander.stop() self.motion_commander.turn_right(45, 70) self.motion_commander.start_forward(0.15) if is_close(self.measurement['right']): print('RIGHT') self.motion_commander.left(0.1, 0.2) self.motion_commander.stop() self.motion_commander.turn_left(45, 70) self.motion_commander.start_forward(0.15) def disconnected(self, URI): print('Disconnected') def connected(self, URI): print('We are now connected to {}'.format(URI)) # The definition of the logconfig can be made before connecting lpos = LogConfig(name='Position', period_in_ms=100) lpos.add_variable('lighthouse.x') lpos.add_variable('lighthouse.y') lpos.add_variable('lighthouse.z') lpos.add_variable('stabilizer.roll') lpos.add_variable('stabilizer.pitch') lpos.add_variable('stabilizer.yaw') try: self.cf.log.add_config(lpos) lpos.data_received_cb.add_callback(self.pos_data) lpos.start() except KeyError as e: print('Could not start log configuration,' '{} not found in TOC'.format(str(e))) except AttributeError: print('Could not add Position log config, bad configuration.') lmeas = LogConfig(name='Meas', period_in_ms=100) lmeas.add_variable('range.front') lmeas.add_variable('range.back') lmeas.add_variable('range.up') lmeas.add_variable('range.left') lmeas.add_variable('range.right') lmeas.add_variable('range.zrange') lmeas.add_variable('stabilizer.roll') lmeas.add_variable('stabilizer.pitch') lmeas.add_variable('stabilizer.yaw') try: self.cf.log.add_config(lmeas) lmeas.data_received_cb.add_callback(self.meas_data) lmeas.start() except KeyError as e: print('Could not start log configuration,' '{} not found in TOC'.format(str(e))) except AttributeError: print('Could not add Measurement log config, bad configuration.') lbat = LogConfig( name='Battery', period_in_ms=500) # read battery status with 2 Hz rate lbat.add_variable('pm.vbat', 'float') try: self.cf.log.add_config(lbat) lbat.data_received_cb.add_callback(self.battery_data) lbat.start() except KeyError as e: print('Could not start log configuration,' '{} not found in TOC'.format(str(e))) except AttributeError: print('Could not add Measurement log config, bad configuration.') def pos_data(self, timestamp, data, logconf): # Transformation according to https://wiki.bitcraze.io/doc:lighthouse:setup position = [ -data['lighthouse.z'], -data['lighthouse.x'], data['lighthouse.y'] # data['stateEstimate.x'], # data['stateEstimate.y'], # data['stateEstimate.z'] ] orientation = [ data['stabilizer.roll'], data['stabilizer.pitch'], data['stabilizer.yaw'] ] self.position = position self.orientation = orientation # self.processing.set_position(position, orientation) def meas_data(self, timestamp, data, logconf): measurement = { 'roll': data['stabilizer.roll'], 'pitch': data['stabilizer.pitch'], 'yaw': data['stabilizer.yaw'], 'front': data['range.front'], 'back': data['range.back'], 'up': data['range.up'], 'down': data['range.zrange'], 'left': data['range.left'], 'right': data['range.right'] } self.measurement = measurement # self.processing.set_measurement(measurement) def battery_data(self, timestamp, data, logconf): self.V_bat = data['pm.vbat'] # print('Battery status: %.2f [V]' %self.V_bat) if self.V_bat <= V_BATTERY_TO_GO_HOME: self.battery_state = 'needs_charging' # print('Battery is not charged: %.2f' %self.V_bat) elif self.V_bat >= V_BATTERY_CHARGED: self.battery_state = 'fully_charged'