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 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
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()
thread.start() thread.join() e2 = cv2.getTickCount() t = (e2 - e1) / cv2.getTickFrequency() exeTime.append(t) calculator.writeDistance(frame) cv2.imshow('frame', frame) # print(calculator.distance_x) if (calculator.distance_x == 0): 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
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