#!/usr/bin/env python
import rospy
from math import pi
from random import random
from swarm.msg import QuadStamped
if __name__ == '__main__':
pub = rospy.Publisher('des_pos', QuadStamped, queue_size=1)
rospy.init_node('go_up_and_stay', anonymous=True)
rate = rospy.Rate(100)
quad = QuadStamped()
quad.header.frame_id = 'world'
quad.x = 0
quad.y = 0
quad.z = 0
quad.yaw = 0
try:
t = 2
while not rospy.is_shutdown():
quad.header.stamp = rospy.Time.now()
if quad.header.stamp.secs >= t:
quad.x = 5 * random() - 2.5
quad.y = 5 * random() - 2.5
quad.z = 1.5 * random() + 0.5
quad.yaw = 2 * pi * random() - pi
t += 4
pub.publish(quad)
rospy.loginfo("[%f, %f, %f - %f]", quad.x, quad.y, quad.z,
quad.yaw)
#!/usr/bin/env python
import rospy
from swarm.msg import QuadStamped
if __name__ == '__main__':
pub = rospy.Publisher('des_pos', QuadStamped, queue_size=1)
rospy.init_node('go_up_and_stay', anonymous=True)
rate = rospy.Rate(100)
quad = QuadStamped();
quad.header.frame_id = 'world'
quad.x = 0
quad.y = 0
quad.z = 0
quad.yaw = 0
try:
while not rospy.is_shutdown():
quad.header.stamp = rospy.Time.now()
if quad.header.stamp.secs >= 2:
quad.z = 1
if quad.header.stamp.secs >= 3:
quad.y = 1
pub.publish(quad)
rospy.loginfo("[%f, %f, %f - %f]", quad.x, quad.y, quad.z, quad.yaw)
rate.sleep()
except rospy.ROSInterruptException:
pass
finally:
#!/usr/bin/env python
import rospy
from math import pi
from random import randrange
from swarm.msg import QuadStamped
if __name__ == '__main__':
pub = rospy.Publisher('des_pos', QuadStamped, queue_size=1)
rospy.init_node('go_up_and_stay', anonymous=True)
rate = rospy.Rate(100)
quad = QuadStamped()
quad.header.frame_id = 'world'
quad.x = 0
quad.y = 0
quad.z = 0
quad.yaw = 0
try:
while not rospy.is_shutdown():
quad.header.stamp = rospy.Time.now()
if quad.header.stamp.secs >= 2:
quad.z = 1
if quad.header.stamp.secs >= 3:
quad.yaw = pi / 4.0
if quad.header.stamp.secs >= 4:
if quad.x == 0: quad.x = randrange(-10, 11, 1) / 10.0
if quad.y == 0: quad.y = randrange(-10, 11, 1) / 10.0
pub.publish(quad)
rospy.loginfo("[%f, %f, %f - %f]", quad.x, quad.y, quad.z,
quad.yaw)
def apply_reynolds_rules(quad_state, n, D, c_frict, r, a_pot, a_slip, nn):
global quad
# Restart variables:
for i in range(n):
a_pot[i].x = 0.0
a_pot[i].y = 0.0
a_pot[i].z = 0.0
a_pot[i].yaw = 0.0
a_slip[i].x = 0.0
a_slip[i].y = 0.0
a_slip[i].z = 0.0
a_slip[i].yaw = 0.0
nn[i] = 0.0
dist = QuadStamped()
for i in range(n):
for j in range(i, n):
if not (i == j):
dist.x = quad_state[j].pos.x - quad_state[i].pos.x
dist.y = quad_state[j].pos.y - quad_state[i].pos.y
dist.z = quad_state[j].pos.z - quad_state[i].pos.z
dist.yaw = quad_state[j].pos.yaw - quad_state[i].pos.yaw
d = sqrt(dist.x * dist.x + dist.y * dist.y + dist.z * dist.z +
dist.yaw * dist.yaw)
if d > 0.0:
if d < r[2]:
d2 = max(d - (r[2] - r[4]), r[3])
d2 = d2 * d2
nn[i] += 1.0
nn[j] += 1.0
a_slip[i].x += quad_state[
j].vel.x - quad_state[i].vel.x / d2
a_slip[i].y += quad_state[
j].vel.y - quad_state[i].vel.y / d2
a_slip[i].z += quad_state[
j].vel.z - quad_state[i].vel.z / d2
a_slip[i].yaw += quad_state[
j].vel.yaw - quad_state[i].vel.yaw / d2
a_slip[j].x -= a_slip[i].x
a_slip[j].y -= a_slip[i].y
a_slip[j].z -= a_slip[i].z
a_slip[j].yaw -= a_slip[i].yaw
if d < r[0]:
a_pot[i].x += min(r[1], r[0] - d) * dist.x / d
a_pot[i].y += min(r[1], r[0] - d) * dist.y / d
a_pot[i].z += min(r[1], r[0] - d) * dist.z / d
a_pot[i].yaw += min(r[1], r[0] - d) * dist.yaw / d
a_pot[j].x -= a_pot[i].x
a_pot[j].y -= a_pot[i].y
a_pot[j].z -= a_pot[i].z
a_pot[j].yaw -= a_pot[i].yaw
m = max(nn[i], 1)
a_slip[i].x /= m
a_slip[i].y /= m
a_slip[i].z /= m
a_slip[i].yaw /= m
# Final movements:
quad[
i].x = quad_state[i].pos.x - D * a_pot[i].x + c_frict * a_slip[i].x
quad[
i].y = quad_state[i].pos.y - D * a_pot[i].y + c_frict * a_slip[i].y
quad[
i].z = quad_state[i].pos.z - D * a_pot[i].z + c_frict * a_slip[i].z
quad[i].yaw = quad_state[
i].pos.yaw - D * a_pot[i].yaw + c_frict * a_slip[i].yaw
#!/usr/bin/env python
import rospy
from swarm.msg import QuadStamped
if __name__ == '__main__':
pub = rospy.Publisher('des_pos', QuadStamped, queue_size=1)
rospy.init_node('go_up_and_stay', anonymous=True)
rate = rospy.Rate(100)
quad = QuadStamped()
quad.header.frame_id = 'world'
quad.x = 0
quad.y = 0
quad.z = 0
quad.yaw = 0
try:
while not rospy.is_shutdown():
quad.header.stamp = rospy.Time.now()
if quad.header.stamp.secs >= 2:
quad.z = 1
if quad.header.stamp.secs >= 3:
quad.x = 1
pub.publish(quad)
rospy.loginfo("[%f, %f, %f - %f]", quad.x, quad.y, quad.z,
quad.yaw)
rate.sleep()
except rospy.ROSInterruptException:
pass
#!/usr/bin/env python
import rospy
from math import pi
from swarm.msg import QuadStamped
if __name__ == '__main__':
pub = rospy.Publisher('des_pos', QuadStamped, queue_size=1)
rospy.init_node('go_up_and_stay', anonymous=True)
rate = rospy.Rate(100)
quad = QuadStamped()
quad.header.frame_id = 'world'
quad.x = 0
quad.y = 0
quad.z = 0
quad.yaw = 0
try:
while not rospy.is_shutdown():
quad.header.stamp = rospy.Time.now()
if quad.header.stamp.secs >= 2:
quad.z = 1
if quad.header.stamp.secs >= 3:
quad.x = 1
if quad.header.stamp.secs >= 4:
quad.yaw = 1 # pi / 2.0
if quad.header.stamp.secs >= 5:
quad.x = 2
pub.publish(quad)
rospy.loginfo("[%f, %f, %f - %f]", quad.x, quad.y, quad.z,
quad.yaw)
rospy.myargv(argv)
n = int(argv[1])
# Counter for other quads
other = 0
# Current quad and initial position:
name = rospy.get_namespace()
current = int(name[-2])
xy = rospy.get_param(name)
# Publisher and initial message:
pub = rospy.Publisher('des_pos', QuadStamped, queue_size=10)
quad = QuadStamped()
quad.header.frame_id = 'world'
quad.x = xy['x']
quad.y = xy['y']
quad.z = 0.0
quad.yaw = 0.0
sub = []
ts = []
for i in range(n):
sub.append(
message_filters.Subscriber('/uav' + str(i) + '/next_generation',
QuadState))
for i in range(n):
if not current == i:
ts.append(
message_filters.ApproximateTimeSynchronizer(
[sub[current], sub[i]], 10, 0.01))