#!/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 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.0
pub.publish(quad)
rospy.loginfo("[%f, %f, %f - %f]", quad.x, quad.y, quad.z,
quad.yaw)
rate.sleep()
except rospy.ROSInterruptException:
pass
finally:
quad.header.stamp = rospy.Time.now()
try:
pub.publish(twist)
# rospy.loginfo("quad_twist = [%f, %f, %f - %f]", twist.linear.x, twist.linear.y, twist.linear.z, twist.angular.z)
except rospy.ROSException:
pass
if __name__ == '__main__':
rospy.init_node('pos_controller', anonymous=True)
rospy.loginfo("Node %s started!", rospy.get_name())
pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
twist = Twist()
twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
des = QuadStamped()
try:
rospy.sleep(1)
rospy.Subscriber('quad_state', QuadState, info_callback)
rospy.Subscriber('des_pos', QuadStamped, des_callback)
rospy.loginfo("Node %s start spining!", rospy.get_name())
rospy.spin()
except rospy.ROSInterruptException:
pass
finally:
twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
pub.publish(twist)
from random import random
from swarm.msg import QuadStamped
if __name__ == '__main__':
rospy.init_node('swarm_go_and_keep_moving', anonymous=True)
rospy.loginfo("Node %s started!", rospy.get_name())
rate = rospy.Rate(100)
argv = sys.argv
rospy.myargv(argv)
n = int(argv[1])
pub = []
quad = []
for i in range(n):
pub.append(rospy.Publisher('/uav' + str(i) + '/mid_state', QuadStamped, queue_size=n))
quad.append(QuadStamped())
quad[i].header.frame_id = 'world'
xy = rospy.get_param('/uav' + str(i))
quad[i].x = xy['x']
quad[i].y = xy['y']
quad[i].z = 0
quad[i].yaw = 0
try:
t = 1
rospy.loginfo("Node %s start sending!", rospy.get_name())
while not rospy.is_shutdown():
for i in range(n):
quad[i].header.stamp = rospy.Time.now()
if quad[i].header.stamp.secs >= t:
if i==0:
#!/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)
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 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)
c_frict = 0.1
r = [1.5, 1.0, 2.0, 1.1, 1.9]
# Publisher, initial messages and initial variables:
pub = []
quad = []
quad_state = []
a_pot = []
a_slip = []
nn = []
for i in range(n):
pub.append(
rospy.Publisher('/uav' + str(i) + '/des_pos',
QuadStamped,
queue_size=10))
quad.append(QuadStamped())
quad[i].header.frame_id = 'world'
xy = rospy.get_param('/uav' + str(i))
quad[i].x = xy['x']
quad[i].y = xy['y']
quad[i].z = 0.0
quad[i].yaw = 0.0
quad_state.append(QuadState())
a_pot.append(QuadStamped())
a_slip.append(QuadStamped())
nn.append(0.0)
try:
rospy.loginfo("Node %s start subscribing!", rospy.get_name())
while not rospy.is_shutdown():
for i in range(n):
#!/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.yaw = 1 # pi / 2.0
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 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 = 3
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 >= t:
quad.yaw = 2 * pi * random() - pi
t += 2
pub.publish(quad)
rospy.loginfo("[%f, %f, %f - %f]", quad.x, quad.y, quad.z,
quad.yaw)
rate.sleep()
#!/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.yaw = -2 * pi / 3.0
if quad.header.stamp.secs >= 6:
quad.yaw = 2 * pi / 3.0
pub.publish(quad)
rospy.loginfo("[%f, %f, %f - %f]", quad.x, quad.y, quad.z,
quad.yaw)
rate.sleep()
# n number of quads in total:
argv = sys.argv
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(