def compute_formation_positions(self):
# (dX, dY) sign for each position in tier
tier_poses_sign = [(-1, -1), (-1, 1), (1, 1), (1, -1)]
for i in range(self._n_agents):
if rospy.is_shutdown():
break
# Initialize agent formation goal
self._agents_goals[i] = Position()
# Find tier information
# i=0 -> tier=0, i=1,2,3,4 -> tier = 1, i=5,6,7,8 -> tier = 2 ...
tier_num = ceil(i / 4.0)
tier_pos = i % 4 # Position in the tier
square_length = 2 * sin(self.theta) * self.tier_dist * tier_num
# Compute formation position
z_dist = -1 * tier_num * self.tier_dist
x_dist = tier_poses_sign[tier_pos][0] * square_length / 2
y_dist = tier_poses_sign[tier_pos][1] * square_length / 2
# information from center
center_dist, theta, center_height = compute_info_from_center(
[x_dist, y_dist, z_dist])
self._center_dist[i] = center_dist
self._angle[i] = theta
self._center_height[i] = center_height
def compute_formation_positions(self):
for i in range(self._n_agents):
if rospy.is_shutdown():
break
# Initialize agent formation goal
self._agents_goals[i] = Position()
# Compute formation position
z_dist = 0
if i == 0:
x_dist = 0
y_dist = 0
else:
angle = i * self.angle_between_agents
x_dist = self._scale * cos(angle)
y_dist = self._scale * sin(angle)
# Compute information from center
center_dist, theta, center_height = compute_info_from_center(
[x_dist, y_dist, z_dist])
self._center_dist[i] = center_dist
self._angle[i] = theta
self._center_height[i] = center_height
return self._agents_goals
def compute_formation_positions(self):
for i in range(self._n_agents):
if rospy.is_shutdown():
break
# Initialize agent formation goal
self._agents_goals[i] = Position()
# Compute formation position
# TODO: Compute agent position from center
# Compute information from center
center_dist, theta, center_height = compute_info_from_center(
[x_dist, y_dist, z_dist])
self._center_dist[i] = center_dist
self._angle[i] = theta
self._center_height[i] = center_height
return self._agents_goals
def compute_formation_positions(self):
center_offset = self._scale/2
for i in range(self._n_agents):
if rospy.is_shutdown():
break
# Initialize agent formation goal
self._agents_goals[i] = Position()
# Compute formation position
z_dist = 0
x_dist = 0
y_dist = self.agents_dist*i - center_offset
# Compute information from center
center_dist, theta, center_height = compute_info_from_center([x_dist, y_dist, z_dist])
self._center_dist[i] = center_dist
self._angle[i] = theta
self._center_height[i] = center_height
def compute_formation_positions(self):
for i in range(self._n_agents):
if rospy.is_shutdown():
break
# Initialize agent formation goal
self._agents_goals[i] = Position()
# Find row number
# i = 0 -> row = 0, i = 1 -> row = 1, i = 2 -> row = 1, i = 3 -> row = 2 ...
row_num = ceil(i/2.0)
# Find if above or below center
sign = -1
if i % 2 == 0:
sign = 1
x_dist = -self.dist*row_num * cos(self.theta/2)
y_dist = self.dist*row_num * sin(self.theta/2) * sign
center_dist, theta, center_height = compute_info_from_center([x_dist, y_dist, 0])
self._center_dist[i] = center_dist
self._angle[i] = theta
self._center_height[i] = center_height