class Drone: """Drone instance. Starts and flies drone""" logging.basicConfig(level=logging.ERROR) URI = 'radio://0/80/250K' 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) #self.multiranger = Multiranger(self.scf) def take_off(self, height=1.0, velocity=0.2): """has the drone take off""" # drone takes off self.motionCommander.take_off(height=height, velocity=velocity) def land(self): self.motionCommander.land() def move(self, vector): """moves the drone at a constant speed in one direction""" if vector['y'] > 0.1: print "move up by {}m".format(vector['y']) self.motionCommander.up(vector['y'], velocity=0.4) elif vector['y'] < -0.1: print "move down by {}m".format(abs(vector['y'])) self.motionCommander.down(abs(vector['y']), velocity=0.4) if vector['x'] > 0.1: print "move left by {}m".format(vector['x']) self.motionCommander.left(vector['x'], velocity=0.4) elif vector['x'] < -0.1: print "move right by {}m".format(abs(vector['x'])) self.motionCommander.right(abs(vector['x']), velocity=0.4) if vector['z'] > 2.1: print "move fowards by {}m".format(vector['z'] - 2.0) self.motionCommander.forward(vector['z'] - 2.0, velocity=0.4) self elif vector['z'] < 1.9: print "move backwards by {}m".format(2.0 - vector['z']) self.motionCommander.back(2.0 - vector['z'], velocity=0.4) def disconnect(self): self.scf.close_link() def is_close(range): MIN_DISTANCE = 0.4 # m if range is None: return False else: return range < MIN_DISTANCE
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 MyCrazyFlieClient: 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 land(self): self.client.land() self.scf.close_link() def check_if_in_target(self, goal): pos = self.get_position() distance = np.sqrt( np.power((goal[0] - pos[0]), 2) + np.power((goal[1] - pos[1]), 2)) if distance < 1: self.land() return True return False def get_position(self): self.logger_pos.connect() data = self.logger_pos.next()[1] self.logger_pos.disconnect() self.logger_pos._queue.empty() return [data['kalman.stateX'], -data['kalman.stateY']] def get_orientation(self): self.logger_orientation.connect() data = self.logger_orientation.next()[1] self.logger_orientation.disconnect() self.logger_orientation._queue.empty() return -data['stabilizer.yaw'] def straight(self, speed): self.client.forward(0.25, speed) start = time.time() return start def yaw_right(self): self.client.turn_right(30, 72) start = time.time() return start def yaw_left(self): self.client.turn_left(30, 72) start = time.time() return start def take_action(self, action): start = time.time() collided = False if action == 0: start = self.straight(0.25) if action == 1: start = self.yaw_right() if action == 2: start = self.yaw_left() # print(start) return collided def goal_direction(self, goal, pos): yaw = self.get_orientation() print('yaw now %s' % yaw) pos_angle = math.atan2(goal[1] - pos[1], goal[0] - pos[0]) pos_angle = math.degrees(pos_angle) % 360 print('pos angle %s' % pos_angle) track = pos_angle - yaw track = ((track - 180) % 360) - 180 print('Track = %s ' % track) return track 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
import cflib.crtp from cflib.crazyflie import Crazyflie from cflib.crazyflie.log import LogConfig from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.crazyflie.syncLogger import SyncLogger from cflib.positioning.motion_commander import MotionCommander URI = 'radio://0/30/2M/E7E7E7E7E7' cflib.crtp.init_drivers(enable_debug_driver=False) with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: mc = MotionCommander(scf) mc.take_off(1.0, 0.3) # 利用更底层函数精确指定悬浮高度为 1m scf.cf.commander.send_hover_setpoint(0, 0, 0, 1.0) mc._thread._z_base = 1.0 mc._thread._z_velocity = 0.0 mc._thread._z_base_time = time.time() time.sleep(1) try: while True: time.sleep(1) mc.forward(0.5, 0.3) mc.right(0.5, 0.3) mc.back(0.5, 0.3) mc.left(0.5, 0.3) except: mc.land()
class Mission: def __init__(self, URI): # Tool to process the data from drone's sensors self.processing = Processing(URI) cflib.crtp.init_drivers(enable_debug_driver=False) self.cf = Crazyflie(ro_cache=None, rw_cache='./cache') # Connect callbacks from the Crazyflie API self.cf.connected.add_callback(self.connected) self.cf.disconnected.add_callback(self.disconnected) # Connect to the Crazyflie self.cf.open_link(URI) time.sleep(3) self.pose_home = self.position self.pose_home[2] = 0.1 self.velocity = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'yaw': 0.0} self.goal = np.array([0.5, 0.0, 0.3]) self.mc = MotionCommander(self.cf) time.sleep(3) self.mc.take_off(0.15, 0.2) time.sleep(1) self.mc.circle_right(0.8, velocity=0.5, angle_degrees=360) time.sleep(2) self.mc.up(0.1) time.sleep(1) self.mc.circle_right(0.8, velocity=0.5, angle_degrees=360) time.sleep(2) self.mc.up(0.1) time.sleep(1) self.mc.circle_right(0.8, velocity=0.5, angle_degrees=360) time.sleep(2) self.mc.land(0.2) ''' Mission to a goal and return to home position ''' # self.mc.take_off(0.5, 0.2) # time.sleep(1) # rate = rospy.Rate(10) # while not rospy.is_shutdown(): # self.sendVelocityCommand() # rate.sleep() def coverage_mission(self, height=0.2, length=1.0, width=0.3, numiters=2): self.mc.take_off(height, 0.2) time.sleep(1) for _ in range(numiters): self.mc.forward(length) time.sleep(0.1) self.mc.turn_right(90) time.sleep(0.1) self.mc.forward(width) time.sleep(0.1) self.mc.turn_right(90) time.sleep(0.1) self.mc.forward(length) time.sleep(0.1) self.mc.turn_left(90) time.sleep(0.1) self.mc.forward(width) time.sleep(0.1) self.mc.turn_left(90) time.sleep(0.1) self.mc.land(0.2) time.sleep(1) def square_mission(self, numiters=2): ''' Square trajectory to collect data with multiranger. ''' self.mc.take_off(0.15, 0.2) time.sleep(1) self.mc.turn_left(45) time.sleep(0.1) # sequence of repeated squares for _ in range(numiters): # sqaure for _ in range(4): self.mc.forward(1.4) time.sleep(2) self.mc.turn_right(90) time.sleep(1) self.mc.up(0.1) time.sleep(1) for _ in range(numiters): # sqaure for _ in range(4): self.mc.forward(1.4) time.sleep(2) self.mc.turn_right(90) time.sleep(1) self.mc.land(0.2) time.sleep(1) def sendVelocityCommand(self): direction = normalize(self.goal - self.position) v_x = SPEED_FACTOR * direction[0] v_y = SPEED_FACTOR * direction[1] v_z = SPEED_FACTOR * direction[2] # Local movement correction from obstacles dV = 0.1 # [m/s] if is_close(self.measurement['left']): # print('Obstacle on the LEFT') v_y -= dV if is_close(self.measurement['right']): # print('Obstacle on the RIGHT') v_y += dV self.velocity['x'] = v_x self.velocity['y'] = v_y self.velocity['z'] = v_z # print('Sending velocity:', self.velocity) goal_dist = norm(self.goal - self.position) # print('Distance to goal %.2f [m]:' %goal_dist) if goal_dist < GOAL_TOLERANCE: # goal is reached # print('Goal is reached. Going home...') self.goal = self.pose_home self.cf.commander.send_velocity_world_setpoint(self.velocity['x'], self.velocity['y'], self.velocity['z'], self.velocity['yaw']) def disconnected(self, URI): print('Disconnected') def connected(self, URI): print('We are now connected to {}'.format(URI)) # The definition of the logconfig can be made before connecting lpos = LogConfig(name='Position', period_in_ms=100) lpos.add_variable('lighthouse.x') lpos.add_variable('lighthouse.y') lpos.add_variable('lighthouse.z') lpos.add_variable('stabilizer.roll') lpos.add_variable('stabilizer.pitch') lpos.add_variable('stabilizer.yaw') try: self.cf.log.add_config(lpos) lpos.data_received_cb.add_callback(self.pos_data) lpos.start() except KeyError as e: print('Could not start log configuration,' '{} not found in TOC'.format(str(e))) except AttributeError: print('Could not add Position log config, bad configuration.') lmeas = LogConfig(name='Meas', period_in_ms=100) lmeas.add_variable('range.front') lmeas.add_variable('range.back') lmeas.add_variable('range.up') lmeas.add_variable('range.left') lmeas.add_variable('range.right') lmeas.add_variable('range.zrange') lmeas.add_variable('stabilizer.roll') lmeas.add_variable('stabilizer.pitch') lmeas.add_variable('stabilizer.yaw') try: self.cf.log.add_config(lmeas) lmeas.data_received_cb.add_callback(self.meas_data) lmeas.start() except KeyError as e: print('Could not start log configuration,' '{} not found in TOC'.format(str(e))) except AttributeError: print('Could not add Measurement log config, bad configuration.') lbat = LogConfig( name='Battery', period_in_ms=500) # read battery status with 2 Hz rate lbat.add_variable('pm.vbat', 'float') try: self.cf.log.add_config(lbat) lbat.data_received_cb.add_callback(self.battery_data) lbat.start() except KeyError as e: print('Could not start log configuration,' '{} not found in TOC'.format(str(e))) except AttributeError: print('Could not add Measurement log config, bad configuration.') def pos_data(self, timestamp, data, logconf): # Transformation according to https://wiki.bitcraze.io/doc:lighthouse:setup position = [ -data['lighthouse.z'], -data['lighthouse.x'], data['lighthouse.y'] # data['stateEstimate.x'], # data['stateEstimate.y'], # data['stateEstimate.z'] ] orientation = [ data['stabilizer.roll'], data['stabilizer.pitch'], data['stabilizer.yaw'] ] self.position = np.array(position) self.orientation = np.array(orientation) self.processing.set_position(position, orientation) def meas_data(self, timestamp, data, logconf): measurement = { 'roll': data['stabilizer.roll'], 'pitch': data['stabilizer.pitch'], 'yaw': data['stabilizer.yaw'], 'front': data['range.front'], 'back': data['range.back'], 'up': data['range.up'], 'down': data['range.zrange'], 'left': data['range.left'], 'right': data['range.right'] } self.measurement = measurement self.processing.set_measurement(measurement) def battery_data(self, timestamp, data, logconf): self.V_bat = data['pm.vbat'] # print('Battery status: %.2f [V]' %self.V_bat) if self.V_bat <= V_BATTERY_TO_GO_HOME: self.battery_state = 'needs_charging' # print('Battery is not charged: %.2f' %self.V_bat) self.goal = self.pose_home elif self.V_bat >= V_BATTERY_CHARGED: self.battery_state = 'fully_charged'