Formation Manager

Module to compute the position of agents in a formation.

A formation is defined as a certain number of agents moving in a specific shape.

Note

An agent is NOT a crazyflie. A crazyflie is the actual robot. An agent is a position in a formation

This package uses two classes:

• FormationManager to switch between formations
• FormationClass to compute the position of each agent for a formation. One for each formation.

To set a new formation, set_formation service is called. The new positions of agents will be computed using the desired FormationClass child class.

Based on the position of each crazyflie, FormationManager will then link each CF to an agent. Once a CF is linked to an agent, this CF will follow this agent’s position in the formation.

FormationManager class

class formation_manager_ros.FormationManager(cf_list, min_dist, start_goal)

To change between formations and associate agents and CFs

Parameters:

• cf_list (list of str) – List of all CFs in swarm
• min_dist (float) – Minimum distance between agents in formation
• start_goal (list of float) – Formation start goal

Variables:

• abs_ctrl_mode (bool) – In abs ctrl mode, moves in world/ In rel ctrl mode, moves relative to yaw
• scale (float) – Formation scale
• formation (str) – Current formation
• extra_agents (list of str) – Id of CFs in extra

init_formation(initial_positions)

Initialize formation goal and cf positions.

initial_positions used to associate CF and agents

Parameters:initial_positions (dict of list) – Keys: Id of CF, Items: Initial position [x, y, z]

link_swarm_and_formation()

Link each agent of formation to a CF of the swarm and initialize formation goals

run_formation()

Execute formation

FormationClass class

Abstract Class that represents a general formation.

Cylindrical coordinates are used to specify the position of each agent in the formation.Center of formation is defined as (0, 0, 0)

class general_formation.FormationClass(min_dist)

Basic formation type

Parameters:min_dist (float) – Minimum distance between agents in formation

get_agents_goals()

Return goal of each agent in formation

Returns:Keys: agents id, Items: goal [x, y, z] Return type:dict of list

set_scale(new_scale)

Set scale of the formation

Parameters:new_scale (float) – New formation scale

update_agents_positions(formation_goal, crazyflies=None)

Compute goal of each agent and updates corresponding CF goal.

Agent goal is calculated based on distance and angle of agent id from formation center.

If crazyflies is not None, will update their goal.

Parameters:

• formation_goal (Position) – Goal of the formation
• crazyflies (dict, optional) – Information of each Crazyflie

find_extra_agents()

Find id of agents in extra.

Extra agents can happen when there are too many agents for a formation. i.e: Making a square with 10 agents will result in one extra agent.

set_n_agents(n_agents)

Make sure there number of CF is supported by formation and sets it

Parameters:n (int) – Number of CF

compute_min_scale()

Find minimum scale to make sure distance between agents is greater than min_dist

compute_formation_positions()

Compute position of each agent from formation center

Position are defined by radius from center (x, y plane), height from center and radius angle

update_formation_scale()

Compute new formation information after the scale is changed.

i.e: Distance/angle between agents

Unique to each formation

general_formation.compute_info_from_center(agent_position)

Calculate distance and angle from formation center

Formation center is considered to be at 0, 0, 0

Parameters:cf_position (list of float) – Position from [0 , 0 ,0][x, y, z] Returns:[distance from center, angle from center, height from center] Return type:list of float

general_formation.calculate_rot(start_orientation, rot)

Apply rotation to a quaternion

Parameters:

• start_orientation (Quaternion) – Initial orientation
• rot (Vector3) – Angular speed to apply

Returns:

Result

Return type:

Quaternion

general_formation.yaw_from_quat(quaternion)

Returns yaw from a quaternion

Parameters:quaternion (Quaternion) – Returns:Yaw Return type:float

general_formation.quat_from_yaw(yaw)

Compute a quaternion from yaw

Pitch and roll are considered zero

Parameters:yaw (float) – Yaw Returns:Quaternion