forked from xtdrone/XTDrone
完善二维运动规划
This commit is contained in:
parent
1f80ff0868
commit
677cccedb7
|
|
@ -1,5 +1,5 @@
|
|||
#!/bin/bash
|
||||
iris_num=6
|
||||
iris_num=18
|
||||
typhoon_h480_num=0
|
||||
solo_num=0
|
||||
plane_num=0
|
||||
|
|
|
|||
|
|
@ -10,12 +10,12 @@ 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
|
||||
avoid_vel_pub = [None]*vehicle_num
|
||||
avoid_kp = 0.5
|
||||
avoid_radius = 1.5
|
||||
aid_vec1 = [1, 0, 0]
|
||||
aid_vec2 = [0, 1, 0]
|
||||
uavs_avoid_accel = [Vector3()] * vehicle_num
|
||||
vehicles_avoid_vel = [Vector3()] * vehicle_num
|
||||
|
||||
def pose_callback(msg, id):
|
||||
pose[id] = msg
|
||||
|
|
@ -23,7 +23,7 @@ def pose_callback(msg, id):
|
|||
rospy.init_node('avoid')
|
||||
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)
|
||||
avoid_vel_pub[i] = rospy.Publisher("/xtdrone/"+vehicle_type+'_'+str(i)+"/avoid_vel", Vector3,queue_size=10)
|
||||
|
||||
time.sleep(1)
|
||||
rate = rospy.Rate(50)
|
||||
|
|
@ -37,14 +37,18 @@ while not rospy.is_shutdown():
|
|||
cos1 = dir_vec.dot(aid_vec1)/(numpy.linalg.norm(dir_vec) * numpy.linalg.norm(aid_vec1))
|
||||
cos2 = dir_vec.dot(aid_vec2)/(numpy.linalg.norm(dir_vec) * numpy.linalg.norm(aid_vec2))
|
||||
if abs(cos1) < abs(cos2):
|
||||
avoid_accel = avoid_kp * numpy.cross(dir_vec, aid_vec1)/numpy.linalg.norm(numpy.cross(dir_vec, aid_vec1))
|
||||
avoid_vel = avoid_kp * numpy.cross(dir_vec, aid_vec1)/numpy.linalg.norm(numpy.cross(dir_vec, aid_vec1))
|
||||
else:
|
||||
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])
|
||||
avoid_vel = avoid_kp * numpy.cross(dir_vec, aid_vec2)/numpy.linalg.norm(numpy.cross(dir_vec, aid_vec2))
|
||||
if dir_vec[2] * avoid_vel[2] > 0:
|
||||
vehicles_avoid_vel[i] = Vector3(vehicles_avoid_vel[i].x+avoid_vel[0],vehicles_avoid_vel[i].y+avoid_vel[1],vehicles_avoid_vel[i].z+avoid_vel[2])
|
||||
vehicles_avoid_vel[i+j] = Vector3(vehicles_avoid_vel[i+j].x-avoid_vel[0],vehicles_avoid_vel[i+j].y-avoid_vel[1],vehicles_avoid_vel[i+j].z-avoid_vel[2])
|
||||
else:
|
||||
vehicles_avoid_vel[i] = Vector3(vehicles_avoid_vel[i].x-avoid_vel[0],vehicles_avoid_vel[i].y-avoid_vel[1],vehicles_avoid_vel[i].z-avoid_vel[2])
|
||||
vehicles_avoid_vel[i+j] = Vector3(vehicles_avoid_vel[i+j].x+avoid_vel[0],vehicles_avoid_vel[i+j].y+avoid_vel[1],vehicles_avoid_vel[i+j].z+avoid_vel[2])
|
||||
for i in range(vehicle_num):
|
||||
avoid_accel_pub[i].publish(uavs_avoid_accel[i])
|
||||
uavs_avoid_accel = [Vector3()] * vehicle_num
|
||||
avoid_vel_pub[i].publish(vehicles_avoid_vel[i])
|
||||
vehicles_avoid_vel = [Vector3()] * vehicle_num
|
||||
rate.sleep()
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -34,26 +34,20 @@ class Follower:
|
|||
self.local_pose_queue.put(PoseStamped())
|
||||
self.local_velocity = TwistStamped()
|
||||
self.cmd_vel_enu = Twist()
|
||||
self.cmd_accel_enu = Vector3()
|
||||
self.avoid_accel = Vector3()
|
||||
self.avoid_vel = Vector3()
|
||||
self.following_switch = False
|
||||
self.arrive_print = True
|
||||
self.following_ids = []
|
||||
self.formation_config = 'waiting'
|
||||
self.following_count = 0
|
||||
self.Kp = 100 #100
|
||||
#self.kr = (4/int((self.uav_num-1)/2))**0.5
|
||||
self.kr = 1
|
||||
self.velxy_max = 0.5
|
||||
self.velz_max = 0.5
|
||||
self.Kp = 1
|
||||
self.velxy_max = 0.8
|
||||
self.velz_max = 0.8
|
||||
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(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.avoid_vel_sub = rospy.Subscriber("/xtdrone/"+self.uav_type+'_'+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/'+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)
|
||||
|
|
@ -67,40 +61,21 @@ class Follower:
|
|||
pose_comparison = self.local_pose_queue.get()
|
||||
self.local_pose_queue.put(self.local_pose)
|
||||
comparison = (self.local_pose.pose.position.x - pose_comparison.pose.position.x)**2+(self.local_pose.pose.position.y - pose_comparison.pose.position.y)**2+(self.local_pose.pose.position.z - pose_comparison.pose.position.z)**2
|
||||
#if self.id == 6:
|
||||
#print('comparison1:',comparison)
|
||||
#print('comparison2:',float(self.velxy_max**2+self.velxy_max**2+self.velz_max**2)/1e3)
|
||||
#print(self.arrive_count)
|
||||
if comparison < float(self.velxy_max**2+self.velxy_max**2+self.velz_max**2)/1e5:
|
||||
#if self.id == 2:
|
||||
#print('here')
|
||||
self.arrive_count += 1
|
||||
else:
|
||||
self.arrive_count = 0
|
||||
|
||||
def local_velocity_callback(self, msg):
|
||||
self.local_velocity = msg
|
||||
|
||||
|
||||
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
|
||||
|
||||
def formation_switch_callback(self, msg):
|
||||
if not self.formation_config == msg.data:
|
||||
self.following_switch = True
|
||||
self.formation_config = msg.data
|
||||
|
||||
self.formation_config = msg.data
|
||||
|
||||
def avoid_accel_callback(self, msg):
|
||||
self.avoid_accel = msg
|
||||
def avoid_vel_callback(self, msg):
|
||||
self.avoid_vel = msg
|
||||
|
||||
def loop(self):
|
||||
rospy.init_node('follower'+str(self.id-1))
|
||||
|
|
@ -124,9 +99,7 @@ class Follower:
|
|||
self.first_formation=False
|
||||
self.L_matrix = self.get_L_central_matrix()
|
||||
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[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
|
||||
|
|
@ -138,44 +111,19 @@ class Follower:
|
|||
self.L_matrix = self.get_L_central_matrix()
|
||||
self.change_id = self.KM()
|
||||
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[self.formation_config])
|
||||
#if self.id==2:
|
||||
#print(self.L_matrix)
|
||||
|
||||
self.following_ids = numpy.argwhere(self.L_matrix[self.id,:] == 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(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
|
||||
|
||||
|
||||
#self.cmd_accel_enu = Vector3(0, 0, 0)
|
||||
|
||||
self.cmd_vel_enu.linear = Vector3(0, 0, 0)
|
||||
#self.cmd_vel_enu.linear = copy.deepcopy(self.avoid_accel)
|
||||
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[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[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]
|
||||
'''
|
||||
self.cmd_vel_enu.linear.x += self.following_local_pose[following_id[0]].pose.position.x - self.local_pose.pose.position.x + formation_dict[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[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[self.formation_config][2, self.id-2]
|
||||
|
|
@ -183,13 +131,10 @@ class Follower:
|
|||
self.cmd_vel_enu.linear.x -= formation_dict[self.formation_config][0, following_id[0]-1]
|
||||
self.cmd_vel_enu.linear.y -= formation_dict[self.formation_config][1, following_id[0]-1]
|
||||
self.cmd_vel_enu.linear.z -= formation_dict[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.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)
|
||||
#self.cmd_vel_enu.linear.z = self.local_velocity.twist.linear.z + self.Kp * (self.avoid_accel.z + self.cmd_accel_enu.z / self.f)
|
||||
self.cmd_vel_enu.linear.x = self.Kp * self.cmd_vel_enu.linear.x + self.avoid_vel.x
|
||||
self.cmd_vel_enu.linear.y = self.Kp * self.cmd_vel_enu.linear.y + self.avoid_vel.y
|
||||
self.cmd_vel_enu.linear.z = self.Kp * self.cmd_vel_enu.linear.z + self.avoid_vel.z
|
||||
|
||||
if self.cmd_vel_enu.linear.x > self.velxy_max:
|
||||
self.cmd_vel_enu.linear.x = self.velxy_max
|
||||
elif self.cmd_vel_enu.linear.x < - self.velxy_max:
|
||||
|
|
@ -209,8 +154,6 @@ class Follower:
|
|||
self.arrive_count += 1
|
||||
else:
|
||||
self.arrive_count = 0
|
||||
#else:
|
||||
#self.cmd_pub.publish(self.hover)
|
||||
rate.sleep()
|
||||
|
||||
def build_graph(self,orig_formation,change_formation):
|
||||
|
|
@ -235,13 +178,11 @@ class Follower:
|
|||
return True
|
||||
else:
|
||||
self.slack_right[j]=min(gap,self.slack_right[j])
|
||||
#print('1',slack_right)
|
||||
return False
|
||||
|
||||
def KM(self):
|
||||
|
||||
for i in range(self.uav_num-1):
|
||||
#print(i)
|
||||
self.slack_right = numpy.array([100]*(self.uav_num-1))
|
||||
while True:
|
||||
self.visit_left = numpy.array([0]*(self.uav_num-1))
|
||||
|
|
@ -249,7 +190,6 @@ class Follower:
|
|||
if self.find_path(i):
|
||||
break
|
||||
d = numpy.inf
|
||||
#print ('2',slack_right)
|
||||
for j, slack in enumerate(self.slack_right):
|
||||
if not self.visit_right[j] :
|
||||
d = min(d,slack)
|
||||
|
|
@ -261,11 +201,6 @@ class Follower:
|
|||
self.label_right[k] += d
|
||||
else:
|
||||
self.slack_right[k] -=d
|
||||
'''
|
||||
for j in range(uav_num-1):
|
||||
if match_right[j] >=0 and match_right[j] < uav_num:
|
||||
res += adj_matrix[match_right[j]][j]
|
||||
'''
|
||||
return self.match_right
|
||||
|
||||
def get_new_formation(self,change_id,change_formation):
|
||||
|
|
@ -273,39 +208,6 @@ class Follower:
|
|||
|
||||
new_formation=numpy.zeros((3,self.uav_num-1))
|
||||
position=numpy.zeros((3,self.uav_num-1))
|
||||
'''
|
||||
num_array=range(0,self.uav_num-1)
|
||||
num_array_possi=list(permutations(num_array, self.uav_num-1))
|
||||
possi_num=len(num_array_possi)
|
||||
|
||||
distance=[0 for i in range(self.uav_num-1)]
|
||||
distance_sum=0
|
||||
distance_min=100
|
||||
distance_max=100 # maybe larger
|
||||
min_num=0
|
||||
|
||||
for k in range(possi_num):
|
||||
for i in range(self.uav_num-1):
|
||||
distance[i]=numpy.linalg.norm(orig_formation[:,i]-change_formation[:,num_array_possi[k][i]])
|
||||
distance_mid_max=max(distance)
|
||||
if distance_mid_max<distance_max:
|
||||
distance_max=distance_mid_max
|
||||
min_num=k
|
||||
|
||||
for k in range(possi_num):
|
||||
for i in range(self.uav_num-1):
|
||||
distance[i]=numpy.linalg.norm(orig_formation[:,i]-change_formation[:,num_array_possi[k][i]])
|
||||
distance_sum=distance_sum+distance[i]
|
||||
if distance_sum>distance_min:
|
||||
break
|
||||
if distance_min>distance_sum:
|
||||
distance_min=distance_sum
|
||||
#print(distance_min)
|
||||
min_num=k
|
||||
distance_sum=0
|
||||
|
||||
change_id=num_array_possi[min_num]
|
||||
'''
|
||||
change_id=[i + 1 for i in change_id]
|
||||
#print (change_id)
|
||||
for i in range(0,self.uav_num-1):
|
||||
|
|
@ -325,208 +227,6 @@ class Follower:
|
|||
L[i][0]=1
|
||||
L[i][i]=-1
|
||||
return L
|
||||
'''
|
||||
#distributed control 函数输入为相对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(sys.argv[1],int(sys.argv[2]),int(sys.argv[3]))
|
||||
|
|
|
|||
|
|
@ -18,8 +18,8 @@
|
|||
<arg name="verbose" value="$(arg verbose)"/>
|
||||
<arg name="paused" value="$(arg paused)"/>
|
||||
</include>
|
||||
<!-- iris_0 -->
|
||||
<group ns="iris_0">
|
||||
<!-- plane_0 -->
|
||||
<group ns="plane_0">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="1"/>
|
||||
<arg name="ID_in_group" value="0"/>
|
||||
|
|
@ -32,8 +32,8 @@
|
|||
<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="vehicle" value="plane"/>
|
||||
<arg name="sdf" value="plane"/>
|
||||
<arg name="mavlink_udp_port" value="24562"/>
|
||||
<arg name="mavlink_tcp_port" value="4561"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
|
|
@ -46,25 +46,373 @@
|
|||
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
||||
<arg name="tgt_component" value="1"/>
|
||||
</include>
|
||||
</group>
|
||||
<!-- plane_1 -->
|
||||
<group ns="plane_1">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="2"/>
|
||||
<arg name="ID_in_group" value="1"/>
|
||||
<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="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="plane"/>
|
||||
<arg name="sdf" value="plane"/>
|
||||
<arg name="mavlink_udp_port" value="24564"/>
|
||||
<arg name="mavlink_tcp_port" value="4562"/>
|
||||
<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>
|
||||
<!-- plane_2 -->
|
||||
<group ns="plane_2">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="3"/>
|
||||
<arg name="ID_in_group" value="2"/>
|
||||
<arg name="fcu_url" default="udp://:14543@127.0.0.1:34576"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.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="plane"/>
|
||||
<arg name="sdf" value="plane"/>
|
||||
<arg name="mavlink_udp_port" value="24566"/>
|
||||
<arg name="mavlink_tcp_port" value="4563"/>
|
||||
<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_0 -->
|
||||
<group ns="typhoon_h480_0">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="4"/>
|
||||
<arg name="ID_in_group" value="0"/>
|
||||
<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="3"/>
|
||||
<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="mavlink_udp_port" value="24568"/>
|
||||
<arg name="mavlink_tcp_port" value="4564"/>
|
||||
<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_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="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="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="3"/>
|
||||
<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>
|
||||
<!-- solo_0 -->
|
||||
<group ns="solo_0">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="7"/>
|
||||
<arg name="ID_in_group" value="0"/>
|
||||
<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="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="solo"/>
|
||||
<arg name="sdf" value="solo_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>
|
||||
<!-- solo_1 -->
|
||||
<group ns="solo_1">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="8"/>
|
||||
<arg name="ID_in_group" value="1"/>
|
||||
<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="6"/>
|
||||
<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="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_2 -->
|
||||
<group ns="solo_2">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="9"/>
|
||||
<arg name="ID_in_group" value="2"/>
|
||||
<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="6"/>
|
||||
<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="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_3 -->
|
||||
<group ns="solo_3">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="10"/>
|
||||
<arg name="ID_in_group" value="3"/>
|
||||
<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="6"/>
|
||||
<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="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_4 -->
|
||||
<group ns="solo_4">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="11"/>
|
||||
<arg name="ID_in_group" value="4"/>
|
||||
<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="6"/>
|
||||
<arg name="y" value="15"/>
|
||||
<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_5 -->
|
||||
<group ns="solo_5">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="12"/>
|
||||
<arg name="ID_in_group" value="5"/>
|
||||
<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="6"/>
|
||||
<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="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_0 -->
|
||||
<group ns="iris_0">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="13"/>
|
||||
<arg name="ID_in_group" value="0"/>
|
||||
<arg name="fcu_url" default="udp://:14553@127.0.0.1:34596"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<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="iris"/>
|
||||
<arg name="sdf" value="iris_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="24586"/>
|
||||
<arg name="mavlink_tcp_port" value="4573"/>
|
||||
<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_1 -->
|
||||
<group ns="iris_1">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="2"/>
|
||||
<arg name="ID" value="14"/>
|
||||
<arg name="ID_in_group" value="1"/>
|
||||
<arg name="fcu_url" default="udp://:14542@127.0.0.1:34574"/>
|
||||
<arg name="fcu_url" default="udp://:14554@127.0.0.1:34598"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="3"/>
|
||||
<arg name="y" value="3"/>
|
||||
<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"/>
|
||||
<arg name="Y" value="0"/>
|
||||
<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="mavlink_udp_port" value="24588"/>
|
||||
<arg name="mavlink_tcp_port" value="4574"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
|
|
@ -79,21 +427,21 @@
|
|||
<!-- iris_2 -->
|
||||
<group ns="iris_2">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="3"/>
|
||||
<arg name="ID" value="15"/>
|
||||
<arg name="ID_in_group" value="2"/>
|
||||
<arg name="fcu_url" default="udp://:14543@127.0.0.1:34576"/>
|
||||
<arg name="fcu_url" default="udp://:14555@127.0.0.1:34600"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="6"/>
|
||||
<arg name="x" value="9"/>
|
||||
<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="iris"/>
|
||||
<arg name="sdf" value="iris_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="24566"/>
|
||||
<arg name="mavlink_tcp_port" value="4563"/>
|
||||
<arg name="mavlink_udp_port" value="24590"/>
|
||||
<arg name="mavlink_tcp_port" value="4575"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
|
|
@ -108,21 +456,21 @@
|
|||
<!-- iris_3 -->
|
||||
<group ns="iris_3">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="4"/>
|
||||
<arg name="ID" value="16"/>
|
||||
<arg name="ID_in_group" value="3"/>
|
||||
<arg name="fcu_url" default="udp://:14544@127.0.0.1:34578"/>
|
||||
<arg name="fcu_url" default="udp://:14556@127.0.0.1:34602"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="3"/>
|
||||
<arg name="y" value="6"/>
|
||||
<arg name="x" value="9"/>
|
||||
<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="iris"/>
|
||||
<arg name="sdf" value="iris_stereo_camera"/>
|
||||
<arg name="mavlink_udp_port" value="24568"/>
|
||||
<arg name="mavlink_tcp_port" value="4564"/>
|
||||
<arg name="mavlink_udp_port" value="24592"/>
|
||||
<arg name="mavlink_tcp_port" value="4576"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
|
|
@ -137,21 +485,21 @@
|
|||
<!-- iris_4 -->
|
||||
<group ns="iris_4">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="5"/>
|
||||
<arg name="ID" value="17"/>
|
||||
<arg name="ID_in_group" value="4"/>
|
||||
<arg name="fcu_url" default="udp://:14545@127.0.0.1:34580"/>
|
||||
<arg name="fcu_url" default="udp://:14557@127.0.0.1:34604"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="0"/>
|
||||
<arg name="y" value="9"/>
|
||||
<arg name="x" value="9"/>
|
||||
<arg name="y" value="15"/>
|
||||
<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="24570"/>
|
||||
<arg name="mavlink_tcp_port" value="4565"/>
|
||||
<arg name="mavlink_udp_port" value="24594"/>
|
||||
<arg name="mavlink_tcp_port" value="4577"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
|
|
@ -166,21 +514,108 @@
|
|||
<!-- iris_5 -->
|
||||
<group ns="iris_5">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="6"/>
|
||||
<arg name="ID" value="18"/>
|
||||
<arg name="ID_in_group" value="5"/>
|
||||
<arg name="fcu_url" default="udp://:14546@127.0.0.1:34582"/>
|
||||
<arg name="fcu_url" default="udp://:14558@127.0.0.1:34606"/>
|
||||
<!-- 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="9"/>
|
||||
<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="mavlink_udp_port" value="24596"/>
|
||||
<arg name="mavlink_tcp_port" value="4578"/>
|
||||
<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>
|
||||
<!-- tiltrotor_0 -->
|
||||
<group ns="tiltrotor_0">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="19"/>
|
||||
<arg name="ID_in_group" value="0"/>
|
||||
<arg name="fcu_url" default="udp://:14559@127.0.0.1:34608"/>
|
||||
<!-- PX4 SITL and vehicle spawn -->
|
||||
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
||||
<arg name="x" value="12"/>
|
||||
<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="tiltrotor"/>
|
||||
<arg name="sdf" value="tiltrotor"/>
|
||||
<arg name="mavlink_udp_port" value="24598"/>
|
||||
<arg name="mavlink_tcp_port" value="4579"/>
|
||||
<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>
|
||||
<!-- tiltrotor_1 -->
|
||||
<group ns="tiltrotor_1">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="20"/>
|
||||
<arg name="ID_in_group" value="1"/>
|
||||
<arg name="fcu_url" default="udp://:14560@127.0.0.1:34610"/>
|
||||
<!-- 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="tiltrotor"/>
|
||||
<arg name="sdf" value="tiltrotor"/>
|
||||
<arg name="mavlink_udp_port" value="24600"/>
|
||||
<arg name="mavlink_tcp_port" value="4580"/>
|
||||
<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>
|
||||
<!-- tiltrotor_2 -->
|
||||
<group ns="tiltrotor_2">
|
||||
<!-- MAVROS and vehicle configs -->
|
||||
<arg name="ID" value="21"/>
|
||||
<arg name="ID_in_group" value="2"/>
|
||||
<arg name="fcu_url" default="udp://:14561@127.0.0.1:34612"/>
|
||||
<!-- 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="tiltrotor"/>
|
||||
<arg name="sdf" value="tiltrotor"/>
|
||||
<arg name="mavlink_udp_port" value="24602"/>
|
||||
<arg name="mavlink_tcp_port" value="4581"/>
|
||||
<arg name="ID" value="$(arg ID)"/>
|
||||
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
||||
</include>
|
||||
|
|
|
|||
|
|
@ -1,18 +1,20 @@
|
|||
<launch>
|
||||
<node pkg="tf" type="static_transform_publisher" name="odom_to_amp"
|
||||
args="0.0 0.0 0 0.0 0.0 0.0 /odom /map 40" />
|
||||
<!-- Arguments -->
|
||||
<arg name="motion_planning" default="/home/robin/px4/xtdrone/motion_planning"/>
|
||||
<arg name="move_forward_only" default="false"/>
|
||||
<arg name="open_rviz" default="true"/>
|
||||
|
||||
<!-- Map server -->
|
||||
<node pkg="map_server" name="map_server" type="map_server" args="$(arg motion_planning)/map/indoor3.yaml"/>
|
||||
<node pkg="map_server" name="map_server" type="map_server" args="$(arg motion_planning)/map//indoor3.yaml"/>
|
||||
|
||||
<!-- move_base -->
|
||||
<include file="$(find px4)/launch/move_base.launch">
|
||||
<arg name="motion_planning" value="$(arg motion_planning)"/>
|
||||
<arg name="move_forward_only" value="$(arg move_forward_only)"/>
|
||||
<arg name="cmd_vel_topic" default="/xtdrone/cmd_vel_flu" />
|
||||
<arg name="odom_topic" default="/mavros/local_position/odom" />
|
||||
<arg name="cmd_vel_topic" default="/xtdrone/iris_0/cmd_vel_flu" />
|
||||
<arg name="odom_topic" default="/iris_0/mavros/local_position/odom" />
|
||||
</include>
|
||||
|
||||
<!-- rviz -->
|
||||
|
|
|
|||
|
|
@ -1,11 +1,11 @@
|
|||
TrajectoryPlannerROS:
|
||||
|
||||
# Robot Configuration Parameters
|
||||
max_vel_x: 0.08
|
||||
max_vel_x: 0.3
|
||||
min_vel_x: 0.02
|
||||
|
||||
max_vel_theta: 0.3
|
||||
min_vel_theta: -0.3
|
||||
max_vel_theta: 0.1
|
||||
min_vel_theta: -0.1
|
||||
min_in_place_vel_theta: 1.0
|
||||
|
||||
acc_lim_x: 1.0
|
||||
|
|
|
|||
|
|
@ -7,5 +7,5 @@ inflation_radius: 1.0
|
|||
cost_scaling_factor: 2.0
|
||||
|
||||
map_type: costmap
|
||||
observation_sources: scan
|
||||
scan: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true}
|
||||
observation_sources: iris_0/scan
|
||||
iris_0/scan: {sensor_frame: iris_0/laser_2d, data_type: LaserScan, topic: iris_0/scan, marking: true, clearing: true}
|
||||
|
|
|
|||
|
|
@ -1,18 +1,18 @@
|
|||
DWAPlannerROS:
|
||||
|
||||
# Robot Configuration Parameters
|
||||
max_vel_x: 0.1
|
||||
min_vel_x: -0.1
|
||||
max_vel_x: 0.2
|
||||
min_vel_x: -0.2
|
||||
|
||||
max_vel_y: 0.1
|
||||
min_vel_y: -0.1
|
||||
max_vel_y: 0.2
|
||||
min_vel_y: -0.2
|
||||
|
||||
# The velocity when robot is moving in a straight line
|
||||
max_trans_vel: 0.1
|
||||
min_trans_vel: 0.05
|
||||
max_trans_vel: 0.2
|
||||
min_trans_vel: 0.01
|
||||
|
||||
max_rot_vel: 0.5
|
||||
min_rot_vel: 0.1
|
||||
max_rot_vel: 0.1
|
||||
min_rot_vel: 0.01
|
||||
|
||||
acc_lim_x: 1
|
||||
acc_lim_y: 0.0
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
global_costmap:
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
robot_base_frame: iris_0/base_link
|
||||
|
||||
update_frequency: 10.0
|
||||
publish_frequency: 10.0
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
local_costmap:
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
robot_base_frame: iris_0/base_link
|
||||
|
||||
update_frequency: 10.0
|
||||
publish_frequency: 10.0
|
||||
|
|
|
|||
|
|
@ -90,7 +90,7 @@ Visualization Manager:
|
|||
Size (Pixels): 3
|
||||
Size (m): 0.1
|
||||
Style: Flat Squares
|
||||
Topic: /scan
|
||||
Topic: /iris_0/scan
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
|
|
|
|||
|
|
@ -58,6 +58,8 @@ slam_gmapping
|
|||
<param name="llsamplestep" value="0.05"/>
|
||||
<param name="lasamplerange" value="0.05"/>
|
||||
<param name="lasamplestep" value="0.05"/>
|
||||
<remap from="scan" to="iris_0/scan"/>
|
||||
<remap from="pose2D" to="iris_0/pose2D"/>
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -8,7 +8,7 @@ import sys
|
|||
vehicle_type = sys.argv[1]
|
||||
vehicle_id = sys.argv[2]
|
||||
laser_slam_type = sys.argv[3]
|
||||
rospy.init_node(vehicle_type+vehicle_id+'_'+laser_slam_type+'_laser_slam_data_transfer')
|
||||
rospy.init_node(vehicle_type+vehicle_id+'_'+laser_slam_type+'_laser_transfer')
|
||||
pose_pub = rospy.Publisher(vehicle_type+'_'+ vehicle_id+"/mavros/vision_pose/pose", PoseStamped, queue_size=2)
|
||||
local_pose = PoseStamped()
|
||||
local_pose.header.frame_id = 'map'
|
||||
|
|
@ -53,10 +53,10 @@ def aloam():
|
|||
|
||||
if __name__ == '__main__':
|
||||
|
||||
if laser_slam_type == '2D':
|
||||
if laser_slam_type == '2d':
|
||||
odom_groundtruth_sub = rospy.Subscriber('/xtdrone/'+vehicle_type+'_'+ vehicle_id+'/ground_truth/odom', Odometry, odm_groundtruth_callback)
|
||||
laser_scan_matcher()
|
||||
elif laser_slam_type == '3D':
|
||||
elif laser_slam_type == '3d':
|
||||
odom_aloam_sub = rospy.Subscriber('/laser_odom_to_init', Odometry, odm_aloam_callback)
|
||||
aloam()
|
||||
else:
|
||||
Loading…
Reference in New Issue