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