forked from xtdrone/XTDrone
完善编队demo
This commit is contained in:
parent
2a03e671c4
commit
8aef6e4948
|
@ -1,341 +0,0 @@
|
|||
#!/usr/bin/python
|
||||
# -*- coding: UTF-8 -*-
|
||||
import rospy
|
||||
from geometry_msgs.msg import Twist, Vector3, PoseStamped, TwistStamped
|
||||
from std_msgs.msg import String
|
||||
from pyquaternion import Quaternion
|
||||
from formation_dict import formation_dict_6
|
||||
import time
|
||||
import math
|
||||
import numpy
|
||||
import sys
|
||||
import heapq
|
||||
import copy
|
||||
|
||||
class Follower:
|
||||
|
||||
def __init__(self, uav_id, uav_num):
|
||||
self.hover = True
|
||||
self.id = uav_id
|
||||
self.uav_num = uav_num
|
||||
self.local_pose = PoseStamped()
|
||||
self.local_velocity = TwistStamped()
|
||||
self.cmd_vel_enu = Twist()
|
||||
self.cmd_accel_enu = Vector3()
|
||||
self.avoid_vel = Vector3()
|
||||
self.following_switch = False
|
||||
self.following_ids = []
|
||||
self.formation_config = 'waiting'
|
||||
self.following_count = 0
|
||||
self.kp = 1
|
||||
#self.kr = (4/int((self.uav_num-1)/2))**0.5
|
||||
self.kr = 0.1
|
||||
self.f = 100
|
||||
self.first_x=True
|
||||
self.first_y=True
|
||||
self.first_z=True
|
||||
self.following_local_pose = [PoseStamped() for i in range (self.uav_num)]
|
||||
self.following_local_pose_sub = [None]*self.uav_num
|
||||
self.following_local_velocity = [TwistStamped() for i in range (self.uav_num)]
|
||||
self.following_local_velocity_sub = [None]*self.uav_num
|
||||
self.arrive_count = 0
|
||||
self.local_pose_sub = rospy.Subscriber("/uav"+str(self.id)+"/mavros/local_position/pose", PoseStamped , self.local_pose_callback)
|
||||
self.local_velocity_sub = rospy.Subscriber("/uav"+str(self.id)+"/mavros/local_position/velocity_local", TwistStamped , self.local_velocity_callback)
|
||||
#self.cmd_vel_sub = rospy.Subscriber("/xtdrone/follower/cmd_vel", Twist, self.cmd_vel_callback)
|
||||
#self.avoid_vel_sub = rospy.Subscriber("/xtdrone/uav"+str(self.id)+"/avoid_vel", Vector3, self.avoid_vel_callback)
|
||||
self.formation_switch_sub = rospy.Subscriber("/xtdrone/formation_switch",String, self.formation_switch_callback)
|
||||
#self.vel_enu_pub = rospy.Publisher('/xtdrone/uav'+str(self.id)+'/cmd_vel_enu', Twist, queue_size=10)
|
||||
self.accel_enu_pub = rospy.Publisher('/xtdrone/uav'+str(self.id)+'/cmd_accel_enu', Vector3, queue_size=10)
|
||||
self.info_pub = rospy.Publisher('/xtdrone/uav'+str(self.id)+'/info', String, queue_size=10)
|
||||
|
||||
|
||||
def local_pose_callback(self, msg):
|
||||
self.local_pose = msg
|
||||
#print(self.arrive_count)
|
||||
|
||||
def local_velocity_callback(self, msg):
|
||||
self.local_velocity = msg
|
||||
#print(self.arrive_count)
|
||||
|
||||
def following_local_pose_callback(self, msg, id):
|
||||
self.following_local_pose[id] = msg
|
||||
|
||||
|
||||
def following_local_velocity_callback(self, msg, id):
|
||||
self.following_local_velocity[id] = msg
|
||||
|
||||
|
||||
def cmd_vel_callback(self, msg):
|
||||
self.cmd_vel_enu = msg
|
||||
if msg.linear.z == 0:
|
||||
self.hover = True
|
||||
else:
|
||||
self.hover = False
|
||||
|
||||
def formation_switch_callback(self, msg):
|
||||
if not self.formation_config == msg.data:
|
||||
self.following_switch = True
|
||||
self.formation_config = msg.data
|
||||
|
||||
|
||||
#def avoid_vel_callback(self, msg):
|
||||
#self.avoid_vel = msg
|
||||
|
||||
def loop(self):
|
||||
rospy.init_node('follower'+str(self.id-1))
|
||||
rate = rospy.Rate(self.f)
|
||||
while not rospy.is_shutdown():
|
||||
if self.arrive_count > 100 and self.arrive_print:
|
||||
print("Follower"+str(self.id-1)+":Arrived")
|
||||
self.arrive_print = False
|
||||
if self.following_switch:
|
||||
self.following_switch = False
|
||||
for i in range(self.f/10):
|
||||
self.info_pub.publish("Received")
|
||||
print("Follower"+str(self.id-1)+": Switch to Formation "+self.formation_config)
|
||||
self.L_matrix = self.get_L_matrix(formation_dict_6[self.formation_config])
|
||||
#print(self.L_matrix)
|
||||
#self.L_matrix = numpy.array([[0,0,0,0,0,0,0,0,0],[1,-1,0,0,0,0,0,0,0],[1,0,-1,0,0,0,0,0,0],[1,0,0,-1,0,0,0,0,0],[1,0,0,0,-1,0,0,0,0],[1,0,0,0,0,-1,0,0,0],[1,0,0,0,0,0,-1,0,0],[1,0,0,0,0,0,0,-1,0],[1,0,0,0,0,0,0,0,-1]])
|
||||
self.following_ids = numpy.argwhere(self.L_matrix[self.id-1,:] == 1)
|
||||
#if self.id == 2:
|
||||
#print(self.following_ids)
|
||||
self.following_count = 0
|
||||
for i in range(self.uav_num):
|
||||
if not self.following_local_pose_sub[i] == None:
|
||||
self.following_local_pose_sub[i].unregister()
|
||||
if not self.following_local_velocity_sub[i] == None:
|
||||
self.following_local_velocity_sub[i].unregister()
|
||||
for following_id in self.following_ids:
|
||||
#print('here')
|
||||
self.following_local_pose_sub[following_id[0]] = rospy.Subscriber("/uav"+str(following_id[0]+1)+"/mavros/local_position/pose", PoseStamped , self.following_local_pose_callback,following_id[0])
|
||||
self.following_local_velocity_sub[following_id[0]] = rospy.Subscriber("/uav"+str(following_id[0]+1)+"/mavros/local_position/velocity_local", TwistStamped , self.following_local_velocity_callback,following_id[0])
|
||||
self.following_count += 1
|
||||
|
||||
self.cmd_accel_enu = Vector3(0, 0, 0)
|
||||
#self.cmd_vel_enu.linear = copy.deepcopy(self.avoid_vel)
|
||||
for following_id in self.following_ids:
|
||||
if self.following_local_pose[following_id[0]] == None and self.following_local_velocity[following_id[0]] == None:
|
||||
print(following_id)
|
||||
self.cmd_accel_enu.x += self.kp * (self.following_local_pose[following_id[0]].pose.position.x - self.local_pose.pose.position.x + self.kr * (self.following_local_velocity[following_id[0]].twist.linear.x - self.local_velocity.twist.linear.x) + formation_dict_6[self.formation_config][0, self.id-2])
|
||||
self.cmd_accel_enu.y += self.kp * (self.following_local_pose[following_id[0]].pose.position.y - self.local_pose.pose.position.y + self.kr * (self.following_local_velocity[following_id[0]].twist.linear.y - self.local_velocity.twist.linear.y) + formation_dict_6[self.formation_config][1, self.id-2])
|
||||
self.cmd_accel_enu.z += self.kp * (self.following_local_pose[following_id[0]].pose.position.z - self.local_pose.pose.position.z + formation_dict_6[self.formation_config][2, self.id-2]+ self.kr * (self.following_local_velocity[following_id[0]].twist.linear.z - self.local_velocity.twist.linear.z))
|
||||
|
||||
if not following_id[0] == 0:
|
||||
self.cmd_accel_enu.x -= formation_dict_6[self.formation_config][0, following_id[0]-1]
|
||||
self.cmd_accel_enu.y -= formation_dict_6[self.formation_config][1, following_id[0]-1]
|
||||
self.cmd_accel_enu.z -= formation_dict_6[self.formation_config][2, following_id[0]-1]
|
||||
|
||||
if self.arrive_count > 100:
|
||||
self.cmd_accel_enu = Vector3(0,0,0)
|
||||
if not self.formation_config == 'waiting':
|
||||
self.accel_enu_pub.publish(self.cmd_accel_enu)
|
||||
rate.sleep()
|
||||
|
||||
|
||||
#函数输入为相对leader的位置矩阵和无人机数量,输出为L矩阵
|
||||
def get_L_matrix(self,rel_posi):
|
||||
|
||||
c_num=int((self.uav_num-1)/2)
|
||||
min_num_index_list = [0]*c_num
|
||||
|
||||
comm=[[]for i in range (self.uav_num)]
|
||||
w=numpy.ones((self.uav_num,self.uav_num))*0
|
||||
nodes_next=[]
|
||||
node_flag = [self.uav_num-1]
|
||||
node_mid_flag=[]
|
||||
|
||||
rel_d=[0]*(self.uav_num-1)
|
||||
|
||||
|
||||
for i in range(0,self.uav_num-1):
|
||||
|
||||
rel_d[i]=pow(rel_posi[0][i],2)+pow(rel_posi[1][i],2)+pow(rel_posi[2][i],2)
|
||||
|
||||
c=numpy.copy(rel_d)
|
||||
c.sort()
|
||||
count=0
|
||||
|
||||
for j in range(0,c_num):
|
||||
for i in range(0,self.uav_num-1):
|
||||
if rel_d[i]==c[j]:
|
||||
if not i in node_mid_flag:
|
||||
min_num_index_list[count]=i
|
||||
node_mid_flag.append(i)
|
||||
count=count+1
|
||||
if count==c_num:
|
||||
break
|
||||
if count==c_num:
|
||||
break
|
||||
|
||||
|
||||
for j in range(0,c_num):
|
||||
nodes_next.append(min_num_index_list[j])
|
||||
|
||||
comm[self.uav_num-1].append(min_num_index_list[j])
|
||||
|
||||
size_=len(node_flag)
|
||||
|
||||
while (nodes_next!=[]) and (size_<(self.uav_num-1)):
|
||||
|
||||
next_node= nodes_next[0]
|
||||
nodes_next=nodes_next[1:]
|
||||
min_num_index_list = [0]*c_num
|
||||
node_mid_flag=[]
|
||||
rel_d=[0]*(self.uav_num-1)
|
||||
for i in range(0,self.uav_num-1):
|
||||
|
||||
if i==next_node or i in node_flag:
|
||||
|
||||
rel_d[i]=2000
|
||||
else:
|
||||
|
||||
rel_d[i]=pow((rel_posi[0][i]-rel_posi[0][next_node]),2)+pow((rel_posi[1][i]-rel_posi[1][next_node]),2)+pow((rel_posi[2][i]-rel_posi[2][next_node]),2)
|
||||
|
||||
c=numpy.copy(rel_d)
|
||||
c.sort()
|
||||
count=0
|
||||
|
||||
for j in range(0,c_num):
|
||||
for i in range(0,self.uav_num-1):
|
||||
if rel_d[i]==c[j]:
|
||||
if not i in node_mid_flag:
|
||||
min_num_index_list[count]=i
|
||||
node_mid_flag.append(i)
|
||||
count=count+1
|
||||
if count==c_num:
|
||||
break
|
||||
if count==c_num:
|
||||
break
|
||||
node_flag.append(next_node)
|
||||
|
||||
size_=len(node_flag)
|
||||
|
||||
for j in range(0,c_num):
|
||||
|
||||
if min_num_index_list[j] in node_flag:
|
||||
|
||||
nodes_next=nodes_next
|
||||
|
||||
else:
|
||||
if min_num_index_list[j] in nodes_next:
|
||||
nodes_next=nodes_next
|
||||
else:
|
||||
nodes_next.append(min_num_index_list[j])
|
||||
|
||||
comm[next_node].append(min_num_index_list[j])
|
||||
|
||||
for i in range (0,self.uav_num):
|
||||
for j in range(0,self.uav_num-1):
|
||||
|
||||
if i==0:
|
||||
if j in comm[self.uav_num-1]:
|
||||
w[j+1][i]=1
|
||||
else:
|
||||
w[j+1][i]=0
|
||||
else:
|
||||
if j in comm[i-1]:
|
||||
w[j+1][i]=1
|
||||
else:
|
||||
w[j+1][i]=0
|
||||
|
||||
L=w
|
||||
for i in range (0,self.uav_num):
|
||||
|
||||
L[i][i]=-sum(w[i])
|
||||
|
||||
return L
|
||||
'''
|
||||
def get_L_matrix(self, rel_posi):
|
||||
|
||||
#假设无论多少UAV,都假设尽可能3层通信(叶子节点除外)
|
||||
c_num=int((self.uav_num-1)/3)
|
||||
|
||||
comm=[[]for i in range (self.uav_num)]
|
||||
w=numpy.ones((self.uav_num,self.uav_num))*0 # 定义邻接矩阵
|
||||
nodes_next=[]
|
||||
node_flag = [self.uav_num-1]
|
||||
|
||||
rel_d=[0]*(self.uav_num-1)
|
||||
# 规定每个无人机可以随机连接三台距离自己最近的无人机,且不能连接在flag中的无人机(即已经判断过连接点的无人机)。
|
||||
# 计算第一层通信(leader):获得离自己最近的三台无人机编号
|
||||
|
||||
for i in range(0,self.uav_num-1):
|
||||
|
||||
rel_d[i]=pow(rel_posi[0][i],2)+pow(rel_posi[1][i],2)+pow(rel_posi[2][i],2)
|
||||
|
||||
min_num_index_list = map(rel_d.index, heapq.nsmallest(c_num, rel_d))
|
||||
min_num_index_list=list(min_num_index_list)
|
||||
#leader 连接的无人机编号:
|
||||
comm[self.uav_num-1]=min_num_index_list
|
||||
|
||||
nodes_next.extend(comm[self.uav_num-1])
|
||||
|
||||
size_=len(node_flag)
|
||||
|
||||
while (nodes_next!=[]) and (size_<(self.uav_num-1)):
|
||||
|
||||
next_node= nodes_next[0]
|
||||
nodes_next=nodes_next[1:]
|
||||
|
||||
rel_d=[0]*(self.uav_num-1)
|
||||
for i in range(0,self.uav_num-1):
|
||||
|
||||
if i==next_node or i in node_flag:
|
||||
|
||||
rel_d[i]=2000 #这个2000是根据相对位置和整个地图的大小决定的,要比最大可能相对距离大
|
||||
else:
|
||||
|
||||
rel_d[i]=pow((rel_posi[0][i]-rel_posi[0][next_node]),2)+pow((rel_posi[1][i]-rel_posi[1][next_node]),2)+pow((rel_posi[2][i]-rel_posi[2][next_node]),2)
|
||||
|
||||
min_num_index_list = map(rel_d.index, heapq.nsmallest(c_num, rel_d))
|
||||
min_num_index_list=list(min_num_index_list)
|
||||
node_flag.append(next_node)
|
||||
|
||||
size_=len(node_flag)
|
||||
|
||||
for j in range(0,c_num):
|
||||
|
||||
if min_num_index_list[j] in node_flag:
|
||||
|
||||
nodes_next=nodes_next
|
||||
|
||||
else:
|
||||
if min_num_index_list[j] in nodes_next:
|
||||
nodes_next=nodes_next
|
||||
else:
|
||||
nodes_next.append(min_num_index_list[j])
|
||||
|
||||
comm[next_node].append(min_num_index_list[j])
|
||||
# comm为每个uav连接其他uav的编号,其中数组的最后一行为leader
|
||||
#print (comm)
|
||||
#leader是拉普拉斯矩阵的第一行,为0
|
||||
#第0号飞机(相对位置矩阵中的第一个位置)为第二行,以此类推
|
||||
|
||||
for i in range (0,self.uav_num):
|
||||
for j in range(0,self.uav_num-1):
|
||||
|
||||
if i==0:
|
||||
if j in comm[self.uav_num-1]:
|
||||
w[j+1][i]=1
|
||||
else:
|
||||
w[j+1][i]=0
|
||||
else:
|
||||
if j in comm[i-1]:
|
||||
w[j+1][i]=1
|
||||
else:
|
||||
w[j+1][i]=0
|
||||
|
||||
L=w #定义拉普拉斯矩阵
|
||||
for i in range (0,self.uav_num):
|
||||
|
||||
L[i][i]=-sum(w[i])
|
||||
|
||||
#print (L)
|
||||
return L
|
||||
'''
|
||||
|
||||
if __name__ == '__main__':
|
||||
follower = Follower(int(sys.argv[1]),int(sys.argv[2]))
|
||||
#follower = Follower(int(sys.argv[1]),9)
|
||||
#follower = Follower(9,9)
|
||||
follower.loop()
|
|
@ -1,219 +0,0 @@
|
|||
#!/usr/bin/python
|
||||
# -*- coding: UTF-8 -*-
|
||||
import rospy
|
||||
from geometry_msgs.msg import Twist, Vector3, PoseStamped
|
||||
from std_msgs.msg import String
|
||||
from pyquaternion import Quaternion
|
||||
from formation_dict import formation_dict_9
|
||||
import time
|
||||
import math
|
||||
import numpy
|
||||
import sys
|
||||
import heapq
|
||||
import copy
|
||||
|
||||
class Follower:
|
||||
|
||||
def __init__(self, uav_id, uav_num):
|
||||
self.hover = True
|
||||
self.id = uav_id
|
||||
self.uav_num = uav_num
|
||||
self.local_pose = PoseStamped()
|
||||
self.cmd_vel_enu = Twist()
|
||||
self.avoid_vel = Vector3()
|
||||
self.following_switch = False
|
||||
self.following_ids = []
|
||||
self.formation_config = 'waiting'
|
||||
self.following_count = 0
|
||||
self.Kp = 1
|
||||
self.vel_xy_max = 1
|
||||
self.vel_z_max = 1
|
||||
self.following_local_pose = [None]*self.uav_num
|
||||
self.following_local_pose_sub = [None]*self.uav_num
|
||||
self.arrive_count = 0
|
||||
self.local_pose_sub = rospy.Subscriber("/uav"+str(self.id)+"/mavros/local_position/pose", PoseStamped , self.local_pose_callback)
|
||||
#self.cmd_vel_sub = rospy.Subscriber("/xtdrone/follower/cmd_vel", Twist, self.cmd_vel_callback)
|
||||
self.avoid_vel_sub = rospy.Subscriber("/xtdrone/uav"+str(self.id)+"/avoid_vel", Vector3, self.avoid_vel_callback)
|
||||
self.formation_switch_sub = rospy.Subscriber("/xtdrone/formation_switch",String, self.formation_switch_callback)
|
||||
self.vel_enu_pub = rospy.Publisher('/xtdrone/uav'+str(self.id)+'/cmd_vel_enu', Twist, queue_size=10)
|
||||
self.info_pub = rospy.Publisher('/xtdrone/uav'+str(self.id)+'/info', String, queue_size=10)
|
||||
|
||||
def local_pose_callback(self, msg):
|
||||
self.local_pose = msg
|
||||
#print(self.arrive_count)
|
||||
|
||||
def following_local_pose_callback(self, msg, id):
|
||||
self.following_local_pose[id] = msg
|
||||
#print('here')
|
||||
|
||||
def cmd_vel_callback(self, msg):
|
||||
self.cmd_vel_enu = msg
|
||||
if msg.linear.z == 0:
|
||||
self.hover = True
|
||||
else:
|
||||
self.hover = False
|
||||
|
||||
def formation_switch_callback(self, msg):
|
||||
if not self.formation_config == msg.data:
|
||||
self.following_switch = True
|
||||
else:
|
||||
self.following_switch = False
|
||||
self.formation_config = msg.data
|
||||
|
||||
|
||||
def avoid_vel_callback(self, msg):
|
||||
self.avoid_vel = msg
|
||||
#if not self.avoid_vel.x == 0:
|
||||
# print('follower'+str(self.id-1)+' avoid_vel: ',self.avoid_vel)
|
||||
|
||||
def loop(self):
|
||||
rospy.init_node('follower'+str(self.id-1))
|
||||
rate = rospy.Rate(50)
|
||||
while not rospy.is_shutdown():
|
||||
if self.arrive_count > 100:
|
||||
self.info_pub.publish("Arrived")
|
||||
#print("Arrived")
|
||||
if self.following_switch:
|
||||
self.following_switch = False
|
||||
self.info_pub.publish("Received")
|
||||
print("Follower"+str(self.id-1)+": Switch to Formation "+self.formation_config)
|
||||
self.L_matrix = self.get_L_matrix(formation_dict_9[self.formation_config])
|
||||
#print(self.L_matrix)
|
||||
#self.L_matrix = numpy.array([[0,0,0,0,0,0,0,0,0],[1,-1,0,0,0,0,0,0,0],[1,0,-1,0,0,0,0,0,0],[1,0,0,-1,0,0,0,0,0],[1,0,0,0,-1,0,0,0,0],[1,0,0,0,0,-1,0,0,0],[1,0,0,0,0,0,-1,0,0],[1,0,0,0,0,0,0,-1,0],[1,0,0,0,0,0,0,0,-1]])
|
||||
self.following_ids = numpy.argwhere(self.L_matrix[self.id-1,:] == 1)
|
||||
#if self.id == 2:
|
||||
#print(self.following_ids)
|
||||
self.following_count = 0
|
||||
for i in range(self.uav_num):
|
||||
if not self.following_local_pose_sub[i] == None:
|
||||
self.following_local_pose_sub[i].unregister()
|
||||
for following_id in self.following_ids:
|
||||
#print('here')
|
||||
self.following_local_pose_sub[following_id[0]] = rospy.Subscriber("/uav"+str(following_id[0]+1)+"/mavros/local_position/pose", PoseStamped , self.following_local_pose_callback,following_id[0])
|
||||
self.following_count += 1
|
||||
time.sleep(1)
|
||||
#print(self.avoid_vel)
|
||||
self.cmd_vel_enu.linear = copy.deepcopy(self.avoid_vel)
|
||||
if self.cmd_vel_enu.linear.x == 0 and self.cmd_vel_enu.linear.y == 0 and self.cmd_vel_enu.linear.z == 0:
|
||||
for following_id in self.following_ids:
|
||||
self.cmd_vel_enu.linear.x += self.following_local_pose[following_id[0]].pose.position.x - self.local_pose.pose.position.x + formation_dict_9[self.formation_config][0, self.id-2]
|
||||
self.cmd_vel_enu.linear.y += self.following_local_pose[following_id[0]].pose.position.y - self.local_pose.pose.position.y + formation_dict_9[self.formation_config][1, self.id-2]
|
||||
self.cmd_vel_enu.linear.z += self.following_local_pose[following_id[0]].pose.position.z - self.local_pose.pose.position.z + formation_dict_9[self.formation_config][2, self.id-2]
|
||||
if not following_id[0] == 0:
|
||||
self.cmd_vel_enu.linear.x -= formation_dict_9[self.formation_config][0, following_id[0]-1]
|
||||
self.cmd_vel_enu.linear.y -= formation_dict_9[self.formation_config][1, following_id[0]-1]
|
||||
self.cmd_vel_enu.linear.z -= formation_dict_9[self.formation_config][2, following_id[0]-1]
|
||||
self.cmd_vel_enu.linear.x = self.Kp * self.cmd_vel_enu.linear.x
|
||||
self.cmd_vel_enu.linear.y = self.Kp * self.cmd_vel_enu.linear.y
|
||||
self.cmd_vel_enu.linear.z = self.Kp * self.cmd_vel_enu.linear.z
|
||||
if self.cmd_vel_enu.linear.x > self.vel_xy_max:
|
||||
self.cmd_vel_enu.linear.x = self.vel_xy_max
|
||||
elif self.cmd_vel_enu.linear.x < - self.vel_xy_max:
|
||||
self.cmd_vel_enu.linear.x = - self.vel_xy_max
|
||||
if self.cmd_vel_enu.linear.y > self.vel_xy_max:
|
||||
self.cmd_vel_enu.linear.y = self.vel_xy_max
|
||||
elif self.cmd_vel_enu.linear.y < - self.vel_xy_max:
|
||||
self.cmd_vel_enu.linear.y = - self.vel_xy_max
|
||||
if self.cmd_vel_enu.linear.z > self.vel_z_max:
|
||||
self.cmd_vel_enu.linear.z = self.vel_z_max
|
||||
elif self.cmd_vel_enu.linear.z < - self.vel_z_max:
|
||||
self.cmd_vel_enu.linear.z = - self.vel_z_max
|
||||
if (self.cmd_vel_enu.linear.x)**2+(self.cmd_vel_enu.linear.y)**2+(self.cmd_vel_enu.linear.z)**2<0.2:
|
||||
self.arrive_count += 1
|
||||
else:
|
||||
self.arrive_count = 0
|
||||
if not self.formation_config == 'waiting':
|
||||
self.vel_enu_pub.publish(self.cmd_vel_enu)
|
||||
rate.sleep()
|
||||
|
||||
def get_L_matrix(self, rel_posi):
|
||||
|
||||
#假设无论多少UAV,都假设尽可能3层通信(叶子节点除外)
|
||||
c_num=int((self.uav_num-1)/3)
|
||||
|
||||
comm=[[]for i in range (self.uav_num)]
|
||||
w=numpy.ones((self.uav_num,self.uav_num))*0 # 定义邻接矩阵
|
||||
nodes_next=[]
|
||||
node_flag = [self.uav_num-1]
|
||||
|
||||
rel_d=[0]*(self.uav_num-1)
|
||||
# 规定每个无人机可以随机连接三台距离自己最近的无人机,且不能连接在flag中的无人机(即已经判断过连接点的无人机)。
|
||||
# 计算第一层通信(leader):获得离自己最近的三台无人机编号
|
||||
|
||||
for i in range(0,self.uav_num-1):
|
||||
|
||||
rel_d[i]=pow(rel_posi[0][i],2)+pow(rel_posi[1][i],2)+pow(rel_posi[2][i],2)
|
||||
|
||||
min_num_index_list = map(rel_d.index, heapq.nsmallest(c_num, rel_d))
|
||||
min_num_index_list=list(min_num_index_list)
|
||||
#leader 连接的无人机编号:
|
||||
comm[self.uav_num-1]=min_num_index_list
|
||||
|
||||
nodes_next.extend(comm[self.uav_num-1])
|
||||
|
||||
size_=len(node_flag)
|
||||
|
||||
while (nodes_next!=[]) and (size_<(self.uav_num-1)):
|
||||
|
||||
next_node= nodes_next[0]
|
||||
nodes_next=nodes_next[1:]
|
||||
|
||||
rel_d=[0]*(self.uav_num-1)
|
||||
for i in range(0,self.uav_num-1):
|
||||
|
||||
if i==next_node or i in node_flag:
|
||||
|
||||
rel_d[i]=2000 #这个2000是根据相对位置和整个地图的大小决定的,要比最大可能相对距离大
|
||||
else:
|
||||
|
||||
rel_d[i]=pow((rel_posi[0][i]-rel_posi[0][next_node]),2)+pow((rel_posi[1][i]-rel_posi[1][next_node]),2)+pow((rel_posi[2][i]-rel_posi[2][next_node]),2)
|
||||
|
||||
min_num_index_list = map(rel_d.index, heapq.nsmallest(c_num, rel_d))
|
||||
min_num_index_list=list(min_num_index_list)
|
||||
node_flag.append(next_node)
|
||||
|
||||
size_=len(node_flag)
|
||||
|
||||
for j in range(0,c_num):
|
||||
|
||||
if min_num_index_list[j] in node_flag:
|
||||
|
||||
nodes_next=nodes_next
|
||||
|
||||
else:
|
||||
if min_num_index_list[j] in nodes_next:
|
||||
nodes_next=nodes_next
|
||||
else:
|
||||
nodes_next.append(min_num_index_list[j])
|
||||
|
||||
comm[next_node].append(min_num_index_list[j])
|
||||
# comm为每个uav连接其他uav的编号,其中数组的最后一行为leader
|
||||
#print (comm)
|
||||
#leader是拉普拉斯矩阵的第一行,为0
|
||||
#第0号飞机(相对位置矩阵中的第一个位置)为第二行,以此类推
|
||||
|
||||
for i in range (0,self.uav_num):
|
||||
for j in range(0,self.uav_num-1):
|
||||
|
||||
if i==0:
|
||||
if j in comm[self.uav_num-1]:
|
||||
w[j+1][i]=1
|
||||
else:
|
||||
w[j+1][i]=0
|
||||
else:
|
||||
if j in comm[i-1]:
|
||||
w[j+1][i]=1
|
||||
else:
|
||||
w[j+1][i]=0
|
||||
|
||||
L=w #定义拉普拉斯矩阵
|
||||
for i in range (0,self.uav_num):
|
||||
|
||||
L[i][i]=-sum(w[i])
|
||||
|
||||
#print (L)
|
||||
return L
|
||||
|
||||
if __name__ == '__main__':
|
||||
follower = Follower(int(sys.argv[1]),int(sys.argv[2]))
|
||||
follower.loop()
|
|
@ -1,99 +0,0 @@
|
|||
#!/usr/bin/python
|
||||
# -*- coding: UTF-8 -*-
|
||||
import rospy
|
||||
from geometry_msgs.msg import TwistStamped, Twist, Vector3, PoseStamped
|
||||
from std_msgs.msg import String
|
||||
from pyquaternion import Quaternion
|
||||
import time
|
||||
import math
|
||||
import numpy
|
||||
import sys
|
||||
|
||||
class Leader:
|
||||
|
||||
def __init__(self, leader_id, uav_num):
|
||||
self.hover = True
|
||||
self.id = leader_id
|
||||
self.local_pose = PoseStamped()
|
||||
self.local_velocity = TwistStamped()
|
||||
self.cmd_vel_enu = Twist()
|
||||
self.cmd_accel_enu = Vector3()
|
||||
self.follower_num = uav_num - 1
|
||||
self.followers_info = [None]*self.follower_num
|
||||
self.follower_arrived_num = 0
|
||||
self.follower_all_arrived = True
|
||||
self.avoid_vel = Vector3(0,0,0)
|
||||
self.formation_config = 'waiting'
|
||||
self.followers_receive = [False]*self.follower_num
|
||||
self.target_height_recorded = False
|
||||
self.f = 200
|
||||
self.K_vxy = 0.5
|
||||
self.K_vz = 0.5
|
||||
self.K_pz = 0.5
|
||||
self.local_pose_sub = rospy.Subscriber("/uav"+str(self.id)+"/mavros/local_position/pose", PoseStamped , self.local_pose_callback)
|
||||
self.local_velocity_sub = rospy.Subscriber("/uav"+str(self.id)+"/mavros/local_position/velocity_local", TwistStamped , self.local_velocity_callback)
|
||||
self.cmd_vel_sub = rospy.Subscriber("/xtdrone/leader/cmd_vel", Twist, self.cmd_vel_callback)
|
||||
self.avoid_vel_sub = rospy.Subscriber("/xtdrone/uav"+str(self.id)+"/avoid_vel", Vector3, self.avoid_vel_callback)
|
||||
self.formation_switch_sub = rospy.Subscriber("/gcs/formation_switch",String, self.cmd_callback)
|
||||
for i in range(self.follower_num):
|
||||
rospy.Subscriber('/xtdrone/uav'+str(i+1)+'/info',String,self.followers_info_callback,i)
|
||||
self.local_pose_pub = rospy.Publisher("/xtdrone/leader/pose", PoseStamped , queue_size=10)
|
||||
self.formation_switch_pub = rospy.Publisher("/xtdrone/formation_switch",String, queue_size=10)
|
||||
self.accel_enu_pub = rospy.Publisher('/xtdrone/uav'+str(self.id)+'/cmd_accel_enu', Vector3, queue_size=10)
|
||||
|
||||
def local_pose_callback(self, msg):
|
||||
self.local_pose = msg
|
||||
|
||||
def local_velocity_callback(self, msg):
|
||||
self.local_velocity = msg
|
||||
|
||||
def cmd_vel_callback(self, msg):
|
||||
self.cmd_vel_enu = msg
|
||||
if msg.linear.z == 0:
|
||||
self.hover = True
|
||||
else:
|
||||
self.hover = False
|
||||
|
||||
def cmd_callback(self, msg):
|
||||
if not msg.data == '':
|
||||
self.formation_config = msg.data
|
||||
#print("Switch to Formation"+self.formation_config)
|
||||
|
||||
def avoid_vel_callback(self, msg):
|
||||
self.avoid_vel = msg
|
||||
#print('leader: ', self.avoid_vel)
|
||||
|
||||
def followers_info_callback(self, msg, id):
|
||||
self.followers_info[id] = msg.data
|
||||
#print("follower"+str(id)+":"+ msg.data)
|
||||
|
||||
def loop(self):
|
||||
rospy.init_node('leader')
|
||||
rate = rospy.Rate(self.f)
|
||||
while True:
|
||||
#self.cmd_vel_enu = Twist()
|
||||
for i in range(self.follower_num):
|
||||
if self.followers_info[i] == "Received":
|
||||
self.followers_receive[i] = True
|
||||
if self.follower_arrived_num > self.follower_num - 1:
|
||||
self.follower_all_arrived = True
|
||||
if self.follower_all_arrived:
|
||||
self.formation_switch_pub.publish(self.formation_config)
|
||||
if self.formation_config == 'pyramid':
|
||||
if not self.target_height_recorded:
|
||||
target_height = self.local_pose.pose.position.z + 2
|
||||
self.target_height_recorded = True
|
||||
self.cmd_vel_enu.linear.z = self.K_vz * (target_height - self.local_pose.pose.position.z)
|
||||
self.cmd_vel_enu.linear.x += self.avoid_vel.x
|
||||
self.cmd_vel_enu.linear.y += self.avoid_vel.y
|
||||
self.cmd_vel_enu.linear.z += self.avoid_vel.z
|
||||
self.cmd_accel_enu.x = self.K_vxy * (self.cmd_vel_enu.linear.x - self.local_velocity.twist.linear.x)
|
||||
self.cmd_accel_enu.y = self.K_vxy * (self.cmd_vel_enu.linear.y - self.local_velocity.twist.linear.y)
|
||||
self.cmd_accel_enu.y = self.K_vz * (self.cmd_vel_enu.linear.z - self.local_velocity.twist.linear.z)
|
||||
self.accel_enu_pub.publish(self.cmd_accel_enu)
|
||||
self.local_pose_pub.publish(self.local_pose)
|
||||
rate.sleep()
|
||||
|
||||
if __name__ == '__main__':
|
||||
leader = Leader(1,int(sys.argv[1]))
|
||||
leader.loop()
|
|
@ -1,156 +0,0 @@
|
|||
#!/usr/bin/python
|
||||
# -*- coding: UTF-8 -*-
|
||||
import rospy
|
||||
from geometry_msgs.msg import Twist,Pose,PoseStamped,TwistStamped,Point
|
||||
from std_msgs.msg import String
|
||||
import sys
|
||||
Kp = 0.15
|
||||
uav_num = int(sys.argv[1])
|
||||
leader_id = 1
|
||||
vision_pose = [None]*(uav_num+1)
|
||||
relative_pose = [None]*(uav_num+1)
|
||||
follower_vel_enu_pub = [None]*(uav_num+1)
|
||||
relative_pose_pub = [None]*(uav_num+1)
|
||||
follower_cmd_vel = [None]*(uav_num+1)
|
||||
leader_cmd_vel = Twist()
|
||||
|
||||
for i in range(uav_num):
|
||||
vision_pose[i+1]=PoseStamped()
|
||||
relative_pose[i+1]=PoseStamped()
|
||||
follower_cmd_vel[i+1]=Twist()
|
||||
|
||||
# uav_5 is the leader in the formation mission
|
||||
formation=[None]*10
|
||||
|
||||
|
||||
'''2x5 formation 0
|
||||
fflff
|
||||
fffff
|
||||
'''
|
||||
formation_temp = [None]*(uav_num+1)
|
||||
for i in range(uav_num):
|
||||
if i+1 <= (uav_num/2):
|
||||
if (i+1)%2 == 1:
|
||||
formation_temp[i+1] = Point(0,i,0)
|
||||
else:
|
||||
formation_temp[i+1] = Point(0,-i-1,0)
|
||||
else:
|
||||
if (i+1)%2 == 1:
|
||||
formation_temp[i+1] = Point( -2, 1+i-(uav_num/2),0 )
|
||||
else:
|
||||
formation_temp[i+1] = Point( -2, (uav_num/2)-i ,0)
|
||||
formation[0] = formation_temp
|
||||
|
||||
''' 'L' formation 1
|
||||
lffff
|
||||
f
|
||||
f
|
||||
f
|
||||
f f
|
||||
'''
|
||||
formation_temp = [None]*(uav_num+1)
|
||||
for i in range(uav_num):
|
||||
if (i+1)%2 == 1:
|
||||
formation_temp[i+1] = Point(0,i,0)
|
||||
elif i+1 != uav_num:
|
||||
formation_temp[i+1] = Point(-i-1,0,0)
|
||||
else:
|
||||
formation_temp[i+1] = Point(-4,4,0)
|
||||
formation[1] = formation_temp
|
||||
|
||||
''' '△' formation 2
|
||||
l
|
||||
f f
|
||||
f f f
|
||||
f f f f
|
||||
'''
|
||||
formation_temp = [None]*(uav_num+1)
|
||||
formation_temp[1]=Point(0,0,0)
|
||||
formation_temp[2]=Point(-2,-2,0);formation_temp[3]=Point(-2,2,0)
|
||||
formation_temp[4]=Point(-4,-4,0);formation_temp[5]=Point(-4,0,0);formation_temp[7]=Point(-4,4,0)
|
||||
formation_temp[10]=Point(-6,-6,0);formation_temp[8]=Point(-6,-2,0);formation_temp[6]=Point(-6,2,0);formation_temp[9]=Point(-6,6,0)
|
||||
formation[2] = formation_temp
|
||||
|
||||
|
||||
|
||||
|
||||
def leader_cmd_vel_callback(msg):
|
||||
global leader_cmd_vel
|
||||
leader_cmd_vel = msg
|
||||
|
||||
def calculate_relative_pose(uav_id):
|
||||
global relative_pose
|
||||
global vision_pose
|
||||
relative_pose[uav_id].pose.position.x = vision_pose[uav_id].pose.pose.position.x - vision_pose[leader_id].pose.pose.position.x
|
||||
relative_pose[uav_id].pose.position.y = vision_pose[uav_id].pose.pose.position.y - vision_pose[leader_id].pose.pose.position.y
|
||||
relative_pose[uav_id].pose.position.z = vision_pose[uav_id].pose.pose.position.z - vision_pose[leader_id].pose.pose.position.z
|
||||
|
||||
|
||||
formation_id = 0
|
||||
|
||||
'''
|
||||
formation.append( [[-1,-1],[0,-1],[1,-1],[-1,0],[0,0],[0,1]] ) #2x3 formation
|
||||
# f l f
|
||||
# f f f
|
||||
formation.append( [[-2,-2],[0,-2],[2,-2],[-1,-1],[0,0],[1,-1]] )#Trianglar formation
|
||||
# l
|
||||
# f f
|
||||
# f f f
|
||||
formation.append( [[0,-4],[0,-2],[0,-6],[-2,0],[0,0],[2,0]] ) #'T' formation
|
||||
# f l f
|
||||
# f
|
||||
# f
|
||||
# f
|
||||
formation_id = int(sys.argv[2])
|
||||
'''
|
||||
|
||||
def leader_cmd_vel_callback(msg):
|
||||
global leader_cmd_vel
|
||||
leader_cmd_vel = msg
|
||||
|
||||
def calculate_relative_pose(uav_id):
|
||||
global relative_pose
|
||||
relative_pose[uav_id].pose.position.x = vision_pose[uav_id].pose.position.x - vision_pose[leader_id].pose.position.x
|
||||
relative_pose[uav_id].pose.position.y = vision_pose[uav_id].pose.position.y - vision_pose[leader_id].pose.position.y
|
||||
relative_pose[uav_id].pose.position.z = vision_pose[uav_id].pose.position.z - vision_pose[leader_id].pose.position.z
|
||||
|
||||
vision_pose_callback = [None]*(uav_num+1)
|
||||
|
||||
rospy.init_node('formation_control')
|
||||
|
||||
def vision_pose_callback(msg,id):
|
||||
global vision_pose
|
||||
vision_pose[id] = msg
|
||||
calculate_relative_pose(id)
|
||||
|
||||
def cmd_callback(msg):
|
||||
global formation_id
|
||||
if not msg.data == '':
|
||||
formation_id = int(msg.data[-1])
|
||||
print("Switch to Formation"+str(formation_id))
|
||||
|
||||
for i in range(uav_num):
|
||||
uav_id = i+1
|
||||
rospy.Subscriber("/uav"+str(uav_id)+"/mavros/vision_pose/pose", PoseStamped , vision_pose_callback,uav_id)
|
||||
|
||||
leader_cmd_vel_sub = rospy.Subscriber("/xtdrone/leader/cmd_vel", Twist, leader_cmd_vel_callback)
|
||||
formation_switch_sub = rospy.Subscriber("/xtdrone/leader_cmd",String, cmd_callback)
|
||||
leader_vel_enu_pub = rospy.Publisher('/xtdrone/uav'+str(leader_id)+'/cmd_vel_enu', Twist, queue_size=10)
|
||||
for i in range(uav_num):
|
||||
uav_id = i+1
|
||||
if uav_id != leader_id:
|
||||
follower_vel_enu_pub[i+1] = rospy.Publisher(
|
||||
'/xtdrone/uav'+str(i+1)+'/cmd_vel_enu', Twist, queue_size=10)
|
||||
|
||||
rate = rospy.Rate(100)
|
||||
while(1):
|
||||
for i in range(uav_num):
|
||||
uav_id = i+1
|
||||
if uav_id != leader_id:
|
||||
follower_cmd_vel[uav_id].linear.x = (leader_cmd_vel.linear.x+Kp*(formation[formation_id][uav_id].x- relative_pose[uav_id].pose.position.x) )
|
||||
follower_cmd_vel[uav_id].linear.y = (leader_cmd_vel.linear.y+Kp*(formation[formation_id][uav_id].y- relative_pose[uav_id].pose.position.y) )
|
||||
follower_cmd_vel[uav_id].linear.z = leader_cmd_vel.linear.z
|
||||
follower_cmd_vel[uav_id].angular.x = 0.0; follower_cmd_vel[uav_id].angular.y = 0.0; follower_cmd_vel[uav_id].angular.z = 0.0
|
||||
follower_vel_enu_pub[uav_id].publish(follower_cmd_vel[uav_id])
|
||||
leader_vel_enu_pub.publish(leader_cmd_vel)
|
||||
rate.sleep()
|
|
@ -1,207 +0,0 @@
|
|||
#!/usr/bin/python
|
||||
# -*- coding: UTF-8 -*-
|
||||
import rospy
|
||||
from geometry_msgs.msg import Twist,Pose,PoseStamped,TwistStamped,Point
|
||||
from std_msgs.msg import String
|
||||
import sys
|
||||
from pyquaternion import Quaternion
|
||||
import time
|
||||
KP_xy = 1.5
|
||||
KP_yaw = 2.8
|
||||
KP_z = 1
|
||||
max_vel_xy = 1
|
||||
max_vel_z = 0.6
|
||||
uav_num = int(sys.argv[1])
|
||||
leader_id = 1
|
||||
local_pose = [None]*(uav_num+1)
|
||||
relative_pose = [None]*(uav_num+1)
|
||||
follower_vel_enu_pub = [None]*(uav_num+1)
|
||||
relative_pose_pub = [None]*(uav_num+1)
|
||||
follower_cmd_vel = [None]*(uav_num+1)
|
||||
leader_cmd_vel = Twist()
|
||||
avoid_vel_z = [0]*(uav_num+1)
|
||||
avoid_pos_z = 0.3
|
||||
leader_height = 0
|
||||
hover = True
|
||||
avoid = False
|
||||
|
||||
for i in range(uav_num):
|
||||
local_pose[i+1]=PoseStamped()
|
||||
relative_pose[i+1]=PoseStamped()
|
||||
follower_cmd_vel[i+1]=Twist()
|
||||
|
||||
# uav_5 is the leader in the formation mission
|
||||
formation=[None]*10
|
||||
|
||||
|
||||
'''2x5 formation 0
|
||||
fflff
|
||||
fffff
|
||||
'''
|
||||
formation_temp = [None]*(uav_num+1)
|
||||
for i in range(uav_num):
|
||||
if i+1 <= (uav_num/2):
|
||||
if (i+1)%2 == 1:
|
||||
formation_temp[i+1] = Point(0,i,0)
|
||||
else:
|
||||
formation_temp[i+1] = Point(0,-i-1,0)
|
||||
else:
|
||||
if (i+1)%2 == 1:
|
||||
formation_temp[i+1] = Point( -2, 1+i-(uav_num/2),0 )
|
||||
else:
|
||||
formation_temp[i+1] = Point( -2, (uav_num/2)-i ,0)
|
||||
formation[0] = formation_temp
|
||||
|
||||
''' 'L' formation 1
|
||||
lffff
|
||||
f
|
||||
f
|
||||
f
|
||||
f f
|
||||
'''
|
||||
formation_temp = [None]*(uav_num+1)
|
||||
for i in range(uav_num):
|
||||
if (i+1)%2 == 1:
|
||||
formation_temp[i+1] = Point(0,i,0)
|
||||
elif i+1 != uav_num:
|
||||
formation_temp[i+1] = Point(-i-1,0,0)
|
||||
else:
|
||||
formation_temp[i+1] = Point(-4,4,0)
|
||||
formation[1] = formation_temp
|
||||
|
||||
''' '△' formation 2
|
||||
l
|
||||
f f
|
||||
f f f
|
||||
f f f f
|
||||
'''
|
||||
formation_temp = [None]*(uav_num+1)
|
||||
formation_temp[1]=Point(0,0,0)
|
||||
formation_temp[2]=Point(-2,-2,0);formation_temp[3]=Point(-2,2,0)
|
||||
formation_temp[4]=Point(-4,-4,0);formation_temp[5]=Point(-4,0,0);formation_temp[7]=Point(-4,4,0)
|
||||
formation_temp[10]=Point(-6,-6,0);formation_temp[8]=Point(-6,-2,0);formation_temp[6]=Point(-6,2,0);formation_temp[9]=Point(-6,6,0)
|
||||
formation[2] = formation_temp
|
||||
|
||||
|
||||
|
||||
|
||||
def leader_cmd_vel_callback(msg):
|
||||
global leader_cmd_vel, hover
|
||||
leader_cmd_vel = msg
|
||||
#print(hover)
|
||||
if msg.linear.z == 0:
|
||||
hover = True
|
||||
else:
|
||||
hover = False
|
||||
|
||||
def calculate_relative_pose(uav_id):
|
||||
global relative_pose
|
||||
global local_pose
|
||||
relative_pose[uav_id].pose.position.x = local_pose[uav_id].pose.pose.position.x - local_pose[leader_id].pose.pose.position.x
|
||||
relative_pose[uav_id].pose.position.y = local_pose[uav_id].pose.pose.position.y - local_pose[leader_id].pose.pose.position.y
|
||||
relative_pose[uav_id].pose.position.z = local_pose[uav_id].pose.pose.position.z - local_pose[leader_id].pose.pose.position.z
|
||||
|
||||
def delta_vel(target_pos, current_pos, KP, vel_max):
|
||||
delta_vel = KP*(target_pos-current_pos)
|
||||
if delta_vel > vel_max:
|
||||
delta_vel = vel_max
|
||||
return delta_vel
|
||||
|
||||
formation_id = 0
|
||||
|
||||
'''
|
||||
formation.append( [[-1,-1],[0,-1],[1,-1],[-1,0],[0,0],[0,1]] ) #2x3 formation
|
||||
# f l f
|
||||
# f f f
|
||||
formation.append( [[-2,-2],[0,-2],[2,-2],[-1,-1],[0,0],[1,-1]] )#Trianglar formation
|
||||
# l
|
||||
# f f
|
||||
# f f f
|
||||
formation.append( [[0,-4],[0,-2],[0,-6],[-2,0],[0,0],[2,0]] ) #'T' formation
|
||||
# f l f
|
||||
# f
|
||||
# f
|
||||
# f
|
||||
formation_id = int(sys.argv[2])
|
||||
'''
|
||||
|
||||
|
||||
def calculate_relative_pose(uav_id):
|
||||
global relative_pose
|
||||
relative_pose[uav_id].pose.position.x = local_pose[uav_id].pose.position.x - local_pose[leader_id].pose.position.x
|
||||
relative_pose[uav_id].pose.position.y = local_pose[uav_id].pose.position.y - local_pose[leader_id].pose.position.y
|
||||
relative_pose[uav_id].pose.position.z = local_pose[uav_id].pose.position.z - local_pose[leader_id].pose.position.z
|
||||
|
||||
|
||||
|
||||
rospy.init_node('formation_control')
|
||||
|
||||
def local_pose_callback(msg,id):
|
||||
global local_pose
|
||||
local_pose[id] = msg
|
||||
calculate_relative_pose(id)
|
||||
|
||||
def cmd_callback(msg):
|
||||
global formation_id
|
||||
if not msg.data == '':
|
||||
formation_id = int(msg.data[-1])
|
||||
print("Switch to Formation"+str(formation_id))
|
||||
|
||||
for i in range(uav_num):
|
||||
uav_id = i+1
|
||||
rospy.Subscriber("/uav"+str(uav_id)+"/mavros/local_position/pose", PoseStamped , local_pose_callback,uav_id)
|
||||
|
||||
leader_cmd_vel_sub = rospy.Subscriber("/xtdrone/leader/cmd_vel", Twist, leader_cmd_vel_callback)
|
||||
formation_switch_sub = rospy.Subscriber("/xtdrone/leader_cmd",String, cmd_callback)
|
||||
leader_vel_enu_pub = rospy.Publisher('/xtdrone/uav'+str(leader_id)+'/cmd_vel_enu', Twist, queue_size=10)
|
||||
for i in range(uav_num):
|
||||
uav_id = i+1
|
||||
if uav_id != leader_id:
|
||||
follower_vel_enu_pub[i+1] = rospy.Publisher(
|
||||
'/xtdrone/uav'+str(i+1)+'/cmd_vel_enu', Twist, queue_size=10)
|
||||
for i in range(10):
|
||||
leader_height = local_pose[leader_id].pose.position.z
|
||||
time.sleep(0.1)
|
||||
rate = rospy.Rate(100)
|
||||
|
||||
while(1):
|
||||
|
||||
leader_orientation = Quaternion(local_pose[leader_id].pose.orientation.w,local_pose[leader_id].pose.orientation.x,local_pose[leader_id].pose.orientation.y,local_pose[leader_id].pose.orientation.z)
|
||||
leader_yaw = leader_orientation.yaw_pitch_roll[0]
|
||||
# Avoid collision with other drones
|
||||
for i in range(uav_num):
|
||||
uav_id = i+1
|
||||
for j in range(1,uav_num-i):
|
||||
if pow(local_pose[uav_id].pose.position.x-local_pose[uav_id+j].pose.position.x,2)\
|
||||
+pow(local_pose[uav_id].pose.position.y-local_pose[uav_id+j].pose.position.y,2)\
|
||||
+pow(local_pose[uav_id].pose.position.z-local_pose[uav_id+j].pose.position.z,2) < 1:
|
||||
avoid = True
|
||||
avoid_vel_z[uav_id] = KP_z*avoid_pos_z
|
||||
avoid_vel_z[uav_id+j] = -KP_z*avoid_pos_z
|
||||
else:
|
||||
avoid_vel_z[uav_id] = 0
|
||||
avoid_vel_z[uav_id+j] = 0
|
||||
for i in range(uav_num):
|
||||
uav_id = i+1
|
||||
if uav_id != leader_id:
|
||||
follower_cmd_vel[uav_id].linear.x = leader_cmd_vel.linear.x+delta_vel(formation[formation_id][uav_id].x,relative_pose[uav_id].pose.position.x,KP_xy, max_vel_xy)
|
||||
follower_cmd_vel[uav_id].linear.y = leader_cmd_vel.linear.y+delta_vel(formation[formation_id][uav_id].y,relative_pose[uav_id].pose.position.y, KP_xy, max_vel_xy)
|
||||
follower_cmd_vel[uav_id].linear.z = leader_cmd_vel.linear.z + delta_vel(leader_height,local_pose[uav_id].pose.position.z, KP_z, max_vel_z) + avoid_vel_z[uav_id] - avoid_vel_z[leader_id]
|
||||
|
||||
orientation = Quaternion(local_pose[uav_id].pose.orientation.w,local_pose[uav_id].pose.orientation.x,local_pose[uav_id].pose.orientation.y,local_pose[uav_id].pose.orientation.z)
|
||||
yaw = orientation.yaw_pitch_roll[0]
|
||||
follower_cmd_vel[uav_id].angular.x = 0.0; follower_cmd_vel[uav_id].angular.y = 0.0; follower_cmd_vel[uav_id].angular.z = KP_yaw*(leader_yaw - yaw)
|
||||
|
||||
follower_vel_enu_pub[uav_id].publish(follower_cmd_vel[uav_id])
|
||||
|
||||
if hover:
|
||||
leader_cmd_vel.linear.z = delta_vel(leader_height,local_pose[leader_id].pose.position.z, KP_z, max_vel_z) + avoid_vel_z[leader_id]
|
||||
else:
|
||||
leader_cmd_vel.linear.z = leader_cmd_vel.linear.z + avoid_vel_z[leader_id]
|
||||
if not avoid:
|
||||
leader_height = local_pose[leader_id].pose.position.z
|
||||
leader_vel_enu_pub.publish(leader_cmd_vel)
|
||||
|
||||
print(leader_height)
|
||||
|
||||
rate.sleep()
|
|
@ -1,141 +0,0 @@
|
|||
#!/usr/bin/python
|
||||
# -*- coding: UTF-8 -*-
|
||||
import rospy
|
||||
from geometry_msgs.msg import Twist,Pose,PoseStamped,TwistStamped
|
||||
from std_msgs.msg import String
|
||||
import sys
|
||||
from pyquaternion import Quaternion
|
||||
import time
|
||||
KP_xy = 1
|
||||
KP_z = 1.5
|
||||
KP_yaw = 2.8
|
||||
max_vel_xy = 1
|
||||
max_vel_z = 0.6
|
||||
uav_num = int(sys.argv[1])
|
||||
leader_id = 5
|
||||
local_pose = [None]*(uav_num+1)
|
||||
relative_pose = [None]*(uav_num+1)
|
||||
follower_vel_enu_pub = [None]*(uav_num+1)
|
||||
relative_pose_pub = [None]*(uav_num+1)
|
||||
follower_cmd_vel = [None]*(uav_num+1)
|
||||
leader_cmd_vel = Twist()
|
||||
avoid_vel_z = [0]*(uav_num+1)
|
||||
avoid_pos_z = 0.3
|
||||
leader_height = 0
|
||||
hover = True
|
||||
avoid = False
|
||||
|
||||
for i in range(uav_num):
|
||||
local_pose[i+1]=PoseStamped()
|
||||
relative_pose[i+1]=PoseStamped()
|
||||
follower_cmd_vel[i+1]=Twist()
|
||||
|
||||
formation=[]
|
||||
formation.append( [[-1,-1],[0,-1],[1,-1],[-1,0],[0,0],[1,0]] ) #2x3 formation
|
||||
# i i i
|
||||
# i i i
|
||||
formation.append( [[-2,-2],[0,-2],[2,-2],[-1,-1],[0,0],[1,-1]] )#Trianglar formation
|
||||
# i
|
||||
# i i
|
||||
# i i i
|
||||
formation.append( [[0,-4],[0,-2],[0,-6],[-2,0],[0,0],[2,0]] ) #'T' formation
|
||||
# i i i
|
||||
# i
|
||||
# i
|
||||
# i
|
||||
formation_id = 0
|
||||
|
||||
|
||||
def leader_cmd_vel_callback(msg):
|
||||
global leader_cmd_vel, hover
|
||||
leader_cmd_vel = msg
|
||||
if msg.linear.z == 0:
|
||||
hover = True
|
||||
else:
|
||||
hover = False
|
||||
|
||||
def calculate_relative_pose(uav_id):
|
||||
global relative_pose
|
||||
relative_pose[uav_id].pose.position.x = local_pose[uav_id].pose.position.x - local_pose[leader_id].pose.position.x
|
||||
relative_pose[uav_id].pose.position.y = local_pose[uav_id].pose.position.y - local_pose[leader_id].pose.position.y
|
||||
relative_pose[uav_id].pose.position.z = local_pose[uav_id].pose.position.z - local_pose[leader_id].pose.position.z
|
||||
|
||||
def delta_vel(target_pos, current_pos, KP, vel_max):
|
||||
delta_vel = KP*(target_pos-current_pos)
|
||||
if delta_vel > vel_max:
|
||||
delta_vel = vel_max
|
||||
return delta_vel
|
||||
|
||||
local_pose_callback = [None]*(uav_num+1)
|
||||
|
||||
rospy.init_node('formation_control')
|
||||
|
||||
|
||||
def local_pose_callback(msg,id):
|
||||
global local_pose
|
||||
local_pose[id] = msg
|
||||
calculate_relative_pose(id)
|
||||
|
||||
def cmd_callback(msg):
|
||||
global formation_id
|
||||
if not msg.data == '':
|
||||
formation_id = int(msg.data[-1])
|
||||
print("Switch to Formation"+str(formation_id))
|
||||
|
||||
for i in range(uav_num):
|
||||
uav_id = i+1
|
||||
rospy.Subscriber("/uav"+str(uav_id)+"/mavros/local_position/pose", PoseStamped , local_pose_callback,uav_id)
|
||||
|
||||
leader_cmd_vel_sub = rospy.Subscriber("/xtdrone/leader/cmd_vel", Twist, leader_cmd_vel_callback)
|
||||
formation_switch_sub = rospy.Subscriber("/xtdrone/leader_cmd",String, cmd_callback)
|
||||
leader_vel_enu_pub = rospy.Publisher('/xtdrone/uav'+str(leader_id)+'/cmd_vel_enu', Twist, queue_size=10)
|
||||
for i in range(uav_num):
|
||||
uav_id = i+1
|
||||
if uav_id != leader_id:
|
||||
follower_vel_enu_pub[uav_id] = rospy.Publisher(
|
||||
'/xtdrone/uav'+str(uav_id)+'/cmd_vel_enu', Twist, queue_size=10)
|
||||
|
||||
for i in range(10):
|
||||
leader_height = local_pose[leader_id].pose.position.z
|
||||
time.sleep(0.1)
|
||||
rate = rospy.Rate(100)
|
||||
while(1):
|
||||
leader_orientation = Quaternion(local_pose[leader_id].pose.orientation.w,local_pose[leader_id].pose.orientation.x,local_pose[leader_id].pose.orientation.y,local_pose[leader_id].pose.orientation.z)
|
||||
leader_yaw = leader_orientation.yaw_pitch_roll[0]
|
||||
# Avoid collision with other drones
|
||||
for i in range(uav_num):
|
||||
uav_id = i+1
|
||||
for j in range(1,uav_num-i):
|
||||
if pow(local_pose[uav_id].pose.position.x-local_pose[uav_id+j].pose.position.x,2)\
|
||||
+pow(local_pose[uav_id].pose.position.y-local_pose[uav_id+j].pose.position.y,2)\
|
||||
+pow(local_pose[uav_id].pose.position.z-local_pose[uav_id+j].pose.position.z,2) < 0.6:
|
||||
avoid = True
|
||||
avoid_vel_z[uav_id] = KP_z*avoid_pos_z
|
||||
avoid_vel_z[uav_id+j] = -KP_z*avoid_pos_z
|
||||
|
||||
else:
|
||||
avoid_vel_z[uav_id] = 0
|
||||
avoid_vel_z[uav_id+j] = 0
|
||||
avoid = False
|
||||
for i in range(uav_num):
|
||||
uav_id = i+1
|
||||
if uav_id != leader_id:
|
||||
follower_cmd_vel[uav_id].linear.x = leader_cmd_vel.linear.x+delta_vel(formation[formation_id][i][0],relative_pose[uav_id].pose.position.x,KP_xy, max_vel_xy)
|
||||
follower_cmd_vel[uav_id].linear.y = leader_cmd_vel.linear.y+delta_vel(formation[formation_id][i][1],relative_pose[uav_id].pose.position.y, KP_xy, max_vel_xy)
|
||||
follower_cmd_vel[uav_id].linear.z = leader_cmd_vel.linear.z + delta_vel(leader_height,local_pose[uav_id].pose.position.z, KP_z, max_vel_z) + avoid_vel_z[uav_id] - avoid_vel_z[leader_id]
|
||||
|
||||
orientation = Quaternion(local_pose[uav_id].pose.orientation.w,local_pose[uav_id].pose.orientation.x,local_pose[uav_id].pose.orientation.y,local_pose[uav_id].pose.orientation.z)
|
||||
yaw = orientation.yaw_pitch_roll[0]
|
||||
follower_cmd_vel[uav_id].angular.x = 0.0; follower_cmd_vel[uav_id].angular.y = 0.0; follower_cmd_vel[uav_id].angular.z = KP_yaw*(leader_yaw - yaw)
|
||||
|
||||
follower_vel_enu_pub[uav_id].publish(follower_cmd_vel[uav_id])
|
||||
|
||||
if hover:
|
||||
leader_cmd_vel.linear.z = delta_vel(leader_height,local_pose[leader_id].pose.position.z, KP_z, max_vel_z) + avoid_vel_z[leader_id]
|
||||
else:
|
||||
leader_cmd_vel.linear.z = leader_cmd_vel.linear.z + avoid_vel_z[leader_id]
|
||||
if not avoid:
|
||||
leader_height = local_pose[leader_id].pose.position.z
|
||||
leader_vel_enu_pub.publish(leader_cmd_vel)
|
||||
|
||||
rate.sleep()
|
|
@ -1,315 +0,0 @@
|
|||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- MAVROS posix SITL environment launch script -->
|
||||
<!-- launches Gazebo environment and 2x: MAVROS, PX4 SITL, and spawns vehicle -->
|
||||
<!-- vehicle model and world -->
|
||||
<arg name="est" default="ekf2"/>
|
||||
<arg name="vehicle" default="iris"/>
|
||||
<arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
|
||||
<!-- gazebo configs -->
|
||||
<arg name="gui" default="true"/>
|
||||
<arg name="debug" default="false"/>
|
||||
<arg name="verbose" default="false"/>
|
||||
<arg name="paused" default="false"/>
|
||||
<!-- Gazebo sim -->
|
||||
<include file="$(find gazebo_ros)/launch/empty_world.launch">
|
||||
<arg name="gui" value="$(arg gui)"/>
|
||||
<arg name="world_name" value="$(arg world)"/>
|
||||
<arg name="debug" value="$(arg debug)"/>
|
||||
<arg name="verbose" value="$(arg verbose)"/>
|
||||
<arg name="paused" value="$(arg paused)"/>
|
||||
</include>
|
||||
<!--<node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/xacro.rviz" required="true" />-->
|
||||
|
||||
|
||||
<!-- UAV1-->
|
||||
<group ns="uav1">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="1"/>
|
||||
<!--default="14540@localhost:14557"/>-->
|
||||
|
||||
<arg name="fcu_url" default="udp://:14540@127.0.0.1:34572"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="0"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_1"/>
|
||||
<arg name="mavlink_udp_port" value="24560"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(arg ID)"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- UAV2-->
|
||||
<group ns="uav2">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="2"/>
|
||||
<!--default="14540@localhost:14557"/>-->
|
||||
|
||||
<arg name="fcu_url" default="udp://:14541@127.0.0.1:34574"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="-1"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_2"/>
|
||||
<arg name="mavlink_udp_port" value="24562"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(arg ID)"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- UAV3-->
|
||||
<group ns="uav3">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="3"/>
|
||||
<!--default="14540@localhost:14557"/>-->
|
||||
|
||||
<arg name="fcu_url" default="udp://:14542@127.0.0.1:34576"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="2"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_3"/>
|
||||
<arg name="mavlink_udp_port" value="24564"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(arg ID)"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- UAV4-->
|
||||
<group ns="uav4">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="4"/>
|
||||
<!--default="14540@localhost:14557"/>-->
|
||||
|
||||
<arg name="fcu_url" default="udp://:14543@127.0.0.1:34578"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="-3"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_4"/>
|
||||
<arg name="mavlink_udp_port" value="24566"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(arg ID)"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- UAV5-->
|
||||
<group ns="uav5">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="5"/>
|
||||
<!--default="14540@localhost:14557"/>-->
|
||||
|
||||
<arg name="fcu_url" default="udp://:14544@127.0.0.1:34580"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="4"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_5"/>
|
||||
<arg name="mavlink_udp_port" value="24568"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(arg ID)"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- UAV6-->
|
||||
<group ns="uav6">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="6"/>
|
||||
<!--default="14540@localhost:14557"/>-->
|
||||
|
||||
<arg name="fcu_url" default="udp://:14545@127.0.0.1:34582"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="-5"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_6"/>
|
||||
<arg name="mavlink_udp_port" value="24570"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(arg ID)"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- UAV7-->
|
||||
<group ns="uav7">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="7"/>
|
||||
<!--default="14540@localhost:14557"/>-->
|
||||
|
||||
<arg name="fcu_url" default="udp://:14546@127.0.0.1:34584"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="6"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_7"/>
|
||||
<arg name="mavlink_udp_port" value="24572"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(arg ID)"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- UAV8-->
|
||||
<group ns="uav8">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="8"/>
|
||||
<!--default="14540@localhost:14557"/>-->
|
||||
|
||||
<arg name="fcu_url" default="udp://:14547@127.0.0.1:34586"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="-7"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_8"/>
|
||||
<arg name="mavlink_udp_port" value="24574"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(arg ID)"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- UAV9-->
|
||||
<group ns="uav9">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="9"/>
|
||||
<!--default="14540@localhost:14557"/>-->
|
||||
|
||||
<arg name="fcu_url" default="udp://:14548@127.0.0.1:34588"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="8"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_9"/>
|
||||
<arg name="mavlink_udp_port" value="24576"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(arg ID)"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- UAV10-->
|
||||
<group ns="uav10">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="10"/>
|
||||
<!--default="14540@localhost:14557"/>-->
|
||||
|
||||
<arg name="fcu_url" default="udp://:14549@127.0.0.1:34590"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="-9"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_10"/>
|
||||
<arg name="mavlink_udp_port" value="24578"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(arg ID)"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
</launch>
|
|
@ -1,315 +0,0 @@
|
|||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- MAVROS posix SITL environment launch script -->
|
||||
<!-- launches Gazebo environment and 2x: MAVROS, PX4 SITL, and spawns vehicle -->
|
||||
<!-- vehicle model and world -->
|
||||
<arg name="est" default="ekf2"/>
|
||||
<arg name="vehicle" default="iris"/>
|
||||
<arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
|
||||
<!-- gazebo configs -->
|
||||
<arg name="gui" default="true"/>
|
||||
<arg name="debug" default="false"/>
|
||||
<arg name="verbose" default="false"/>
|
||||
<arg name="paused" default="false"/>
|
||||
<!-- Gazebo sim -->
|
||||
<include file="$(find gazebo_ros)/launch/empty_world.launch">
|
||||
<arg name="gui" value="$(arg gui)"/>
|
||||
<arg name="world_name" value="$(arg world)"/>
|
||||
<arg name="debug" value="$(arg debug)"/>
|
||||
<arg name="verbose" value="$(arg verbose)"/>
|
||||
<arg name="paused" value="$(arg paused)"/>
|
||||
</include>
|
||||
<!--<node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/xacro.rviz" required="true" />-->
|
||||
|
||||
|
||||
<!-- UAV1-->
|
||||
<group ns="uav1">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="1"/>
|
||||
<!--default="14540@localhost:14557"/>-->
|
||||
|
||||
<arg name="fcu_url" default="udp://:14540@127.0.0.1:34572"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="0"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_1"/>
|
||||
<arg name="mavlink_udp_port" value="24560"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(arg ID)"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- UAV2-->
|
||||
<group ns="uav2">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="2"/>
|
||||
<!--default="14540@localhost:14557"/>-->
|
||||
|
||||
<arg name="fcu_url" default="udp://:14541@127.0.0.1:34574"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="-1"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_2"/>
|
||||
<arg name="mavlink_udp_port" value="24562"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(arg ID)"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- UAV3-->
|
||||
<group ns="uav3">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="3"/>
|
||||
<!--default="14540@localhost:14557"/>-->
|
||||
|
||||
<arg name="fcu_url" default="udp://:14542@127.0.0.1:34576"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="2"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_3"/>
|
||||
<arg name="mavlink_udp_port" value="24564"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(arg ID)"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- UAV4-->
|
||||
<group ns="uav4">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="4"/>
|
||||
<!--default="14540@localhost:14557"/>-->
|
||||
|
||||
<arg name="fcu_url" default="udp://:14543@127.0.0.1:34578"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="-3"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_4"/>
|
||||
<arg name="mavlink_udp_port" value="24566"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(arg ID)"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- UAV5-->
|
||||
<group ns="uav5">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="5"/>
|
||||
<!--default="14540@localhost:14557"/>-->
|
||||
|
||||
<arg name="fcu_url" default="udp://:14544@127.0.0.1:34580"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="4"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_5"/>
|
||||
<arg name="mavlink_udp_port" value="24568"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(arg ID)"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- UAV6-->
|
||||
<group ns="uav6">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="6"/>
|
||||
<!--default="14540@localhost:14557"/>-->
|
||||
|
||||
<arg name="fcu_url" default="udp://:14545@127.0.0.1:34582"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="-5"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_6"/>
|
||||
<arg name="mavlink_udp_port" value="24570"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(arg ID)"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- UAV7-->
|
||||
<group ns="uav7">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="7"/>
|
||||
<!--default="14540@localhost:14557"/>-->
|
||||
|
||||
<arg name="fcu_url" default="udp://:14546@127.0.0.1:34584"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="6"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_7"/>
|
||||
<arg name="mavlink_udp_port" value="24572"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(arg ID)"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- UAV8-->
|
||||
<group ns="uav8">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="8"/>
|
||||
<!--default="14540@localhost:14557"/>-->
|
||||
|
||||
<arg name="fcu_url" default="udp://:14547@127.0.0.1:34586"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="-7"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_8"/>
|
||||
<arg name="mavlink_udp_port" value="24574"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(arg ID)"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- UAV9-->
|
||||
<group ns="uav9">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="9"/>
|
||||
<!--default="14540@localhost:14557"/>-->
|
||||
|
||||
<arg name="fcu_url" default="udp://:14548@127.0.0.1:34588"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="8"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_9"/>
|
||||
<arg name="mavlink_udp_port" value="24576"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(arg ID)"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- UAV10-->
|
||||
<group ns="uav10">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="10"/>
|
||||
<!--default="14540@localhost:14557"/>-->
|
||||
|
||||
<arg name="fcu_url" default="udp://:14549@127.0.0.1:34590"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="-9"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_10"/>
|
||||
<arg name="mavlink_udp_port" value="24578"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(arg ID)"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
</launch>
|
|
@ -1,199 +0,0 @@
|
|||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- MAVROS posix SITL environment launch script -->
|
||||
<!-- launches Gazebo environment and 2x: MAVROS, PX4 SITL, and spawns vehicle -->
|
||||
<!-- vehicle model and world -->
|
||||
<arg name="est" default="ekf2"/>
|
||||
<arg name="vehicle" default="iris"/>
|
||||
<arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
|
||||
<!-- gazebo configs -->
|
||||
<arg name="gui" default="true"/>
|
||||
<arg name="debug" default="false"/>
|
||||
<arg name="verbose" default="false"/>
|
||||
<arg name="paused" default="false"/>
|
||||
<!-- Gazebo sim -->
|
||||
<include file="$(find gazebo_ros)/launch/empty_world.launch">
|
||||
<arg name="gui" value="$(arg gui)"/>
|
||||
<arg name="world_name" value="$(arg world)"/>
|
||||
<arg name="debug" value="$(arg debug)"/>
|
||||
<arg name="verbose" value="$(arg verbose)"/>
|
||||
<arg name="paused" value="$(arg paused)"/>
|
||||
</include>
|
||||
<!--<node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/xacro.rviz" required="true" />-->
|
||||
|
||||
|
||||
<!-- UAV1-->
|
||||
<group ns="uav1">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="1"/>
|
||||
<!--default="14540@localhost:14557"/>-->
|
||||
|
||||
<arg name="fcu_url" default="udp://:14540@127.0.0.1:34572"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="0"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_1"/>
|
||||
<arg name="mavlink_udp_port" value="24560"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(arg ID)"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- UAV2-->
|
||||
<group ns="uav2">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="2"/>
|
||||
<!--default="14540@localhost:14557"/>-->
|
||||
|
||||
<arg name="fcu_url" default="udp://:14541@127.0.0.1:34574"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="-1"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_2"/>
|
||||
<arg name="mavlink_udp_port" value="24562"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(arg ID)"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- UAV3-->
|
||||
<group ns="uav3">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="3"/>
|
||||
<!--default="14540@localhost:14557"/>-->
|
||||
|
||||
<arg name="fcu_url" default="udp://:14542@127.0.0.1:34576"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="2"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_3"/>
|
||||
<arg name="mavlink_udp_port" value="24564"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(arg ID)"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- UAV4-->
|
||||
<group ns="uav4">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="4"/>
|
||||
<!--default="14540@localhost:14557"/>-->
|
||||
|
||||
<arg name="fcu_url" default="udp://:14543@127.0.0.1:34578"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="-3"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_4"/>
|
||||
<arg name="mavlink_udp_port" value="24566"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(arg ID)"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- UAV5-->
|
||||
<group ns="uav5">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="5"/>
|
||||
<!--default="14540@localhost:14557"/>-->
|
||||
|
||||
<arg name="fcu_url" default="udp://:14544@127.0.0.1:34580"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="4"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_5"/>
|
||||
<arg name="mavlink_udp_port" value="24568"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(arg ID)"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- UAV6-->
|
||||
<group ns="uav6">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="6"/>
|
||||
<!--default="14540@localhost:14557"/>-->
|
||||
|
||||
<arg name="fcu_url" default="udp://:14545@127.0.0.1:34582"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="-5"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="rcS" value="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)_6"/>
|
||||
<arg name="mavlink_udp_port" value="24570"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="tgt_system" value="$(arg ID)"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
</launch>
|
|
@ -1,65 +0,0 @@
|
|||
import matplotlib.pyplot as plt
|
||||
from mpl_toolkits.mplot3d import Axes3D
|
||||
import matplotlib.animation as animation
|
||||
import numpy as np
|
||||
import rospy
|
||||
from geometry_msgs.msg import Twist,Pose,PoseStamped,TwistStamped,Vector3
|
||||
from gazebo_msgs.srv import GetModelState
|
||||
import sys
|
||||
from std_msgs.msg import String
|
||||
from copy import deepcopy
|
||||
|
||||
|
||||
uav_num = int(sys.argv[1])
|
||||
|
||||
step_time=0.01
|
||||
Kp=100
|
||||
|
||||
pose_puber=[None]*uav_num
|
||||
vel_puber=[None]*uav_num
|
||||
|
||||
plot_x=[0]*uav_num
|
||||
plot_y=[0]*uav_num
|
||||
plot_z=[0]*uav_num
|
||||
local_vel = [TwistStamped() for i in range (uav_num)]
|
||||
cmd_vel=[Twist() for i in range (uav_num)]
|
||||
arrive_count=0
|
||||
|
||||
for i in range(uav_num):
|
||||
uav_id = i + 1
|
||||
plot_x[i]= i//3
|
||||
plot_y[i]= i%3
|
||||
pose_puber[i]=rospy.Publisher('/uav'+str(uav_id)+'/mavros/local_position/pose', PoseStamped, queue_size=10)
|
||||
vel_puber[i]=rospy.Publisher('/uav'+str(uav_id)+'/mavros/local_position/velocity_local', TwistStamped, queue_size=10)
|
||||
|
||||
|
||||
|
||||
def cmd_accel_callback(msg,id):
|
||||
cmd_vel[id].linear.x = cmd_vel[id].linear.x + Kp * msg.x * step_time
|
||||
cmd_vel[id].linear.y = cmd_vel[id].linear.y + Kp * msg.y * step_time
|
||||
cmd_vel[id].linear.z = cmd_vel[id].linear.z + Kp * msg.z * step_time
|
||||
|
||||
plot_x[id]+=step_time*cmd_vel[id].linear.x
|
||||
plot_y[id]+=step_time*cmd_vel[id].linear.y
|
||||
plot_z[id]+=step_time*cmd_vel[id].linear.z
|
||||
if id == 0:
|
||||
print(id)
|
||||
local_vel[id].twist=deepcopy(cmd_vel[id])
|
||||
|
||||
|
||||
rospy.init_node('simple_3d_simulator')
|
||||
rate = rospy.Rate(1/step_time)
|
||||
|
||||
for i in range(uav_num):
|
||||
uav_id = i + 1
|
||||
rospy.Subscriber('/xtdrone/uav'+str(uav_id)+'/cmd_accel_enu',Vector3, cmd_accel_callback,i)
|
||||
|
||||
while not rospy.is_shutdown():
|
||||
|
||||
for i in range(uav_num):
|
||||
local_pose=PoseStamped()
|
||||
local_pose.pose.position.x=plot_x[i]
|
||||
local_pose.pose.position.y=plot_y[i]
|
||||
local_pose.pose.position.z=plot_z[i]
|
||||
pose_puber[i].publish(local_pose)
|
||||
vel_puber[i].publish(local_vel[i])
|
|
@ -1,88 +0,0 @@
|
|||
import matplotlib.pyplot as plt
|
||||
from mpl_toolkits.mplot3d import Axes3D
|
||||
import matplotlib.animation as animation
|
||||
import numpy as np
|
||||
import rospy
|
||||
from geometry_msgs.msg import Twist,Pose,PoseStamped,TwistStamped
|
||||
from gazebo_msgs.srv import GetModelState
|
||||
import sys
|
||||
from formation_dict import formation_dict_6
|
||||
|
||||
use_1_8 = 1
|
||||
|
||||
|
||||
#uav_num = int(sys.argv[1])
|
||||
uav_num = 6
|
||||
step_time=0.01
|
||||
|
||||
pose_puber=[None]*uav_num
|
||||
vel_puber=[None]*uav_num
|
||||
|
||||
plot_x=[0]*(uav_num)
|
||||
plot_y=[0]*(uav_num)
|
||||
plot_z=[0]*(uav_num)
|
||||
local_vel = [TwistStamped()]*(uav_num)
|
||||
|
||||
for i in range(uav_num-1):
|
||||
uav_id=i+use_1_8
|
||||
plot_x[i]= formation_dict_6['diamond'][0][i]
|
||||
plot_y[i]= formation_dict_6['diamond'][1][i]
|
||||
plot_z[i]= formation_dict_6['diamond'][2][i]
|
||||
pose_puber[i]=rospy.Publisher('/uav'+str(uav_id)+'/mavros/local_position/pose', PoseStamped, queue_size=10)
|
||||
vel_puber[i]=rospy.Publisher('/uav'+str(uav_id)+'/mavros/local_position/velocity_local', TwistStamped, queue_size=10)
|
||||
pose_puber[uav_num-1]=rospy.Publisher('/uav'+str(uav_id)+'/mavros/local_position/pose', PoseStamped, queue_size=10)
|
||||
vel_puber[uav_num-1]=rospy.Publisher('/uav'+str(uav_id)+'/mavros/local_position/velocity_local', TwistStamped, queue_size=10)
|
||||
fig = plt.figure()
|
||||
plt.ion()
|
||||
ax = Axes3D(fig)
|
||||
label_lim = 20
|
||||
|
||||
def scroll_call_back(event):
|
||||
global label_lim
|
||||
if event.button == 'up':
|
||||
label_lim+=2
|
||||
#print('up')
|
||||
elif event.button == 'down':
|
||||
label_lim=label_lim-2 if label_lim>1 else 1
|
||||
#print('down')
|
||||
|
||||
|
||||
fig.canvas.mpl_connect('scroll_event', scroll_call_back)
|
||||
|
||||
def init():
|
||||
ax.set_xlim3d(-label_lim, label_lim)
|
||||
ax.set_ylim3d(-label_lim, label_lim)
|
||||
ax.set_zlim3d(-label_lim, label_lim)
|
||||
|
||||
|
||||
def cmd_vel_callback(msg,id):
|
||||
local_vel[id].twist=msg
|
||||
plot_x[id]+=step_time*local_vel[id].twist.linear.x
|
||||
plot_y[id]+=step_time*local_vel[id].twist.linear.y
|
||||
plot_z[id]+=step_time*local_vel[id].twist.linear.z
|
||||
|
||||
|
||||
rospy.init_node('simple_3d_simulator')
|
||||
rate = rospy.Rate(1/step_time)
|
||||
|
||||
for i in range(uav_num):
|
||||
uav_id=i+use_1_8
|
||||
rospy.Subscriber('/xtdrone/uav'+str(uav_id)+'/cmd_vel_flu', Twist, cmd_vel_callback,i)
|
||||
rospy.Subscriber('/xtdrone/uav'+str(uav_id)+'/cmd_vel_enu', Twist, cmd_vel_callback,i)
|
||||
|
||||
try:
|
||||
while not rospy.is_shutdown():
|
||||
for i in range(uav_num):
|
||||
local_pose=PoseStamped()
|
||||
local_pose.pose.position.x=plot_x[i]
|
||||
local_pose.pose.position.y=plot_y[i]
|
||||
local_pose.pose.position.z=plot_z[i]
|
||||
pose_puber[i].publish(local_pose)
|
||||
vel_puber[i].publish(local_vel[i])
|
||||
init()
|
||||
ax.scatter(plot_x,plot_y,plot_z,marker="o")
|
||||
plt.pause(step_time)
|
||||
ax.cla()
|
||||
rate.sleep()
|
||||
except KeyboardInterrupt:
|
||||
plt.ioff()
|
Loading…
Reference in New Issue