forked from xtdrone/XTDrone
完善编队
This commit is contained in:
parent
2ea9790f37
commit
2a03e671c4
|
|
@ -1,5 +1,5 @@
|
|||
#!/bin/bash
|
||||
iris_num=9
|
||||
iris_num=6
|
||||
typhoon_h480_num=0
|
||||
solo_num=0
|
||||
plane_num=0
|
||||
|
|
|
|||
|
|
@ -119,7 +119,7 @@ class Communication:
|
|||
self.arm_state = not self.disarm()
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+": Armed "+str(self.arm_state))
|
||||
|
||||
elif msg.data == 'takeoff' or msg.data == 'land' or msg.data == 'loiter' or msg.data == 'idle':
|
||||
elif msg.data in ['takeoff', 'land', 'loiter', 'idle']:
|
||||
self.mission = msg.data
|
||||
print(self.mission)
|
||||
else:
|
||||
|
|
|
|||
|
|
@ -206,7 +206,7 @@ class Communication:
|
|||
print(self.vehicle_type+'_'+self.vehicle_id+':'+msg.data)
|
||||
print(self.transition(state=4))
|
||||
|
||||
elif msg.data == 'loiter' or msg.data == 'idle':
|
||||
elif msg.data in ['loiter', 'idle']:
|
||||
self.plane_mission = msg.data
|
||||
print(self.vehicle_type+'_'+self.vehicle_id+':'+self.plane_mission)
|
||||
|
||||
|
|
|
|||
|
|
@ -89,6 +89,11 @@ if __name__=="__main__":
|
|||
multirotor_num = int(sys.argv[2])
|
||||
control_type = sys.argv[3]
|
||||
|
||||
if multirotor_num == 9:
|
||||
formation_configs = ['waiting', 'cube', 'pyramid', 'triangle']
|
||||
if multirotor_num == 6:
|
||||
formation_configs = ['waiting', 'T', 'diamond', 'triangle']
|
||||
|
||||
cmd= String()
|
||||
twist = Twist()
|
||||
|
||||
|
|
@ -101,7 +106,7 @@ if __name__=="__main__":
|
|||
multi_cmd_vel_flu_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd_vel_flu', Twist, queue_size=10)
|
||||
multi_cmd_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd',String,queue_size=10)
|
||||
leader_cmd_vel_flu_pub = rospy.Publisher("/xtdrone/leader/cmd_vel_flu", Twist, queue_size=10)
|
||||
leader_cmd_pub = rospy.Publisher("/xtdrone/leader_cmd", String, queue_size=10)
|
||||
leader_cmd_pub = rospy.Publisher("/xtdrone/leader/cmd", String, queue_size=10)
|
||||
|
||||
else:
|
||||
multi_cmd_accel_flu_pub = [None]*multirotor_num
|
||||
|
|
@ -110,7 +115,7 @@ if __name__=="__main__":
|
|||
multi_cmd_accel_flu_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd_accel_flu', Twist, queue_size=10)
|
||||
multi_cmd_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd',String,queue_size=10)
|
||||
leader_cmd_accel_flu_pub = rospy.Publisher("/xtdrone/leader/cmd_accel_flu", Twist, queue_size=10)
|
||||
leader_cmd_pub = rospy.Publisher("/xtdrone/leader_cmd", String, queue_size=10)
|
||||
leader_cmd_pub = rospy.Publisher("/xtdrone/leader/cmd", String, queue_size=10)
|
||||
|
||||
forward = 0.0
|
||||
leftward = 0.0
|
||||
|
|
@ -207,7 +212,7 @@ if __name__=="__main__":
|
|||
elif key == 'g':
|
||||
ctrl_leader = not ctrl_leader
|
||||
print_msg()
|
||||
elif key == 'k' or key == 's':
|
||||
elif key in ['k', 's']:
|
||||
cmd_vel_mask = False
|
||||
forward = 0.0
|
||||
leftward = 0.0
|
||||
|
|
@ -220,7 +225,7 @@ if __name__=="__main__":
|
|||
else:
|
||||
for i in range(10):
|
||||
if key == str(i):
|
||||
cmd = 'mission'+key
|
||||
cmd = formation_configs[i]
|
||||
print_msg()
|
||||
print(cmd)
|
||||
cmd_vel_mask = True
|
||||
|
|
|
|||
|
|
@ -6,30 +6,31 @@ import math
|
|||
import numpy
|
||||
import sys
|
||||
|
||||
uav_num = int(sys.argv[1])
|
||||
pose = [None]*uav_num
|
||||
pose_sub = [None]*uav_num
|
||||
avoid_accel_pub = [None]*uav_num
|
||||
vehicle_type = sys.argv[1]
|
||||
vehicle_num = int(sys.argv[2])
|
||||
pose = [None]*vehicle_num
|
||||
pose_sub = [None]*vehicle_num
|
||||
avoid_accel_pub = [None]*vehicle_num
|
||||
avoid_kp = 0.2
|
||||
avoid_radius = 1
|
||||
aid_vec1 = [1, 0, 0]
|
||||
aid_vec2 = [0, 1, 0]
|
||||
uavs_avoid_accel = [Vector3()] * uav_num
|
||||
uavs_avoid_accel = [Vector3()] * vehicle_num
|
||||
|
||||
def pose_callback(msg, id):
|
||||
pose[id] = msg
|
||||
|
||||
rospy.init_node('avoid')
|
||||
for i in range(uav_num):
|
||||
pose_sub[i] = rospy.Subscriber('/uav'+str(i+1)+'/mavros/local_position/pose',PoseStamped,pose_callback,i)
|
||||
avoid_accel_pub[i] = rospy.Publisher("/xtdrone/uav"+str(i+1)+"/avoid_accel", Vector3,queue_size=10)
|
||||
for i in range(vehicle_num):
|
||||
pose_sub[i] = rospy.Subscriber(vehicle_type+'_'+str(i)+'/mavros/local_position/pose',PoseStamped,pose_callback,i)
|
||||
avoid_accel_pub[i] = rospy.Publisher("/xtdrone/"+vehicle_type+'_'+str(i+1)+"/avoid_accel", Vector3,queue_size=10)
|
||||
|
||||
time.sleep(1)
|
||||
rate = rospy.Rate(50)
|
||||
while not rospy.is_shutdown():
|
||||
for i in range(uav_num):
|
||||
for i in range(vehicle_num):
|
||||
position1 = numpy.array([pose[i].pose.position.x, pose[i].pose.position.y, pose[i].pose.position.z])
|
||||
for j in range(1, uav_num-i):
|
||||
for j in range(1, vehicle_num-i):
|
||||
position2 = numpy.array([pose[i+j].pose.position.x, pose[i+j].pose.position.y, pose[i+j].pose.position.z])
|
||||
dir_vec = position1-position2
|
||||
if numpy.linalg.norm(dir_vec) < avoid_radius:
|
||||
|
|
@ -41,9 +42,9 @@ while not rospy.is_shutdown():
|
|||
avoid_accel = avoid_kp * numpy.cross(dir_vec, aid_vec2)/numpy.linalg.norm(numpy.cross(dir_vec, aid_vec2))
|
||||
uavs_avoid_accel[i] = Vector3(uavs_avoid_accel[i].x+avoid_accel[0],uavs_avoid_accel[i].y+avoid_accel[1],uavs_avoid_accel[i].z+avoid_accel[2])
|
||||
uavs_avoid_accel[i+j] = Vector3(uavs_avoid_accel[i+j].x-avoid_accel[0],uavs_avoid_accel[i+j].y-avoid_accel[1],uavs_avoid_accel[i+j].z-avoid_accel[2])
|
||||
for i in range(uav_num):
|
||||
for i in range(vehicle_num):
|
||||
avoid_accel_pub[i].publish(uavs_avoid_accel[i])
|
||||
uavs_avoid_accel = [Vector3()] * uav_num
|
||||
uavs_avoid_accel = [Vector3()] * vehicle_num
|
||||
rate.sleep()
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -4,11 +4,14 @@ 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_9
|
||||
import sys
|
||||
if sys.argv[3] == '6':
|
||||
from formation_dict import formation_dict_6 as formation_dict
|
||||
elif sys.argv[3] == '9':
|
||||
from formation_dict import formation_dict_9 as formation_dict
|
||||
import time
|
||||
import math
|
||||
import numpy
|
||||
import sys
|
||||
import heapq
|
||||
import copy
|
||||
import Queue
|
||||
|
|
@ -16,9 +19,10 @@ from itertools import permutations
|
|||
|
||||
class Follower:
|
||||
|
||||
def __init__(self, uav_id, uav_num):
|
||||
def __init__(self, uav_type, uav_id, uav_num):
|
||||
self.hover = "HOVER"
|
||||
self.offboard = "OFFBOARD"
|
||||
self.uav_type = uav_type
|
||||
self.id = uav_id
|
||||
self.uav_num = uav_num
|
||||
self.f = 100
|
||||
|
|
@ -35,7 +39,7 @@ class Follower:
|
|||
self.following_ids = []
|
||||
self.formation_config = 'waiting'
|
||||
self.following_count = 0
|
||||
self.Kp = 100 #100
|
||||
self.Kp = 1000 #100
|
||||
#self.kr = (4/int((self.uav_num-1)/2))**0.5
|
||||
self.kr = 1
|
||||
self.velxy_max = 2
|
||||
|
|
@ -45,13 +49,13 @@ class Follower:
|
|||
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.avoid_accel_sub = rospy.Subscriber("/xtdrone/uav"+str(self.id)+"/avoid_accel", Vector3, self.avoid_accel_callback)
|
||||
self.local_pose_sub = rospy.Subscriber(self.uav_type+'_'+str(self.id)+"/mavros/local_position/pose", PoseStamped, self.local_pose_callback)
|
||||
self.local_velocity_sub = rospy.Subscriber(self.uav_type+'_'+str(self.id)+"/mavros/local_position/velocity_local", TwistStamped, self.local_velocity_callback)
|
||||
self.avoid_accel_sub = rospy.Subscriber("/xtdrone/"+self.uav_type+'_'+str(self.id)+"/avoid_accel", Vector3, self.avoid_accel_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)
|
||||
self.cmd_pub = rospy.Publisher('/xtdrone/uav'+str(self.id)+'/cmd',String,queue_size=10)
|
||||
self.vel_enu_pub = rospy.Publisher('/xtdrone/'+self.uav_type+'_'+str(self.id)+'/cmd_vel_enu', Twist, queue_size=10)
|
||||
self.info_pub = rospy.Publisher('/xtdrone/'+self.uav_type+'_'+str(self.id)+'/info', String, queue_size=10)
|
||||
self.cmd_pub = rospy.Publisher('/xtdrone/'+self.uav_type+'_'+str(self.id)+'/cmd',String,queue_size=10)
|
||||
self.first_formation = True
|
||||
self.orig_formation = None
|
||||
self.new_formation = None
|
||||
|
|
@ -112,15 +116,15 @@ class Follower:
|
|||
self.info_pub.publish("Received")
|
||||
print("Follower"+str(self.id-1)+": Switch to Formation "+self.formation_config)
|
||||
if self.formation_config=='waiting':
|
||||
self.L_matrix = self.get_L_matrix(formation_dict_9[self.formation_config])
|
||||
self.L_matrix = self.get_L_matrix(formation_dict[self.formation_config])
|
||||
else:
|
||||
if self.first_formation:
|
||||
self.first_formation=False
|
||||
self.orig_formation=formation_dict_9[self.formation_config]
|
||||
self.L_matrix = self.get_L_matrix(formation_dict_9[self.formation_config])
|
||||
self.orig_formation=formation_dict[self.formation_config]
|
||||
self.L_matrix = self.get_L_matrix(formation_dict[self.formation_config])
|
||||
else:
|
||||
#self.new_formation=self.get_new_formation(self.orig_formation,formation_dict_9[self.formation_config])
|
||||
self.adj_matrix = self.build_graph(self.orig_formation,formation_dict_9[self.formation_config])
|
||||
#self.new_formation=self.get_new_formation(self.orig_formation,formation_dict[self.formation_config])
|
||||
self.adj_matrix = self.build_graph(self.orig_formation,formation_dict[self.formation_config])
|
||||
self.label_left = numpy.max(self.adj_matrix, axis=1) # init label for the left
|
||||
self.label_right = numpy.array([0]*(self.uav_num-1)) # init label for the right set
|
||||
|
||||
|
|
@ -129,15 +133,15 @@ class Follower:
|
|||
self.visit_right = numpy.array([0]*(self.uav_num-1))
|
||||
self.slack_right = numpy.array([100]*(self.uav_num-1))
|
||||
self.change_id = self.KM()
|
||||
self.new_formation=self.get_new_formation(self.change_id,formation_dict_9[self.formation_config])
|
||||
self.new_formation=self.get_new_formation(self.change_id,formation_dict[self.formation_config])
|
||||
self.L_matrix = self.get_L_matrix(self.new_formation)
|
||||
self.orig_formation=self.new_formation
|
||||
|
||||
#self.L_matrix = self.get_L_matrix(formation_dict_9[self.formation_config])
|
||||
#self.L_matrix = self.get_L_matrix(formation_dict[self.formation_config])
|
||||
if self.id==2:
|
||||
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)
|
||||
self.following_ids = numpy.argwhere(self.L_matrix[self.id,:] == 1)
|
||||
#if self.id == 2:
|
||||
#print(self.following_ids)
|
||||
self.following_count = 0
|
||||
|
|
@ -148,8 +152,8 @@ class Follower:
|
|||
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_local_pose_sub[following_id[0]] = rospy.Subscriber(self.uav_type+'_'+str(following_id[0])+"/mavros/local_position/pose", PoseStamped , self.following_local_pose_callback,following_id[0])
|
||||
self.following_local_velocity_sub[following_id[0]] = rospy.Subscriber(self.uav_type+'_'+str(following_id[0])+"/mavros/local_position/velocity_local", TwistStamped , self.following_local_velocity_callback,following_id[0])
|
||||
self.following_count += 1
|
||||
|
||||
|
||||
|
|
@ -158,14 +162,14 @@ class Follower:
|
|||
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.following_local_pose[following_id[0]].pose.position.x + self.kr * self.following_local_velocity[following_id[0]].twist.linear.x - self.local_pose.pose.position.x - self.kr * self.local_velocity.twist.linear.x + formation_dict_9[self.formation_config][0, self.id-2]
|
||||
self.cmd_accel_enu.y += self.following_local_pose[following_id[0]].pose.position.y + self.kr * self.following_local_velocity[following_id[0]].twist.linear.y - self.local_pose.pose.position.y - self.kr * self.local_velocity.twist.linear.y + formation_dict_9[self.formation_config][1, self.id-2]
|
||||
self.cmd_accel_enu.z += self.following_local_pose[following_id[0]].pose.position.z + self.kr * self.following_local_velocity[following_id[0]].twist.linear.z - self.local_pose.pose.position.z - self.kr * self.local_velocity.twist.linear.z + formation_dict_9[self.formation_config][2, self.id-2]
|
||||
self.cmd_accel_enu.x += self.following_local_pose[following_id[0]].pose.position.x + self.kr * self.following_local_velocity[following_id[0]].twist.linear.x - self.local_pose.pose.position.x - self.kr * self.local_velocity.twist.linear.x + formation_dict[self.formation_config][0, self.id-2]
|
||||
self.cmd_accel_enu.y += self.following_local_pose[following_id[0]].pose.position.y + self.kr * self.following_local_velocity[following_id[0]].twist.linear.y - self.local_pose.pose.position.y - self.kr * self.local_velocity.twist.linear.y + formation_dict[self.formation_config][1, self.id-2]
|
||||
self.cmd_accel_enu.z += self.following_local_pose[following_id[0]].pose.position.z + self.kr * self.following_local_velocity[following_id[0]].twist.linear.z - self.local_pose.pose.position.z - self.kr * self.local_velocity.twist.linear.z + formation_dict[self.formation_config][2, self.id-2]
|
||||
|
||||
if not following_id[0] == 0:
|
||||
self.cmd_accel_enu.x -= formation_dict_9[self.formation_config][0, following_id[0]-1]
|
||||
self.cmd_accel_enu.y -= formation_dict_9[self.formation_config][1, following_id[0]-1]
|
||||
self.cmd_accel_enu.z -= formation_dict_9[self.formation_config][2, following_id[0]-1]
|
||||
self.cmd_accel_enu.x -= formation_dict[self.formation_config][0, following_id[0]-1]
|
||||
self.cmd_accel_enu.y -= formation_dict[self.formation_config][1, following_id[0]-1]
|
||||
self.cmd_accel_enu.z -= formation_dict[self.formation_config][2, following_id[0]-1]
|
||||
if self.arrive_count <= 2000:
|
||||
self.cmd_vel_enu.linear.x = self.local_velocity.twist.linear.x + self.Kp * (self.avoid_accel.x + self.cmd_accel_enu.x / self.f)
|
||||
self.cmd_vel_enu.linear.y = self.local_velocity.twist.linear.y + self.Kp * (self.avoid_accel.y + self.cmd_accel_enu.y / self.f)
|
||||
|
|
@ -495,5 +499,5 @@ class Follower:
|
|||
'''
|
||||
|
||||
if __name__ == '__main__':
|
||||
follower = Follower(int(sys.argv[1]),int(sys.argv[2]))
|
||||
follower = Follower(sys.argv[1],int(sys.argv[2]),int(sys.argv[3]))
|
||||
follower.loop()
|
||||
|
|
@ -8,10 +8,14 @@ import time
|
|||
import math
|
||||
import numpy
|
||||
import sys
|
||||
if sys.argv[2] == '6':
|
||||
from formation_dict import formation_dict_6 as formation_dict
|
||||
elif sys.argv[2] == '9':
|
||||
from formation_dict import formation_dict_9 as formation_dict
|
||||
|
||||
class Leader:
|
||||
|
||||
def __init__(self, leader_id, uav_num):
|
||||
def __init__(self, uav_type, leader_id, uav_num):
|
||||
self.hover = True
|
||||
self.id = leader_id
|
||||
self.local_pose = PoseStamped()
|
||||
|
|
@ -23,17 +27,20 @@ class Leader:
|
|||
self.avoid_accel = Vector3(0,0,0)
|
||||
self.formation_config = 'waiting'
|
||||
self.target_height_recorded = False
|
||||
self.cmd = String()
|
||||
self.f = 200
|
||||
self.Kz = 0.5
|
||||
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/leader/cmd_vel", Twist, self.cmd_vel_callback)
|
||||
self.avoid_vel_sub = rospy.Subscriber("/xtdrone/uav"+str(self.id)+"/avoid_accel", Vector3, self.avoid_accel_callback)
|
||||
self.formation_switch_sub = rospy.Subscriber("/gcs/formation_switch",String, self.cmd_callback)
|
||||
self.local_pose_sub = rospy.Subscriber(uav_type+'_'+str(self.id)+"/mavros/local_position/pose", PoseStamped , self.local_pose_callback)
|
||||
self.cmd_vel_sub = rospy.Subscriber("/xtdrone/leader/cmd_vel_flu", Twist, self.cmd_vel_callback)
|
||||
self.avoid_vel_sub = rospy.Subscriber("/xtdrone/"+uav_type+'_'+str(self.id)+"/avoid_accel", Vector3, self.avoid_accel_callback)
|
||||
self.leader_cmd_sub = rospy.Subscriber("/xtdrone/leader/cmd",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)
|
||||
rospy.Subscriber('/xtdrone/'+uav_type+'_'+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.vel_enu_pub = rospy.Publisher('/xtdrone/uav'+str(self.id)+'/cmd_vel_enu', Twist, queue_size=10)
|
||||
self.vel_enu_pub = rospy.Publisher('/xtdrone/'+uav_type+'_'+str(self.id)+'/cmd_vel_enu', Twist, queue_size=10)
|
||||
self.cmd_pub = rospy.Publisher('/xtdrone/'+uav_type+'_'+str(self.id)+'/cmd', String, queue_size=10)
|
||||
|
||||
def local_pose_callback(self, msg):
|
||||
self.local_pose = msg
|
||||
|
|
@ -46,9 +53,10 @@ class Leader:
|
|||
self.hover = False
|
||||
|
||||
def cmd_callback(self, msg):
|
||||
if not msg.data == '':
|
||||
if msg.data in formation_dict.keys():
|
||||
self.formation_config = msg.data
|
||||
#print("Switch to Formation"+self.formation_config)
|
||||
else:
|
||||
self.cmd = msg.data
|
||||
|
||||
def avoid_accel_callback(self, msg):
|
||||
self.avoid_accel = msg
|
||||
|
|
@ -79,8 +87,9 @@ class Leader:
|
|||
self.cmd_vel_enu.linear.z += self.avoid_accel.z
|
||||
self.vel_enu_pub.publish(self.cmd_vel_enu)
|
||||
self.local_pose_pub.publish(self.local_pose)
|
||||
self.cmd_pub.publish(self.cmd)
|
||||
rate.sleep()
|
||||
|
||||
if __name__ == '__main__':
|
||||
leader = Leader(1,int(sys.argv[1]))
|
||||
leader = Leader(sys.argv[1], 0, int(sys.argv[2]))
|
||||
leader.loop()
|
||||
|
|
|
|||
|
|
@ -1,9 +1,8 @@
|
|||
#!/bin/bash
|
||||
python leader.py $1 &
|
||||
uav_num=2
|
||||
while(( $uav_num<= $1 ))
|
||||
python leader.py $1 $2 &
|
||||
uav_num=1
|
||||
while(( $uav_num< $2 ))
|
||||
do
|
||||
python follower_accel_control.py $uav_num $1&
|
||||
#echo $uav_num
|
||||
python follower_accel_control.py $1 $uav_num $2&
|
||||
let "uav_num++"
|
||||
done
|
||||
|
|
|
|||
|
|
@ -1,4 +1,3 @@
|
|||
#-*-coding：gbk-*-
|
||||
import sys
|
||||
|
||||
welcome_msg = """
|
||||
|
|
|
|||
|
|
@ -32,8 +32,8 @@
|
|||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="rover"/>
|
||||
<arg name="sdf" value="rover_with_sensors"/>
|
||||
<arg name="vehicle" value="iris"/>
|
||||
<arg name="sdf" value="iris_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="24560"/>
|
||||
<arg name="mavlink_tcp_port" value="4560"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
|
|
@ -47,8 +47,8 @@
|
|||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- rover_1 -->
|
||||
<group ns="rover_1">
|
||||
<!-- iris_1 -->
|
||||
<group ns="iris_1">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="1"/>
|
||||
<arg name="ID_in_group" value="1"/>
|
||||
|
|
@ -61,8 +61,8 @@
|
|||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="rover"/>
|
||||
<arg name="sdf" value="rover_with_sensors"/>
|
||||
<arg name="vehicle" value="iris"/>
|
||||
<arg name="sdf" value="iris_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="24562"/>
|
||||
<arg name="mavlink_tcp_port" value="4561"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
|
|
@ -76,22 +76,22 @@
|
|||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- rover_2 -->
|
||||
<group ns="rover_2">
|
||||
<!-- iris_2 -->
|
||||
<group ns="iris_2">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="2"/>
|
||||
<arg name="ID_in_group" value="2"/>
|
||||
<arg name="fcu_url" default="udp://:14542@127.0.0.1:34574"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="3"/>
|
||||
<arg name="y" value="9"/>
|
||||
<arg name="x" value="6"/>
|
||||
<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="rover"/>
|
||||
<arg name="sdf" value="rover_with_sensors"/>
|
||||
<arg name="vehicle" value="iris"/>
|
||||
<arg name="sdf" value="iris_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="24564"/>
|
||||
<arg name="mavlink_tcp_port" value="4562"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
|
|
@ -105,8 +105,8 @@
|
|||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- rover_3 -->
|
||||
<group ns="rover_3">
|
||||
<!-- iris_3 -->
|
||||
<group ns="iris_3">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="3"/>
|
||||
<arg name="ID_in_group" value="3"/>
|
||||
|
|
@ -114,13 +114,13 @@
|
|||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="6"/>
|
||||
<arg name="y" value="3"/>
|
||||
<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="rover"/>
|
||||
<arg name="sdf" value="rover_with_sensors"/>
|
||||
<arg name="vehicle" value="iris"/>
|
||||
<arg name="sdf" value="iris_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="24566"/>
|
||||
<arg name="mavlink_tcp_port" value="4563"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
|
|
@ -134,22 +134,22 @@
|
|||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- typhoon_h480_0 -->
|
||||
<group ns="typhoon_h480_0">
|
||||
<!-- iris_4 -->
|
||||
<group ns="iris_4">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="4"/>
|
||||
<arg name="ID_in_group" value="0"/>
|
||||
<arg name="ID_in_group" value="4"/>
|
||||
<arg name="fcu_url" default="udp://:14544@127.0.0.1:34578"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="12"/>
|
||||
<arg name="x" value="9"/>
|
||||
<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="typhoon_h480"/>
|
||||
<arg name="sdf" value="typhoon_h480_stereo"/>
|
||||
<arg name="vehicle" value="iris"/>
|
||||
<arg name="sdf" value="iris_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="24568"/>
|
||||
<arg name="mavlink_tcp_port" value="4564"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
|
|
@ -162,238 +162,6 @@
|
|||
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- typhoon_h480_1 -->
|
||||
<group ns="typhoon_h480_1">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="5"/>
|
||||
<arg name="ID_in_group" value="1"/>
|
||||
<arg name="fcu_url" default="udp://:14545@127.0.0.1:34580"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="12"/>
|
||||
<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="typhoon_h480"/>
|
||||
<arg name="sdf" value="typhoon_h480_stereo"/>
|
||||
<arg name="mavlink_udp_port" value="24570"/>
|
||||
<arg name="mavlink_tcp_port" value="4565"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</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="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- typhoon_h480_2 -->
|
||||
<group ns="typhoon_h480_2">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="6"/>
|
||||
<arg name="ID_in_group" value="2"/>
|
||||
<arg name="fcu_url" default="udp://:14546@127.0.0.1:34582"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="12"/>
|
||||
<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="typhoon_h480"/>
|
||||
<arg name="sdf" value="typhoon_h480_stereo"/>
|
||||
<arg name="mavlink_udp_port" value="24572"/>
|
||||
<arg name="mavlink_tcp_port" value="4566"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</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="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- typhoon_h480_3 -->
|
||||
<group ns="typhoon_h480_3">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="7"/>
|
||||
<arg name="ID_in_group" value="3"/>
|
||||
<arg name="fcu_url" default="udp://:14547@127.0.0.1:34584"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="12"/>
|
||||
<arg name="y" value="12"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="typhoon_h480"/>
|
||||
<arg name="sdf" value="typhoon_h480_stereo"/>
|
||||
<arg name="mavlink_udp_port" value="24574"/>
|
||||
<arg name="mavlink_tcp_port" value="4567"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</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="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- solo_0 -->
|
||||
<group ns="solo_0">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="8"/>
|
||||
<arg name="ID_in_group" value="0"/>
|
||||
<arg name="fcu_url" default="udp://:14548@127.0.0.1:34586"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="24"/>
|
||||
<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="solo"/>
|
||||
<arg name="sdf" value="solo_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="24576"/>
|
||||
<arg name="mavlink_tcp_port" value="4568"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</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="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- solo_1 -->
|
||||
<group ns="solo_1">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="9"/>
|
||||
<arg name="ID_in_group" value="1"/>
|
||||
<arg name="fcu_url" default="udp://:14549@127.0.0.1:34588"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="24"/>
|
||||
<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="solo"/>
|
||||
<arg name="sdf" value="solo_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="24578"/>
|
||||
<arg name="mavlink_tcp_port" value="4569"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</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="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- solo_2 -->
|
||||
<group ns="solo_2">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="10"/>
|
||||
<arg name="ID_in_group" value="2"/>
|
||||
<arg name="fcu_url" default="udp://:14550@127.0.0.1:34590"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="27"/>
|
||||
<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="solo"/>
|
||||
<arg name="sdf" value="solo_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="24580"/>
|
||||
<arg name="mavlink_tcp_port" value="4570"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</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="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- solo_3 -->
|
||||
<group ns="solo_3">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="11"/>
|
||||
<arg name="ID_in_group" value="3"/>
|
||||
<arg name="fcu_url" default="udp://:14551@127.0.0.1:34592"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="27"/>
|
||||
<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="solo"/>
|
||||
<arg name="sdf" value="solo_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="24582"/>
|
||||
<arg name="mavlink_tcp_port" value="4571"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</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="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- solo_4 -->
|
||||
<group ns="solo_4">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="12"/>
|
||||
<arg name="ID_in_group" value="4"/>
|
||||
<arg name="fcu_url" default="udp://:14552@127.0.0.1:34594"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="30"/>
|
||||
<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="solo"/>
|
||||
<arg name="sdf" value="solo_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="24584"/>
|
||||
<arg name="mavlink_tcp_port" value="4572"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</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="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- iris_5 -->
|
||||
<group ns="iris_5">
|
||||
|
|
@ -403,8 +171,8 @@
|
|||
<arg name="fcu_url" default="udp://:14545@127.0.0.1:34580"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="15"/>
|
||||
<arg name="x" value="9"/>
|
||||
<arg name="y" value="6"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
|
|
@ -424,92 +192,5 @@
|
|||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- iris_6 -->
|
||||
<group ns="iris_6">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="6"/>
|
||||
<arg name="ID_in_group" value="6"/>
|
||||
<arg name="fcu_url" default="udp://:14546@127.0.0.1:34582"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="18"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="iris"/>
|
||||
<arg name="sdf" value="iris_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="24572"/>
|
||||
<arg name="mavlink_tcp_port" value="4566"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</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="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- iris_7 -->
|
||||
<group ns="iris_7">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="7"/>
|
||||
<arg name="ID_in_group" value="7"/>
|
||||
<arg name="fcu_url" default="udp://:14547@127.0.0.1:34584"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="21"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="iris"/>
|
||||
<arg name="sdf" value="iris_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="24574"/>
|
||||
<arg name="mavlink_tcp_port" value="4567"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</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="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- iris_8 -->
|
||||
<group ns="iris_8">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="8"/>
|
||||
<arg name="ID_in_group" value="8"/>
|
||||
<arg name="fcu_url" default="udp://:14548@127.0.0.1:34586"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="24"/>
|
||||
<arg name="z" value="0"/>
|
||||
<arg name="R" value="0"/>
|
||||
<arg name="P" value="0"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<arg name="vehicle" value="iris"/>
|
||||
<arg name="sdf" value="iris_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="24576"/>
|
||||
<arg name="mavlink_tcp_port" value="4568"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</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="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
</launch>
|
||||
<!--the launch file is generated by XTDrone multi-vehicle generator.py -->
|
||||
|
|
@ -7,10 +7,8 @@ from geometry_msgs.msg import Twist,Pose,PoseStamped,TwistStamped
|
|||
from gazebo_msgs.srv import GetModelState
|
||||
import sys
|
||||
|
||||
use_1_8 = 1
|
||||
|
||||
#uav_num=9
|
||||
uav_num = int(sys.argv[1])
|
||||
uav_type = sys.argv[1]
|
||||
uav_num = int(sys.argv[2])
|
||||
|
||||
step_time=0.005
|
||||
|
||||
|
|
@ -23,11 +21,10 @@ plot_z=[0]*(uav_num)
|
|||
local_vel = [TwistStamped() for i in range (uav_num)]
|
||||
|
||||
for i in range(uav_num):
|
||||
uav_id=i+use_1_8
|
||||
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)
|
||||
pose_puber[i]=rospy.Publisher(uav_type+'_'+str(i)+'/mavros/local_position/pose', PoseStamped, queue_size=2)
|
||||
vel_puber[i]=rospy.Publisher(uav_type+'_'+str(i)+'/mavros/local_position/velocity_local', TwistStamped, queue_size=2)
|
||||
|
||||
|
||||
def cmd_vel_callback(msg,id):
|
||||
|
|
@ -41,9 +38,8 @@ 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)
|
||||
rospy.Subscriber('/xtdrone/'+uav_type+'_'+str(i)+'/cmd_vel_flu', Twist, cmd_vel_callback,i)
|
||||
rospy.Subscriber('/xtdrone/'+uav_type+'_'+str(i)+'/cmd_vel_enu', Twist, cmd_vel_callback,i)
|
||||
|
||||
|
||||
while not rospy.is_shutdown():
|
||||
|
|
|
|||
|
|
@ -7,7 +7,8 @@ from geometry_msgs.msg import Twist,Pose,PoseStamped,TwistStamped
|
|||
from gazebo_msgs.srv import GetModelState
|
||||
import sys
|
||||
|
||||
uav_num = int(sys.argv[1])
|
||||
uav_type = sys.argv[1]
|
||||
uav_num = int(sys.argv[2])
|
||||
|
||||
step_time=0.005
|
||||
|
||||
|
|
@ -51,7 +52,7 @@ rospy.init_node('visualize')
|
|||
rate = rospy.Rate(1/step_time)
|
||||
|
||||
for i in range(uav_num):
|
||||
rospy.Subscriber('/uav'+str(i+1)+'/mavros/local_position/pose', PoseStamped, pose_sub_callback,i)
|
||||
rospy.Subscriber(uav_type+'_'+str(i)+'/mavros/local_position/pose', PoseStamped, pose_sub_callback,i)
|
||||
|
||||
try:
|
||||
while not rospy.is_shutdown():
|
||||
|
|
|
|||
Loading…
Reference in New Issue