def test_that_logging_is_started_on_start(self): # Fixture with patch('cflib.utils.multiranger.LogConfig', return_value=self.log_config_mock): sut = Multiranger(self.cf_mock) # Test sut.start() # Assert self.log_config_mock.start.assert_called_once_with()
def test_that_logging_is_started_on_start(self): # Fixture with patch('cflib.utils.multiranger.LogConfig', return_value=self.log_config_mock): sut = Multiranger(self.cf_mock) # Test sut.start() # Assert self.log_config_mock.start.assert_called_once_with()
def test_that_log_configuration_is_added_on_connect(self): # Fixture with patch('cflib.utils.multiranger.LogConfig', return_value=self.log_config_mock): sut = Multiranger(self.cf_mock) # Test sut.start() # Assert self.log_mock.add_config.assert_called_once_with( self.log_config_mock)
def test_that_log_configuration_is_added_on_connect(self): # Fixture with patch('cflib.utils.multiranger.LogConfig', return_value=self.log_config_mock): sut = Multiranger(self.cf_mock) # Test sut.start() # Assert self.log_mock.add_config.assert_called_once_with( self.log_config_mock)
def test_that_data_callback_is_set(self): # Fixture with patch('cflib.utils.multiranger.LogConfig', return_value=self.log_config_mock): sut = Multiranger(self.cf_mock) # Test sut.start() # Assert self.log_config_mock.start.assert_called_once_with() self.assertEqual(1, len( self.log_config_mock.data_received_cb.callbacks))
def test_that_data_callback_is_set(self): # Fixture with patch('cflib.utils.multiranger.LogConfig', return_value=self.log_config_mock): sut = Multiranger(self.cf_mock) # Test sut.start() # Assert self.log_config_mock.start.assert_called_once_with() self.assertEqual(1, len( self.log_config_mock.data_received_cb.callbacks))
def test_that_log_configuration_is_correct(self): # Fixture with patch('cflib.utils.multiranger.LogConfig', return_value=self.log_config_mock): sut = Multiranger(self.cf_mock) # Test sut.start() # Assert self.log_config_mock.add_variable.assert_has_calls([ call(self.FRONT), call(self.BACK), call(self.LEFT), call(self.RIGHT), call(self.UP), ])
def test_that_log_configuration_is_correct(self): # Fixture with patch('cflib.utils.multiranger.LogConfig', return_value=self.log_config_mock): sut = Multiranger(self.cf_mock) # Test sut.start() # Assert self.log_config_mock.add_variable.assert_has_calls([ call(self.FRONT), call(self.BACK), call(self.LEFT), call(self.RIGHT), call(self.UP), ])
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)