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 gesture_ctrl(self, scf, fc, g): mc = MotionCommander(scf) mc._reset_position_estimator() print ('Enter to start (c to cancel)') char = raw_input() if char == 'c': print('\nClosing link...') scf.close_link() print('Shutting down...') os.kill(os.getpid(), signal.SIGINT) try: while True: g_id = gesture.get_gesture() if gesture is not None: fc.perform_gesture(g_id, mc) except Exception, e: print (str(e)) scf.close_link()
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()
def __init__(self, scf, available): # get yaw-angle and battery level of crazyflie self.psi, self.theta, self.phi, self.vbat = 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
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 __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)
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_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 move_box_limit(scf): with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: # mc.start_forward() # while (1): # if position_estimate[0] > BOX_LIMIT: # print("Back") # mc.start_back(velocity=0.2) # elif position_estimate[0] < -BOX_LIMIT: # print("Forward") # mc.start_forward(velocity=0.2) # time.sleep(0.1) # without this line the CF cannot reach the order body_x_cmd = 0.2 body_y_cmd = 0.1 max_vel = 0.2 while (1): if position_estimate[0] > BOX_LIMIT: body_x_cmd = -max_vel elif position_estimate[0] < -BOX_LIMIT: body_x_cmd = max_vel if position_estimate[1] > BOX_LIMIT: body_y_cmd = -max_vel elif position_estimate[1] < -BOX_LIMIT: body_y_cmd = max_vel mc.start_linear_motion(body_x_cmd, body_y_cmd, 0) time.sleep(0.1)
def main(): cflib.crtp.init_drivers(enable_debug_driver=False) numRepeticions = int(input("Quants cops vols repetir la ruta?")) ruta = input("Quina ruta vols realitzar?") with SyncCrazyflie(URI) as scf: with MotionCommander(scf) as mq90: if ruta == rutaABC: mq90.up(1) mq90.forward(1) ... ... elif ruta == rutaBC: mq90.up(1) mq90.forward(1) ... ... elif ruta == rutaCB: mq90.up(1) mq90.forward(1) ... ... elif ruta == rutaCA: mq90.up(1) mq90.forward(1) ... ... elif ruta == rutaAC: mq90.up(1) mq90.forward(1) ... ... mq90.land()
def move_baduanjin_mc_p3(scf): with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: print("Target Height: {}".format(DEFAULT_HEIGHT)) t_init = time.time() ## Delay 6 sec time.sleep(6) t1 = time.time() - t_init print("t1: ", t1) ## Go up: d_fly meter/4.5 sec print("Target Height: {}".format(d_abs)) mc.move_distance(0, 0, d_fly, velocity=d_fly/4.5) # the final posistion will be "d_abs = DEFAULT_HEIGHT + d_fly" t2 = time.time() - t_init print("t2: ", t2) ## Delay 2.5 sec time.sleep(2.5) t3 = time.time() - t_init print("t3: ", t3) ## Go down: d_fly meter/4.5 sec print("Target Height: {}".format(DEFAULT_HEIGHT)) mc.move_distance(0, 0, -d_fly, velocity=d_fly/4.5) t4 = time.time() - t_init print("t4: ", t4) ## Delay 2.5 sec time.sleep(2.5) t5 = time.time() - t_init print("t5: ", t5)
def main(): cflib.crtp.init_drivers(enable_debug_driver=False) with SyncCrazyflie(URI) as scf: with MotionCommander(scf) as mq90: print("Take off done!") mq90.up(1)
def move_baduanjin_mc_p1(scf): with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: print("Target Height: {}".format(DEFAULT_HEIGHT)) time.sleep(1) t_init = time.time() ## Go up: d_fly meter/4 sec print("Target Height: {}".format(d_abs)) mc.move_distance( 0, 0, d_fly, velocity=d_fly / 5) # the final posistion will be "d_abs = DEFAULT_HEIGHT + d_fly" t1 = time.time() - t_init print("t1: ", t1) ## Delay 4 sec # mc.stop() time.sleep(4) t2 = time.time() - t_init print("t2: ", t2) ## Go down: d_fly meter/5 sec print("Target Height: {}".format(DEFAULT_HEIGHT)) mc.move_distance(0, 0, -d_fly, velocity=d_fly / 5) t3 = time.time() - t_init print("t3: ", t3) time.sleep(1)
def default_fly_to(self, x, y, results_folder): logging.debug('DroneController: Instructed to move to : (' + str(x) + ',' + str(y) + ')') logging.debug('Connecting to the crazyflie') crazyflie = self.connect_to_crazyflie() logging.debug('DroneController: Setting up the variable logging') self.setup_logging(crazyflie, results_folder) # We take off when the commander is created with MotionCommander(crazyflie) as mc: # Allowing the drone to take off time.sleep(1) # Flying forward the Y amount if y != 0: mc.forward(y) time.sleep(1) # Flying to the right for the X amount if x != 0: mc.right(x) time.sleep(1) mc.stop() # Landing occurs when the Motion Commander goes out of scope # Here we are keeping the application alive as the Crazyflie library makes # non blocking calls while self.is_connected: time.sleep(1)
def selectDrone(self): self.cfs = [] self.scfs = [] self.mcs = [] self.selected = [] message = 'Quais drones deseja controlar? (1, 2, 3, 4 ou combinacao delas):' while (len(self.selected) == 0): user_input_numbers = input(message) if (user_input_numbers.find('1') != -1): self.selected.append(1) if (user_input_numbers.find('2') != -1): self.selected.append(2) if (user_input_numbers.find('3') != -1): self.selected.append(3) if (user_input_numbers.find('4') != -1): self.selected.append(4) if (len(self.selected) == 0): print("comando invalido") try: for i in self.selected: cf = Crazyflie(rw_cache='./cache') self.cfs.append(cf) sync = SyncCrazyflie(self.uris[i - 1], cf=cf) sync.open_link() self.mcs.append(MotionCommander(sync)) self.scfs.append(sync) except: print("Exception: Too many packets lost\n") self.selected = []
def take_off_simple(scf): with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: for i in range(FLIGHT_DURATION_S): if (i % CORRECTION_INTERVAL_S == 0): return_to_zero(mc, current_x, current_y, current_z) time.sleep(1) mc.stop()
def main(): cflib.crtp.init_drivers(enable_debug_driver=False) numRepeticiones = int(input("¿Cuántas veces quieres repetir la ruta?")) ruta = input("¿Qué ruta quieres realizar?") with SyncCrazyflie(URI) as scf: with MotionCommander(scf) as mq90: if ruta == rutaABC: mq90.up(1) mq90.forward(1) ... ... elif ruta == rutaBC: mq90.up(1) mq90.forward(1) ... ... elif ruta == rutaCB: mq90.up(1) mq90.forward(1) ... ... elif ruta == rutaCA: mq90.up(1) mq90.forward(1) ... ... elif ruta == rutaAC: mq90.up(1) mq90.forward(1) ... ... mq90.land()
def move_baduanjin_mc_p1(scf, event2): with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: print("Target Height: {}".format(DEFAULT_HEIGHT)) time.sleep(1) t_init = time.time() ## Go up: d_fly meter/6 sec print("Target Height: {}".format(d_abs)) mc.move_distance( 0, 0, d_fly, velocity=d_fly / 6) # the final posistion will be "d_abs = DEFAULT_HEIGHT + d_fly" t1 = time.time() - t_init print("t1: ", t1) ## Delay 4 sec # mc.stop() time.sleep(4) t2 = time.time() - t_init print("t2: ", t2) ## Go down: d_fly meter/6 sec print("Target Height: {}".format(DEFAULT_HEIGHT)) mc.move_distance(0, 0, -d_fly, velocity=d_fly / 6) t3 = time.time() - t_init print("t3: ", t3) time.sleep(1) ## setting the event for turning off the sound feedback process event2.set()
def move_box_limit(scf): with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: body_x_cmd = 0.2 body_y_cmd = 0.1 max_vel = 0.2 while (1): '''if position_estimate[0] > BOX_LIMIT: mc.start_back() elif position_estimate[0] < -BOX_LIMIT: mc.start_forward() ''' if position_estimate[0] > BOX_LIMIT: body_x_cmd = -max_vel elif position_estimate[0] < -BOX_LIMIT: body_x_cmd = max_vel if position_estimate[1] > BOX_LIMIT: body_y_cmd = -max_vel elif position_estimate[1] < -BOX_LIMIT: body_y_cmd = max_vel mc.start_linear_motion(body_x_cmd, body_y_cmd, 0) time.sleep(0.1)
def push(scf): cf=scf.cf with MotionCommander(scf) as motion_commander: with Multiranger(scf) as multiranger: keep_flying = True while keep_flying: VELOCITY = 0.3 velocity_x = 0.0 velocity_y = 0.0 if is_close(multiranger.front): velocity_x -= VELOCITY if is_close(multiranger.back): velocity_x += VELOCITY if is_close(multiranger.left): velocity_y -= VELOCITY if is_close(multiranger.right): velocity_y += VELOCITY if is_close(multiranger.up): keep_flying = False motion_commander.start_linear_motion( velocity_x, velocity_y, 0) time.sleep(0.1) print('Demo terminated!')
def __init__(self, link_uri): """ Initialize and run the test with the specified link_uri """ self._cf = Crazyflie() self.mc = MotionCommander(self._cf, default_height=1.05) 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) self._cf.open_link(link_uri) print('Connecting to %s' % link_uri) self.is_connected = True
def main(): cflib.crtp.init_drivers(enable_debug_driver=False) with SyncCrazyflie(URI) as scf: with MotionCommander(scf) as mq90: mq90.up(valueToMove) mq90.forward(valueGoForward) mq90.land()
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 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 move_linear_simple(scf): with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: time.sleep(1) mc.forward(0.5) time.sleep(1) mc.turn_left(180) time.sleep(1) mc.forward(0.5) time.sleep(1)
def __init__(self, cfobject): #get initial orientation # We take off when the commander is created with MotionCommander(cfobject) as mc: #update current Yaw angle at 100 Hz threading.Timer(10.0, self._updateYawCurr).start() #use seperate thread for motor operation Thread(target=self._quadMotion(mc)).start()
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)
def __init__(self, mc): self.mc = mc self.calculator = DistanceCalculator() myMotionCommander = MotionCommander(self.mc) self.cap = cv2.VideoCapture(1) CenterLimitH = [305, 335] self.savedX = 50 self.savedY = 0 self.savedAlpha = 180 self.savedBeta = 90
def main(): cflib.crtp.init_drivers(enable_debug_driver=False) with SyncCrazyflie(URI) as scf: with MotionCommander(scf) as mq90: mq90.up(valueToMove) mq90.forward(valueGoForwardBig) mq90.turn_right(totalDegreesTurnRight) mq90.forward(valueGoForwardSmall) mq90.land()
def main(): cflib.crtp.init_drivers(enable_debug_driver=False) with SyncCrazyflie(URI) as scf: with MotionCommander(scf) as mq90: for i in range(5): mq90.forward(0.5) mq90.turn_right(144) mq90.land()
def move(link_uri, forward=2, back=2, velocity=0.05, height=0.2): cflib.crtp.init_drivers() with SyncCrazyflie(link_uri=link_uri, cf=Crazyflie(rw_cache="./cache")) as scf: with MotionCommander(crazyflie=scf, default_height=height) as mc: time.sleep(5) mc.forward(distance_m=forward, velocity=velocity) time.sleep(2) mc.back(distance_m=back, velocity=velocity) time.sleep(5)
def __init__(self, mc): self.mc = mc self.calculator = DistanceCalculator() myMotionCommander = MotionCommander(self.mc) self.cap = cv2.VideoCapture("video_localizacao.webm") CenterLimitH = [305, 335] self.savedX = 50 self.savedY = 0 self.savedAlpha = 180 self.savedBeta = 90 self.map_view = MapView()
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