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 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 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 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 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")
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