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 simple_sequence(): with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: with PositionHlCommander(scf) as pc: with Multiranger(scf) as multiranger: for i in xrange(4): pc.forward(1.0) pc.left(1.0) pc.back(1.0) pc.right(1.0)
def test_that_using_context_manager_calls_start_and_stop(self): # Fixture with patch('cflib.utils.multiranger.LogConfig', return_value=self.log_config_mock): with Multiranger(self.cf_mock): pass # Assert self.log_config_mock.start.assert_called_once_with() self.log_config_mock.delete.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_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 _validate_distance_from_getter(self, expected, getter_name, zranger=False): # Fixture with patch('cflib.utils.multiranger.LogConfig', return_value=self.log_config_mock): sut = Multiranger(self.cf_mock, zranger=zranger) timestmp = 1234 logconf = None self.log_config_mock.data_received_cb.call(timestmp, self.log_data, logconf) # Test actual = getattr(sut, getter_name) # Assert self.assertEqual(expected, actual)
def talker(): pub = rospy.Publisher('chatter', String, queue_size=10) rospy.init_node('talker', anonymous=True) rate = rospy.Rate(10) # 10hz cflib.crtp.init_drivers(enable_debug_driver=False) cf = Crazyflie(rw_cache='./cache') while not rospy.is_shutdown(): with SyncCrazyflie(URI, cf=cf) as scf: with MotionCommander(scf) as motion_commander: with Multiranger(scf) as multiranger: #reset_estimator(scf) start_position_printing(scf) keep_flying = True while keep_flying: print("tadaaaaa-----", x) VELOCITY = 0.5 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 hello_str = "hello world %s" % multiranger.front rospy.loginfo(hello_str) #pub.publish(hello_str) motion_commander.start_linear_motion( velocity_x, velocity_y, 0) #print(data['kalman.stateX']) time.sleep(0.1) print('Demo terminated!') rate.sleep()
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
def simple_log(scf, logconf, x, y, z, cf): keep_flying = True #cf = Crazyflie(rw_cache='./cache') with SyncCrazyflie(URI, cf=cf) as scf: with SyncLogger(scf, lg_stab) as logger: with MotionCommander(scf) as motion_commander: with Multiranger(scf) as multi_ranger: for i in range(100): #while keep_flying: #if is_close(multi_ranger.up): #keep_flying = False cf.param.set_value('posCtlPid.zKp', '1') #cf.param.set_value('posCtlPid.xKp', '1') #cf.param.set_value('posCtlPid.yKp', '1') #cf.param.set_value('postCtlPid.xKp','1') for log_entry in logger: #cf.param.set_value('posCtlPid.xKp', '1') #cf.param.set_value('posCtlPid.yKp', '1') cf.param.set_value('posCtlPid.zKp', '1') motion_commander.start_linear_motion(0, 0, 0.05) timestamp = log_entry[0] data = log_entry[1] logconf_name = log_entry[2] count = 1 for i in data.values(): if count == 1: x.append(i) elif count == 2: y.append(i) else: z.append(i) count = count + 1 print('[%d][%s]: %s' % (timestamp, logconf_name, data)) break time.sleep(0.1) motion_commander.stop() return (x, y, z)
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) # The definition of the logconfig can be made before connecting cflib.crtp.init_drivers(enable_debug_driver=True) cf = Crazyflie(rw_cache='./cache') with SyncCrazyflie(URI, cf=cf) as scf: with MotionCommander(scf) as motion_commander: with Multiranger(scf) as multi_ranger: self._lg_stab = LogConfig(name='Stabilizer', period_in_ms=10) self._lg_stab.add_variable('stabilizer.roll', 'float') self._lg_stab.add_variable('stabilizer.pitch', 'float') self._lg_stab.add_variable('stabilizer.yaw', 'float') # Adding the configuration cannot be done until a Crazyflie is # connected, since we need to check that the variables we # would like to log are in the TOC. try: self._cf.log.add_config(self._lg_stab) # This callback will receive the data self._lg_stab.data_received_cb.add_callback(self._stab_log_data) # This callback will be called on errors self._lg_stab.error_cb.add_callback(self._stab_log_error) # Start the logging self._lg_stab.start() except KeyError as e: print('Could not start log configuration,' '{} not found in TOC'.format(str(e))) except AttributeError: print('Could not add Stabilizer log config, bad configuration.') # Start a timer to disconnect in 10s t = Timer(5, self._cf.close_link) t.start()
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()
MIN_DISTANCE = 0.2 # m if range is None: return False else: return range < MIN_DISTANCE if __name__ == '__main__': # Initialize the low-level drivers (don't list the debug drivers) cflib.crtp.init_drivers(enable_debug_driver=False) cf = Crazyflie(rw_cache='./cache') with SyncCrazyflie(URI, cf=cf) as scf: with MotionCommander(scf) as motion_commander: with Multiranger(scf) as multiranger: keep_flying = True while keep_flying: VELOCITY = 0.5 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):