class SwarmController(object): """Main class """ def __init__(self): rospy.loginfo("Swarm: Initialization...") self._crazyflies = {} #: Dict of :py:class:`CrazyfliePy` self._cf_list = rospy.get_param( "cf_list") #: List of str: List of all cf names in the swarm self._rate = rospy.Rate(10) #: Rate: Rate to publish messages # Initialize each Crazyflie for each_cf in self._cf_list: self._crazyflies[each_cf] = CrazyfliePy(each_cf) self._init_params() self._stabilize_position() # Init services self.formation_services = {} self.formation_services = { "set_formation": None, "get_list": None, "inc_scale": None, "dec_scale": None } self.traj_services = {} self.traj_services = { "set_planner_positions": None, "plan_trajectories": None, "pub_trajectories": None, } self.start_rec_service = None self._init_services() # Subscriber self._formation_goal_vel_pub = rospy.Publisher('/formation_goal_vel', Twist, queue_size=1) self._formation_goal_vel = Twist() self._joy_swarm_vel = Twist( ) #: Twist: Velocity received from joystick rospy.Subscriber("/joy_swarm_vel", Twist, self._joy_swarm_vel_handler) # Initialize formation self.formation = "line" self._formation_list = self.formation_services["get_list"]( ).formations.split(',') self._extra_cf_list = [] #: list of str: ID of extra CF self._landed_cf_ids = [] #: list of str: Swarm Id of landed CFs # Initialize state machine _state_list = { "landed": self._landed_state, "follow_traj": self._follow_traj_state, "in_formation": self._in_formation_state, "hover": self._hover_state, "landing": self._landing_state, } self._state_machine = StateMachine(_state_list) self._state_machine.set_state("landed") # Other self.ctrl_mode = "automatic" self._traj_found = False self._traj_successfull = False self._after_traj_state = "" # State to go once the trajectory is completed self.start_rec_service() rospy.loginfo("Swarm: Ready to start") def _init_services(self): # Services rospy.Service('/swarm_emergency', Empty, self._swarm_emergency_srv) rospy.Service('/stop_swarm', Empty, self._stop_swarm_srv) rospy.Service('/take_off_swarm', Empty, self._take_off_swarm_srv) rospy.Service('/land_swarm', Empty, self._land_swarm_srv) rospy.Service('/set_mode', SetMode, self._set_mode_srv) rospy.Service('/go_to', SetGoals, self._go_to_srv) rospy.Service('/get_positions', GetPositions, self._get_positions_srv) rospy.Service('/set_swarm_formation', SetFormation, self._set_formation_srv) rospy.Service('/next_swarm_formation', Empty, self._next_swarm_formation_srv) rospy.Service('/prev_swarm_formation', Empty, self._prev_swarm_formation_srv) rospy.Service('/inc_swarm_scale', Empty, self._inc_swarm_scale_srv) rospy.Service('/dec_swarm_scale', Empty, self._dec_swarm_scale_srv) rospy.Service('/traj_found', SetBool, self._traj_found_srv) rospy.Service('/traj_done', Empty, self._traj_done_srv) # Trajectory done # Subscribe to services # Formation manager rospy.loginfo("Swarm: waiting for formation services") rospy.wait_for_service("/set_formation") rospy.wait_for_service("/get_formations_list") rospy.wait_for_service("/formation_inc_scale") rospy.wait_for_service("/formation_dec_scale") self.formation_services["set_formation"] = rospy.ServiceProxy( "/set_formation", SetFormation) self.formation_services["get_list"] = rospy.ServiceProxy( "/get_formations_list", GetFormationList) self.formation_services["inc_scale"] = rospy.ServiceProxy( "/formation_inc_scale", Empty) self.formation_services["dec_scale"] = rospy.ServiceProxy( "/formation_dec_scale", Empty) rospy.loginfo("Swarm: formation services found") # Trajectory planner rospy.loginfo("Swarm: waiting for trajectory planner services") self.traj_services["set_planner_positions"] = rospy.ServiceProxy( "/set_planner_positions", SetPositions) self.traj_services["plan_trajectories"] = rospy.ServiceProxy( "/plan_trajectories", Empty) self.traj_services["pub_trajectories"] = rospy.ServiceProxy( "/pub_trajectories", Empty) rospy.loginfo("Swarm: trajectory planner services found") # Flight recorder rospy.loginfo("Swarm: waiting for recorder services") rospy.wait_for_service("/start_recorder") self.start_rec_service = rospy.ServiceProxy("/start_recorder", Empty) rospy.loginfo("Swarm: recorder services found") def _init_params(self): self.update_swarm_param("commander/enHighLevel", 1) self.update_swarm_param("stabilizer/estimator", 2) # Use EKF self.update_swarm_param("stabilizer/controller", 1) # 1: High lvl, 2: Mellinger self.update_swarm_param("kalman/resetEstimation", 1) def _stabilize_position(self): """To wait until position of all CFs is stable. A CF position is considered stable when the variance of it's position in x, y and z is less than a threshold. The variance is calculated over the past 10 sec (10 data) """ rospy.loginfo("Swarm: Waiting for position to stabilize...") # rospy.sleep(1.0) threshold = 0.01 n_data = 50 positions = {} var_list = {} position_stable = False queue_full = False # Initialize position history for cf_id in self._crazyflies: queue_dict = {} queue_dict['x'] = Queue.Queue(n_data) queue_dict['y'] = Queue.Queue(n_data) queue_dict['z'] = Queue.Queue(n_data) positions[cf_id] = queue_dict while not position_stable: for cf_id, cf_info in self._crazyflies.items(): cf_pose = cf_info.pose.pose if positions[cf_id]['x'].full(): queue_full = True positions[cf_id]['x'].get() positions[cf_id]['y'].get() positions[cf_id]['z'].get() positions[cf_id]['x'].put(cf_pose.position.x) positions[cf_id]['y'].put(cf_pose.position.y) positions[cf_id]['z'].put(cf_pose.position.z) if queue_full: all_vars = [ np.var(positions[cf_id]['x'].queue), np.var(positions[cf_id]['y'].queue), np.var(positions[cf_id]['z'].queue) ] var_list[cf_id] = max(all_vars) if queue_full: max_var = max([v for _, v in var_list.items()]) if max_var < threshold: position_stable = True self._rate.sleep() rospy.loginfo("Swarm: All CF position stable") # Methods def control_swarm(self): """Main method, publish on topics depending of current state """ # Execute function depending on current state state_function = self._state_machine.run_state() state_function() if len(self._cf_list) > 1: self.check_collisions() # Publish goal of each CF self._pub_cf_goals() self._rate.sleep() def update_swarm_param(self, param, value): """Update parameter of all swarm Args: param (str): Name of parameter value (int64): Parameter value """ rospy.loginfo("Swarm: Set %s param to %s" % (param, str(value))) self._call_all_cf_service("set_param", kwargs={ 'param': param, 'value': value }) def check_collisions(self): """To check that the minimal distance between each CF is okay. If actual distance between CF is too small, calls emergency service. """ position_dist = [] goal_dist = [] scaling_matrix_inv = inv(np.diag([1, 1, 2])) for cf_id, each_cf in self._crazyflies.items(): cf_position = each_cf.pose.pose cf_position = np.array([ cf_position.position.x, cf_position.position.y, cf_position.position.z ]) cf_goal = each_cf.goals["goal"] cf_goal = np.array([cf_goal.x, cf_goal.y, cf_goal.z]) for other_id, other_cf in self._crazyflies.items(): if other_id != cf_id: other_pos = other_cf.pose.pose other_pos = np.array([ other_pos.position.x, other_pos.position.y, other_pos.position.z ]) other_goal = other_cf.goals["goal"] other_goal = np.array( [other_goal.x, other_goal.y, other_goal.z]) p_dist = norm( dot(scaling_matrix_inv, cf_position - other_pos)) g_dist = norm(dot(scaling_matrix_inv, cf_goal - other_goal)) position_dist.append(p_dist) goal_dist.append(g_dist) min_distances = [min(goal_dist), min(position_dist)] if min_distances[0] < MIN_GOAL_DIST: rospy.logwarn("Goals are too close") if min_distances[1] < MIN_CF_DIST: rospy.logerr("CF too close, emergency") self._swarm_emergency_srv(Empty()) def update_formation(self, formation_goal=None): """To change formation of the swarm. Formation center will be `formation_goal` if is specified. If not, it will be stay at the same place Args: formation_goal (list, optional): New formation goal: [x, y, z, yaw]. Defaults to None. """ # Change state to make sure CF don't 'teleport' to new formation self._state_machine.set_state("hover") rospy.sleep(0.1) # Set new formation self._send_formation(formation_goal) self.go_to_goal() def go_to_goal(self, target_goals=None, land_swarm=False): """Controls swarm to move each CF to it's desired goal while avoiding collisions. If some CFs are in extra (in formation) will land them. Handles extra CF landing. Args: land_swarm (bool, optional): If true, land swarm to initial pos. Defaults to False. goals (dict, optional): To specify target goals. Used when not in formation mode. Defaults to None. """ rospy.loginfo("Swarm: Going to new goals") self._traj_found = False self._landed_cf_ids = [] rospy.sleep(0.1) # Update goal and send positions to planner self._update_cf_goal(land_swarm) self._send_start_positions() self._send_goals(land_swarm=land_swarm, target_goals=target_goals) # Start solver self.traj_services["plan_trajectories"]() # Take off landed CF that are not in extra take_off_list = [ cf for cf in self._landed_cf_ids if cf not in self._extra_cf_list ] self._call_all_cf_service("take_off", cf_list=take_off_list) self._wait_for_take_off() self._wait_for_planner() # Follow traj if self._traj_successfull: self.traj_services["pub_trajectories"]() self._state_machine.set_state("follow_traj") # If traj. planner fails else: self._swarm_emergency_srv(None) def _send_formation(self, formation_goal=None): """Send desired formation to formation manager. `formation_manager` package will compute the new `formation_goal` of each CF and return all CFs in extra. Args: formation_goal (list, optional): New formation goal: [x, y, z, yaw]. Defaults to None. """ start_positions = {} for cf_id, cf_vals in self._crazyflies.items(): cf_pose = cf_vals.pose.pose start_positions[cf_id] = [ cf_pose.position.x, cf_pose.position.y, cf_pose.position.z ] srv_res = self.formation_services["set_formation"]( formation=self.formation, positions=str(start_positions), goal=str(formation_goal)) self._extra_cf_list = srv_res.extra_cf.split(',') def _update_cf_goal(self, land_swarm=False): """Update goal and initial position of landed CFs Also find Id of landed CFs Notes: - If land_swarm: Set all goals 0.5m above initial pose - If landed and not in extra: Set goal 0.5m above initial pose and add to landed list - If landed and in extra: Set goal to initial pose and add to landed list Args: land_swarm (bool): When True, land all CFs in swarm """ self._landed_cf_ids = [] for cf_id, cf_vals in self._crazyflies.items(): if land_swarm: cf_initial_pose = cf_vals.initial_pose cf_vals.goals['goal'].x = cf_initial_pose.position.x cf_vals.goals['goal'].y = cf_initial_pose.position.y cf_vals.goals[ 'goal'].z = cf_initial_pose.position.z + TAKE_OFF_HEIGHT # If CF is landed and not in extra elif cf_vals.state in ["stop", "landed", "land" ] and cf_id not in self._extra_cf_list: self._landed_cf_ids.append(cf_id) cf_pose = cf_vals.pose.pose if cf_vals.initial_pose is None: cf_vals.update_initial_pose() cf_vals.goals["goal"].x = cf_pose.position.x cf_vals.goals["goal"].y = cf_pose.position.y cf_vals.goals["goal"].z = GND_HEIGHT + TAKE_OFF_HEIGHT # If CF is landed and in extra elif cf_vals.state in ["stop", "landed", "land" ] and cf_id in self._extra_cf_list: self._landed_cf_ids.append(cf_id) cf_pose = cf_vals.pose.pose if cf_vals.initial_pose is None: cf_vals.update_initial_pose() cf_vals.goals["goal"].x = cf_pose.position.x cf_vals.goals["goal"].y = cf_pose.position.y cf_vals.goals["goal"].z = cf_pose.position.z def _send_start_positions(self): """Send start positions to trajectory planner Notes: - Doesn't send positon of extra CF if it's landed """ # Send each CF starting position to planner start_positions = {} for cf_id, cf_vals in self._crazyflies.items(): cf_pose = cf_vals.pose.pose # If landed, starting position is 0.5m above if cf_id in self._landed_cf_ids and cf_id not in self._extra_cf_list: start_positions[cf_id] = [ cf_pose.position.x, cf_pose.position.y, cf_pose.position.z + TAKE_OFF_HEIGHT, yaw_from_quat(cf_pose.orientation) ] else: start_positions[cf_id] = [ cf_pose.position.x, cf_pose.position.y, cf_pose.position.z, yaw_from_quat(cf_pose.orientation) ] self.traj_services["set_planner_positions"]( position_type="start_position", positions=str(start_positions)) def _send_goals(self, land_swarm=False, target_goals=None): """Send goal of each CF to trajectory planner Notes: - If CF is in extra, goal is above initial position - If CF is in extra and landed, no goal is sent Args: land_swarm (bool, optional): If True, land all CF in the swarm. Defaults to False. target_goals (dict, optional): To specify each_cf goal. Defaults to None. """ goals = {} for cf_id, cf_vals in self._crazyflies.items(): # Land all CFs in air if land_swarm and cf_id not in self._landed_cf_ids: cf_initial_pose = cf_vals.initial_pose goals[cf_id] = [ cf_initial_pose.position.x, cf_initial_pose.position.y, GND_HEIGHT + TAKE_OFF_HEIGHT, yaw_from_quat(cf_initial_pose.orientation) ] # Goal of CF in formation elif cf_id not in self._extra_cf_list: # If CF in formation target_pose = None if target_goals is None or cf_id not in target_goals.keys(): target_pose = cf_vals.goals["formation"] else: target_pose = target_goals[cf_id] goals[cf_id] = [ target_pose.x, target_pose.y, target_pose.z, target_pose.yaw ] # If CF in extra and not landed, go to initial position elif cf_id not in self._landed_cf_ids: cf_initial_pose = cf_vals.initial_pose goals[cf_id] = [ cf_initial_pose.position.x, cf_initial_pose.position.y, GND_HEIGHT + TAKE_OFF_HEIGHT, yaw_from_quat(cf_initial_pose.orientation), ] self.traj_services["set_planner_positions"](position_type="goal", positions=str(goals)) def _wait_for_take_off(self): """Wait that all CF of formation are in hover state """ all_cf_in_air = False #: bool: True if all CFs are done taking off while not all_cf_in_air: all_cf_in_air = True if rospy.is_shutdown(): break for cf_id, each_cf in self._crazyflies.items(): if each_cf.state != "hover" and cf_id not in self._extra_cf_list: all_cf_in_air = False self._rate.sleep() def _wait_for_planner(self): """Wait until planner finds a trajectory """ rospy.sleep(0.1) # Make sure first traj messages are received while not self._traj_found: if rospy.is_shutdown(): break for _, each_cf in self._crazyflies.items(): each_cf.update_goal() self._rate.sleep() self._traj_found = False def _update_landing_cf_goal(self, land_swarm=False): """Set goal of CF to land to initial position Args: land_swarm (bool, optional): True if all swarm is to be landed. Default: False """ for cf_id, cf_vals in self._crazyflies.items(): # If land all swarm or (cf in extra and not landed) if land_swarm or\ (cf_vals.state not in ["landed", "land", "stop"] and\ cf_id in self._extra_cf_list): cf_initial_pose = cf_vals.initial_pose cf_vals.goals["goal"].x = cf_initial_pose.position.x cf_vals.goals["goal"].y = cf_initial_pose.position.y cf_vals.goals["goal"].z = GND_HEIGHT # cf_vals["goal_msg"].yaw = yaw_from_quat(cf_initial_pose.orientation) # States def _landed_state(self): """All CF are on the ground """ pass def _follow_traj_state(self): """All cf follow a specified trajectory """ # Set CF goal to trajectory goal for cf_id, each_cf in self._crazyflies.items(): traj_goal = each_cf.goals["trajectory"] # Make sure first msg was sent if traj_goal is not None: #If CF in extra and landed, don't follow goal if each_cf.state in ["landed", "land", "stop" ] and cf_id in self._extra_cf_list: each_cf.update_goal() else: each_cf.update_goal("trajectory") def _in_formation_state(self): """Swarm is in a specific formation. Lands extra CFs Formation can be moved /w the joystick """ # Publish goal velocity self._formation_goal_vel = self._joy_swarm_vel self._pub_formation_goal_vel(self._formation_goal_vel) # Set CF goal to formation goal for cf_id, each_cf in self._crazyflies.items(): # If CF part of formation, set goal to formation_goal if cf_id not in self._extra_cf_list: each_cf.update_goal("formation") # IF CF is not landed elif each_cf.state not in ["landed", "land", "stop"]: self._update_landing_cf_goal() self._call_all_cf_service("land", cf_list=self._extra_cf_list) # If CF is already landed else: each_cf.update_goal() def _hover_state(self): """CFs hover in place """ # Set CF goal to current pose for _, each_cf in self._crazyflies.items(): each_cf.update_goal() def _landing_state(self): """Land CF to their starting position """ rospy.loginfo("Swarm: land") self._update_landing_cf_goal(True) self._call_all_cf_service("land") self._state_machine.set_state("landed") # Publisher and subscription def _joy_swarm_vel_handler(self, joy_swarm_vel): """Update swarm goal velocity Args: swarm_goal_vel (Twist): Swarm goal velocity """ self._joy_swarm_vel = joy_swarm_vel def _pub_formation_goal_vel(self, formation_goal_vel): """Publish swarm_goal speed Args: goal_spd (Twist): Speed variation of goal """ self._formation_goal_vel_pub.publish(formation_goal_vel) def _pub_cf_goals(self): """Publish goal of each CF """ for _, each_cf in self._crazyflies.items(): each_cf.publish_goal() # Services def _call_all_cf_service(self, service_name, args=None, kwargs=None, cf_list=None): """Call a service for all the CF in the swarm. If a list /w CF name is specified, will only call the srv for CF in the list. Args: service_name (str): Name of the service to call service_msg (srv_msg, optional): Message to send. Defaults to None. cf_list (list of str, optional): Only call service of CF in list. Defaults to None. """ for cf_id, each_cf in self._crazyflies.items(): if cf_list is None or cf_id in cf_list: each_cf.call_srv(service_name, args, kwargs) return {} def _swarm_emergency_srv(self, _): """Call emergency service """ rospy.logerr("Swarm: EMERGENCY") self._call_all_cf_service("emergency") return {} def _stop_swarm_srv(self, _): """Call stop service """ rospy.loginfo("Swarm: stop") self._call_all_cf_service("stop") return {} def _take_off_swarm_srv(self, _): """Take off all cf in swarm """ self._update_cf_goal() self._call_all_cf_service("take_off", cf_list=self._landed_cf_ids) self._wait_for_take_off() self._state_machine.set_state("hover") if self.ctrl_mode == "formation": self._after_traj_state = "in_formation" self.update_formation() return {} def _land_swarm_srv(self, _): """Land all cf in swarm """ # Bring swarm to initial pose self.go_to_goal(land_swarm=True) # Land self._after_traj_state = "landing" return {} def _set_mode_srv(self, mode_req): new_mode = mode_req.new_mode.lower() avalaible_mode = False if new_mode in ["formation", "automatic"]: self.ctrl_mode = new_mode avalaible_mode = True return {'success': avalaible_mode} def _go_to_srv(self, srv_req): goals = ast.literal_eval(srv_req.goals) target_goals = {} formation_goal = None # Make sure swarm is in hover or formation state if self._state_machine.in_state( "in_formation") or self._state_machine.in_state("hover"): for goal_id, goal_val in goals.items(): if goal_id == "formation": # print "Formation goal: {}".format(goal_val) formation_goal = goal_val elif goal_id in self._cf_list: # print "{} goal: {}".format(goal_id, goal_val) new_goal = Position() new_goal.x = goal_val[0] new_goal.y = goal_val[1] new_goal.z = goal_val[2] new_goal.yaw = goal_val[3] target_goals[goal_id] = new_goal else: rospy.logerr("%s: Invalid goal name" % goal_id) if self.ctrl_mode == "automatic": self._after_traj_state = "hover" self.go_to_goal(target_goals=target_goals) elif self.ctrl_mode == "formation": self._after_traj_state = "in_formation" self.update_formation(formation_goal=formation_goal) else: rospy.logerr("Swarm not ready to move") return {} def _get_positions_srv(self, srv_req): cf_list = ast.literal_eval(srv_req.cf_list) # If not list is passed, send position of all CFs if not cf_list: cf_list = self._cf_list positions = {} for each_cf in cf_list: if each_cf in self._cf_list: pose = self._crazyflies[each_cf].pose.pose positions[each_cf] = [ pose.position.x, pose.position.y, pose.position.z, yaw_from_quat(pose.orientation) ] else: rospy.logerr("%s: Invalid crazyflie name" % each_cf) return {"positions": str(positions)} def _follow_traj_srv(self, _): """Follow a trajectory """ self._state_machine.set_state("follow_traj") return {} def _set_formation_srv(self, srv_req): new_formation = srv_req.formation if self._state_machine.in_state("in_formation"): if new_formation not in self._formation_list: rospy.logerr("%s: Invalid formation" % new_formation) else: self.formation = new_formation self.update_formation() else: self.formation = new_formation return {} def _next_swarm_formation_srv(self, _): """Change swarm formation to next the next one """ if self._state_machine.in_state("in_formation"): idx = self._formation_list.index(self.formation) next_idx = idx + 1 if idx == (len(self._formation_list) - 1): next_idx = 0 self.formation = self._formation_list[next_idx] self.update_formation() return {} def _prev_swarm_formation_srv(self, _): """Change swarm formation to the previous one """ if self._state_machine.in_state("in_formation"): idx = self._formation_list.index(self.formation) prev_idx = idx - 1 if prev_idx < 0: prev_idx = len(self._formation_list) - 1 self.formation = self._formation_list[prev_idx] self.update_formation() return {} def _inc_swarm_scale_srv(self, _): """Increase formation scale """ if self._state_machine.in_state("in_formation"): # Change state to make sure CF don't 'teleport' to new formation self._state_machine.set_state("hover") rospy.sleep(0.1) self.formation_services["inc_scale"]() self.go_to_goal() return {} def _dec_swarm_scale_srv(self, _): """Decrease formation scale """ if self._state_machine.in_state("in_formation"): # Change state to make sure CF don't 'teleport' to new formation self._state_machine.set_state("hover") rospy.sleep(0.1) self.formation_services["dec_scale"]() self.go_to_goal() return {} def _traj_found_srv(self, srv_req): """Called when trajectory solver is done. Args: srv_req.data (bool): True if a non colliding trajectory is found """ self._traj_found = True self._traj_successfull = srv_req.data if not self._traj_successfull: rospy.logerr("Swarm: No trajectory found") return {'success': True, "message": ""} def _traj_done_srv(self, _): """Called when trajectory has all been executed. Goes to formation? """ self._state_machine.set_state(self._after_traj_state) return {}
class Crazyflie(object): """Controller of a single crazyflie. In charge of executing services and following positions Args: object ([type]): [description] """ def __init__(self, cf_id, sim): """ Args: cf_id (str): Name of the CF sim (bool): To sim Attributes: cf_id (str): Name of the CF _sim (bool): To sim _states (list of str): All possible states of the CF _state (str): Current state of the CF """ # Attributes self.cf_id = '/' + cf_id self._sim = sim # Initialize state machine self._state_list = { "landed": self._stop, "take_off": self._take_off, "hover": self._hover, "stop": self._stop, "land": self._land, "teleop": None } self._state_machine = StateMachine(self._state_list) self._state_machine.set_state("landed") rospy.loginfo("%s: Initializing" % self.cf_id) self.world_frame = rospy.get_param("~worldFrame", "/world") self.rate = rospy.Rate(10) # If not in simulation, find services and set parameters if not self._sim: rospy.loginfo(self.cf_id + ": waiting for update_params service...") rospy.wait_for_service(self.cf_id + '/update_params') rospy.loginfo(self.cf_id + ": found update_params service") self.update_param = rospy.ServiceProxy( self.cf_id + '/update_params', UpdateParams) # Declare services self._init_services() # Declare publishers self._init_publishers() # Declare subscriptions self.pose = Pose() self.goal = Position() self.initial_pose = Pose() rospy.Subscriber(self.cf_id + '/pose', PoseStamped, self._pose_handler) rospy.Subscriber(self.cf_id + '/goal', Position, self._goal_handler) # Find initial position self.localization_started = False #: boool: Sets to True upon receiveing first pose self.find_initial_pose() rospy.loginfo("%s: Setup done" % self.cf_id) def _init_publishers(self): """Initialize all publishers""" self.cmd_vel_pub = rospy.Publisher(self.cf_id + '/cmd_vel', Twist, queue_size=1) self.cmd_vel_msg = Twist() self.cmd_hovering_pub = rospy.Publisher(self.cf_id + "/cmd_hovering", Hover, queue_size=1) self.cmd_hovering_msg = Hover() self.cmd_hovering_msg.header.seq = 0 self.cmd_hovering_msg.header.stamp = rospy.Time.now() self.cmd_hovering_msg.header.frame_id = self.world_frame self.cmd_hovering_msg.yawrate = 0 self.cmd_hovering_msg.vx = 0 self.cmd_hovering_msg.vy = 0 self.cmd_pos_pub = rospy.Publisher(self.cf_id + "/cmd_position", Position, queue_size=1) self.cmd_pos_msg = Position() self.cmd_pos_msg.header.seq = 1 self.cmd_pos_msg.header.stamp = rospy.Time.now() self.cmd_pos_msg.header.frame_id = self.world_frame self.cmd_pos_msg.yaw = 0 self.cmd_stop_pub = rospy.Publisher(self.cf_id + "/cmd_stop", Empty_msg, queue_size=1) self.cmd_stop_msg = Empty_msg() self.current_state_pub = rospy.Publisher(self.cf_id + "/state", String, queue_size=1) def _init_services(self): rospy.Service(self.cf_id + '/take_off', Empty_srv, self.take_off) rospy.Service(self.cf_id + '/hover', Empty_srv, self.hover) rospy.Service(self.cf_id + '/land', Empty_srv, self.land) rospy.Service(self.cf_id + '/stop', Empty_srv, self.stop) rospy.Service(self.cf_id + '/set_param', SetParam, self._set_param) # Handlers def _pose_handler(self, pose_stamped): """Update crazyflie position in world """ self.localization_started = True self.pose = pose_stamped.pose def _goal_handler(self, goal): self.goal = goal def find_initial_pose(self): """ Find the initial position of the crazyflie. Position found by calculating the mean during a time interval """ rate = rospy.Rate(100) while not self.localization_started: if rospy.is_shutdown(): break initial_pose = {'x': [], 'y': [], 'z': [], 'yaw': []} while len(initial_pose['x']) < 10: if rospy.is_shutdown(): break initial_pose['x'].append(self.pose.position.x) initial_pose['y'].append(self.pose.position.y) initial_pose['z'].append(self.pose.position.z) initial_pose['yaw'].append(yaw_from_quat(self.pose.orientation)) rate.sleep() self.initial_pose.position.x = np.mean(initial_pose['x']) self.initial_pose.position.y = np.mean(initial_pose['y']) self.initial_pose.position.z = np.mean(initial_pose['z']) self.initial_pose.orientation = quat_from_yaw( np.mean(initial_pose['yaw'])) rospy.loginfo("%s: Initial position found" % self.cf_id) # rospy.loginfo(self.initial_pose) # Setter & Getters def _set_param(self, param_req): """Changes the value of the given parameter. Args: name (str): The parameter's name. value (Any): The parameter's value. """ param_name = param_req.param param_val = param_req.value rospy.set_param(self.cf_id + "/" + param_name, param_val) if not self._sim: self.update_param([param_name]) return {} def get_cf_id(self): """Get crazyflie id Returns: int: cf_id """ return self.cf_id # Services def take_off(self, _): """Take off service """ # rospy.loginfo("%s: Take off" % self.cf_id) self._state_machine.set_state("take_off") return {} def hover(self, _): """Hover service """ # rospy.loginfo("%s: Hover" % self.cf_id) self._state_machine.set_state("hover") return {} def land(self, _): """Land service """ # rospy.loginfo("%s: Landing" % self.cf_id) self._state_machine.set_state("land") return {} def stop(self, _): """Stop service """ self._state_machine.set_state("stop") return {} # Methods depending on state def _take_off(self): x_start = self.pose.position.x y_start = self.pose.position.y z_start = self.pose.position.z yaw_start = yaw_from_quat(self.pose.orientation) z_goal = TAKE_OFF_HEIGHT z_dist = z_goal - z_start duration = 3.0 # in sec n_steps = int(10 * duration) # Where 10 is the frequency z_inc = z_dist / n_steps for i in range(n_steps): if rospy.is_shutdown( ) or not self._state_machine.in_state("take_off"): break new_z = i * z_inc + z_start self.cmd_pos(x_start, y_start, new_z, yaw_start) self.rate.sleep() if self._state_machine.in_state("take_off"): self._state_machine.set_state("hover") def _hover(self): self.cmd_pos_msg.header.seq += 1 self.cmd_pos_msg.header.stamp = rospy.Time.now() self.cmd_pos(self.goal.x, self.goal.y, self.goal.z, self.goal.yaw) self.rate.sleep() def _land(self): rospy.sleep(0.2) x_start = self.pose.position.x y_start = self.pose.position.y z_start = self.pose.position.z yaw_start = yaw_from_quat(self.pose.orientation) z_dist = z_start - GND_HEIGHT time_range = 4 * 10 z_dec = z_dist / time_range for i in range(time_range): if rospy.is_shutdown() or not self._state_machine.in_state("land"): break new_z = z_start - i * z_dec self.cmd_pos(x_start, y_start, new_z, yaw_start) self.rate.sleep() self.cmd_pos(x_start, y_start, GND_HEIGHT, yaw_start) self.rate.sleep() self._state_machine.set_state("stop") def _stop(self): self.cmd_vel(0, 0, 0, 0) self.rate.sleep() # Publishing methods def cmd_vel(self, roll, pitch, yawrate, thrust): """Publish pose in cmd_vel topic Args: roll (float): Roll angle. Degrees. Positive values == roll right. pitch (float): Pitch angle. Degrees. Positive values == pitch forward/down. yawrate (float): Yaw angular velocity. Degrees / second. Positive values == turn counterclockwise. thrust (float): Thrust magnitude. Non-meaningful units in [0, 2^16), where the maximum value corresponds to maximum thrust. """ msg = Twist() msg.linear.x = pitch msg.linear.y = roll msg.angular.z = yawrate msg.linear.z = thrust self.cmd_vel_pub.publish(msg) def cmd_hovering(self, z_distance): """Publish hover in cmd_hover topic Args: zDistance (float): Distance to hover """ self.cmd_hovering_msg.zDistance = z_distance self.cmd_hovering_pub.publish(self.cmd_hovering_msg) def cmd_pos(self, x_val, y_val, _val, yaw): """Publish target position to cmd_positions topic Args: x_val (float): X y_val (float): Y z_val (float): Z yaw (float): Yaw """ self.cmd_pos_msg.x = x_val self.cmd_pos_msg.y = y_val self.cmd_pos_msg.z = _val self.cmd_pos_msg.yaw = yaw self.cmd_pos_pub.publish(self.cmd_pos_msg) def publish_state(self): """Publish current state """ self.current_state_pub.publish(self._state_machine.get_state()) # Run methods def run(self): """Run controller """ state_function = self._state_machine.run_state() state_function() self.publish_state()