6003xuan
/
XTDrone
forked from xtdrone/XTDrone
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

modify code of actor control

This commit is contained in:
MALAN 2020-11-09 13:26:43 +08:00
parent 5500f67495
commit d280b2f7bd
15 changed files with 5820 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
communication/multi_vehicle_communication_ml.sh Normal file
View File

@ -0,0 +1,65 @@
#!/bin/bash
iris_num=0
typhoon_h480_num=6
solo_num=0
plane_num=0
rover_num=0
standard_vtol_num=0
tiltrotor_num=0
tailsitter_num=0
vehicle_num=0
while(( $vehicle_num< iris_num))
do
python multirotor_communication.py iris $vehicle_num&
let "vehicle_num++"
done
vehicle_num=0
while(( $vehicle_num< typhoon_h480_num))
do
python multirotor_communication.py typhoon_h480 $vehicle_num&
let "vehicle_num++"
done
vehicle_num=0
while(( $vehicle_num< solo_num))
do
python multirotor_communication.py solo $vehicle_num&
let "vehicle_num++"
done
vehicle_num=0
while(( $vehicle_num< plane_num))
do
python plane_communication.py $vehicle_num&
let "vehicle_num++"
done
vehicle_num=0
while(( $vehicle_num< rover_num))
do
python rover_communication.py $vehicle_num&
let "vehicle_num++"
done
vehicle_num=0
while(( $vehicle_num< standard_vtol_num))
do
python vtol_communication.py standard_vtol $vehicle_num&
let "vehicle_num++"
done
vehicle_num=0
while(( $vehicle_num< tiltrotor_num))
do
python vtol_communication.py tiltrotor $vehicle_num&
let "vehicle_num++"
done
vehicle_num=0
while(( $vehicle_num< tailsitter_num))
do
python vtol_communication.py tailsitter $vehicle_num&
let "vehicle_num++"
done

65
communication/multi_vehicle_communication_typh.sh Normal file
View File

@ -0,0 +1,65 @@
#!/bin/bash
iris_num=0
typhoon_h480_num=6
solo_num=0
plane_num=0
rover_num=0
standard_vtol_num=0
tiltrotor_num=0
tailsitter_num=0
vehicle_num=0
while(( $vehicle_num< iris_num))
do
python multirotor_communication.py iris $vehicle_num&
let "vehicle_num++"
done
vehicle_num=0
while(( $vehicle_num< typhoon_h480_num))
do
python multirotor_communication.py typhoon_h480 $vehicle_num&
let "vehicle_num++"
done
vehicle_num=0
while(( $vehicle_num< solo_num))
do
python multirotor_communication.py solo $vehicle_num&
let "vehicle_num++"
done
vehicle_num=0
while(( $vehicle_num< plane_num))
do
python plane_communication.py $vehicle_num&
let "vehicle_num++"
done
vehicle_num=0
while(( $vehicle_num< rover_num))
do
python rover_communication.py $vehicle_num&
let "vehicle_num++"
done
vehicle_num=0
while(( $vehicle_num< standard_vtol_num))
do
python vtol_communication.py standard_vtol $vehicle_num&
let "vehicle_num++"
done
vehicle_num=0
while(( $vehicle_num< tiltrotor_num))
do
python vtol_communication.py tiltrotor $vehicle_num&
let "vehicle_num++"
done
vehicle_num=0
while(( $vehicle_num< tailsitter_num))
do
python vtol_communication.py tailsitter $vehicle_num&
let "vehicle_num++"
done

201
control/actor/ObstacleAvoid.py Executable file
View File

@ -0,0 +1,201 @@
import numpy
import math
from geometry_msgs.msg import Point
import sys
class ObstacleAviod:
def __init__(self):
self.indexOfObstInSp = []
self.obstInUAVCoorSys = []
self.obstlist = numpy.loadtxt('obstacle.txt')
self.flag = True
self.useOriginalCurPos = False
self.subTarg = Point()
self.side = 0
self.indexOfFirstObst = 0
self.minDisInSpline = 9000
self.obstList = self.obstList2point()
def obstList2point(self):
length = len(self.obstlist)
list = [Point() for i in range(length)]
for i in range(length):
list[i].x = self.obstlist[i][0]
list[i].y = self.obstlist[i][1]
list[i].z = 0.0
return list
def GetPointList(self, curPos, targetPos, safeDis):
subTargList = []
self.obstInUAVCoorSys = [Point() for i in range(len(self.obstList))]
tempStartPt = curPos
tempTargePos = targetPos
self.flag = True
self.useOriginalCurPos = False
while self.flag:
self.subTarg = Point()
if self.GetSubTarget(tempStartPt, tempTargePos, safeDis):
tempStartPt = Point()
tempStartPt.x = self.subTarg.x
tempStartPt.y = self.subTarg.y
subTargList.append(tempStartPt)
else:
self.flag = False
subTargList.append(tempTargePos)
print subTargList
return subTargList
def GetSubTarget(self, startPos, targetPos, safeDis):
curposToTarget = self.VectDiff(targetPos, startPos) # vector differential, point from the end of the second to that of the first.
if curposToTarget.x == 0.0 and curposToTarget.y == 0.0:
return False
lengthCurToTarget = math.sqrt(pow(curposToTarget.x, 2) + pow(curposToTarget.y, 2)) # distance from the start point to the target point.
ones = Point()
ones.x = 0
ones.y = 0
ones.z = 1
#convert coordinates of obstacles from the the global coordinate system to that of the uav
for k in range(len(self.obstList)): #698
temp1 = self.VectDiff(self.obstList[k], startPos)
self.obstInUAVCoorSys[k].x = self.VectDot(curposToTarget, temp1) / lengthCurToTarget # touying vector shuxiangji
self.obstInUAVCoorSys[k].y = self.VectCross(curposToTarget, temp1) / lengthCurToTarget # touying vector xiangliangji
#is true when calculate subtarget from current poistion of uav
if not self.useOriginalCurPos:
self.indexOfObstInSp = []
for j in range(len(self.obstInUAVCoorSys)):
if ((self.obstInUAVCoorSys[j].x > 0) and (self.obstInUAVCoorSys[j].x < lengthCurToTarget) and (abs(self.obstInUAVCoorSys[j].y) < safeDis)):
self.indexOfObstInSp.append(j)
#print 'self.obstList'+str(j)+':', self.obstList[j]
#print 'self.obstInUAVCoorSys'+str(j)+':', self.obstInUAVCoorSys[j]
if len(self.indexOfObstInSp) > 0:
self.indexOfFirstObst = self.indexOfObstInSp[0]
minDisInSpline = self.obstInUAVCoorSys[self.indexOfFirstObst].x
for i in range(len(self.indexOfObstInSp)):
if minDisInSpline > self.obstInUAVCoorSys[self.indexOfObstInSp[i]].x:
self.indexOfFirstObst = self.indexOfObstInSp[i]
#print 'self.indexOfFirstObst:', self.indexOfFirstObst
minDisInSpline = self.obstInUAVCoorSys[self.indexOfObstInSp[i]].x
#print 'self.obstList[self.indexOfObstInSp[i]]:', self.obstList[self.indexOfObstInSp[i]]
# the assemblage of obstacles that need to be avoid when avoid the first obstacle
# the content are the signals of obstacles in vector 'obstInUAVCoorSys'
G = []
numOfG = 0
G.append(self.indexOfFirstObst)
numOfG = numOfG + 1
for i in range(120): ###############cirletimes
if i < numOfG:
for j in range(len(self.obstList)):
if j == G[i]:
continue
elif self.VectNorm(self.obstList[j], self.obstList[G[i]]) <= 2*safeDis:
if j not in G:
G.append(j)
numOfG = numOfG + 1
maxPosEd = 0
maxNegEd = 0
#print "g0:", self.obstList[G[0]]
for i in range(numOfG):
if self.obstInUAVCoorSys[G[i]].y > 0:
#posEd[posNum++] = obstInUAVCoorSys[G[i]].y
if self.obstInUAVCoorSys[G[i]].y > maxPosEd:
maxPosEd = self.obstInUAVCoorSys[G[i]].y
elif self.obstInUAVCoorSys[G[i]].y < 0:
#negEd[negNum++] = fabs(obstInUAVCoorSys[G[i]].y)
if abs(self.obstInUAVCoorSys[G[i]].y) > maxNegEd:
maxNegEd = abs(self.obstInUAVCoorSys[G[i]].y)
if maxPosEd < maxNegEd:
#leftB = 1
sign_side = 1
# the maximum ed in the left side is smaller than that in the right side
else:
#rigthB = 1
sign_side = -1
alpha = [0 for i in range(numOfG)]
#numOfAlpha = 0
maxAlpha = 0
minAlpha = 10000
#the two index marks the signal of obstacle that has the maximun or minimum angle in G
maxAlphaIndex = 0
minAlphaIndex = 0
for i in range(numOfG):
alpha[i] = math.atan2(self.obstInUAVCoorSys[G[i]].y, self.obstInUAVCoorSys[G[i]].x) + sign_side * (math.asin((safeDis+0.1)/self.VectNorm(self.obstList[G[i]], startPos))+0.1) #0.1 for overshoot
if alpha[i] > maxAlpha:
maxAlpha = alpha[i]
maxAlphaIndex = int(i)
if alpha[i] < minAlpha:
minAlpha = alpha[i]
minAlphaIndex = int(i)
if sign_side == 1:
angle = maxAlpha
obstIndex = maxAlphaIndex
elif sign_side == -1:
angle = minAlpha
obstIndex = minAlphaIndex
try:
resu = self.VectNorm(self.obstList[G[obstIndex]], startPos) / lengthCurToTarget
except:
resu =
self.subTarg.y = startPos.y + (curposToTarget.y * math.cos(angle) + math.sin(angle) * curposToTarget.x) * resu
self.subTarg.x = startPos.x + (curposToTarget.x * math.cos(angle) - math.sin(angle) * curposToTarget.y) * resu
self.useOriginalCurPos = False
#print 'self.subTarg:', self.subTarg
return True
else:
return False
def VectDiff(self, endPt, startPt):
temp = Point()
temp.x = endPt.x - startPt.x
temp.y = endPt.y - startPt.y
return temp
def VectDot(self, Pt1, Pt2):
dot = Pt1.x * Pt2.x + Pt1.y * Pt2.y
return dot
def VectDot3D(self, Pt1, Pt2):
dot = Pt1.x * Pt2.x + Pt1.y * Pt2.y + Pt1.z * Pt2.z
return dot
def VectCross(self, vect1, vect2):
temp = vect1.x * vect2.y - vect1.y * vect2.x
return temp
def VectSign(self, Pt):
if abs(Pt.x) > 1e-5:
Pt.x = Pt.x / abs(Pt.x)
if abs(Pt.y) > 1e-5:
Pt.y = Pt.y / abs(Pt.y)
if abs(Pt.z) > 1e-5:
Pt.z = Pt.z / abs(Pt.z)
return Pt
def VectNorm(self, Pt1, Pt2):
norm = math.sqrt(pow(Pt1.x - Pt2.x, 2) + pow(Pt1.y - Pt2.y, 2))
return norm
# if __name__ == "__main__":
# curr = Point()
# curr.x = float(sys.argv[1])
# curr.y = float(sys.argv[2])
# targ = Point()
# targ.x = float(sys.argv[3])
# targ.y = float(sys.argv[4])
# avoid = ObstacleAviod()
# tar = avoid.GetPointList(curr, targ, 1)

256
control/actor/control_actortest.py Executable file
View File

@ -0,0 +1,256 @@
import rospy
import random
from geometry_msgs.msg import Point, Twist
from gazebo_msgs.srv import GetModelState
import sys
import numpy
import copy
from nav_msgs.msg import Odometry
from ObstacleAvoid import ObstacleAviod
class ControlActor:
def __init__(self, actor_id):
self.count = 0
self.shooting_count = 0
self.uav_num = 6
self.vehicle_type = 'typhoon_h480'
self.f = 10
self.flag = True
self.distance_flag = True
self.suitable_point = True
self.get_moving = False
self.x = 0.0
self.y = 0.0
self.x_max = 150.0
self.x_min = -50.0
self.y_max = 50.0
self.y_min = -50.0
self.id = actor_id
self.velocity = 1.5
self.last_pose = Point()
self.current_pose = Point()
self.target_pose = Point()
self.target_pose.z = 1.25
# obstacle avoidance:
self.Obstacleavoid = ObstacleAviod() #ji wu
self.avoid_finish_flag = True
self.subtarget_count = 0
self.subtarget_length = 0
self.subtarget_pos = []
self.arrive_count = 0
self.gazebo_actor_pose = Point()
self.gazebo_uav_pose = [Point() for i in range(self.uav_num)]
self.gazebo_uav_twist = [Point()for i in range(self.uav_num)]
self.dis_actor_uav = [0.0 for i in range(self.uav_num)] # distance between uav and actor
self.tracking_flag = [0 for i in range(self.uav_num)] # check if there is a uav tracking 'me'
self.catching_flag = 0 # if there is a uav tracking 'me' for a long time
self.catching_uav_num = 10 # get the number of uav of which is catching 'me'
#self.black_box = numpy.array([[[-34, -19], [16, 34]], [[5, 20], [10, 28]], [[53, 68], [13, 31]], [[70, 84], [8, 20]], [[86, 102], [10, 18]], [[77, 96], [22, 35]], [[52, 71], [-34, -25]], [[-6, 6], [-35, -20]], [[12, 40], [-20, -8]], [[-7, 8], [-21, -9]], [[-29, -22], [-16, -27]], [[-37, -30], [-27, -12]], [[-38, -24], [-36, -29]]])
self.black_box = [[[-35.375, -34.825], [-4, -3]], [[-3.275, -2.725], [-4, -3]], [[20.725, 21.275], [-4, -3]], \
[[57.725, 58.275], [-4, -3]], [[83.725, 84.275], [-4, -3]], [[-25.475, -24.925], [3, 4]], [[8.725, 9.275], [3, 4]], \
[[26.725, 27.275], [3, 4]], [[70.725, 71.275], [3, 4]], [[90.225, 90.775], [3, 4]],[[53.2, 54.2],[-9.0, -6.72]], \
[[79, 92], [-33.6, -28]],[[3, 8], [35, 36]], [[-34, -21.7], [18, 33]], [[4.5, 20.5], [11.5, 25]], [[55, 67.3], [15.5, 30.3]], \
[[71.7, 83.5], [9.5, 18.5]], [[88.3, 100.7], [11.9, 16.2]], [[79, 94.7], [22.2, 34.5]], [[53, 70], [-35, -27]], [[-5.4, 6.6], [-19.4, -10.6]], \
[[19.0, 38.5], [-22.0, -6.0]], [[-4.7, 4.4], [-33.4, -21.6]], [[-27.8, -23.5], [-27.5, -15]], [[-36, -24], [-35, -31]], \
[[-35.5, -31.3], [-25.5, -13]], [[3.0,8.2],[34.3,35.7]]]
self.box_num = len(self.black_box)
self.cmd_pub = rospy.Publisher('/actor_' + str(self.id) + '/cmd_pose', Point, queue_size=10)
#self.black_box = numpy.array(
# [[[-32, -21], [18, 32]], [[7, 18], [12, 26]], [[55, 66], [15, 29]], [[72, 82], [10, 18]],
# [[88, 100], [12, 16]], [[79, 94], [24, 33]], [[54, 69], [-34, -27]], [[-4, 4], [-33, -22]],
# [[14, 38], [-18, -10]], [[-7, 6], [-19, -11]], [[-27, -24], [-14, -29]], [[-35, -32], [-25, -14]],
# [[-36, -24], [-34, -31]]])
self.gazeboModelstate = rospy.ServiceProxy('gazebo/get_model_state', GetModelState)
print('actor_' + self.id + ": " + "communication initialized")
self.state_uav0_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+"_0/ground_truth/odom", Odometry, self.cmd_uav0_pose_callback)
self.state_uav1_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+"_1/ground_truth/odom", Odometry, self.cmd_uav1_pose_callback)
self.state_uav2_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+"_2/ground_truth/odom", Odometry, self.cmd_uav2_pose_callback)
self.state_uav3_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+"_3/ground_truth/odom", Odometry, self.cmd_uav3_pose_callback)
self.state_uav4_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+"_4/ground_truth/odom", Odometry, self.cmd_uav4_pose_callback)
self.state_uav5_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+"_5/ground_truth/odom", Odometry, self.cmd_uav5_pose_callback)
def cmd_uav0_pose_callback(self, msg):
self.gazebo_uav_pose[0] = msg.pose.pose.position
self.gazebo_uav_twist[0] = msg.twist.twist.linear
def cmd_uav1_pose_callback(self, msg):
self.gazebo_uav_pose[1] = msg.pose.pose.position
self.gazebo_uav_twist[1] = msg.twist.twist.linear
def cmd_uav2_pose_callback(self, msg):
self.gazebo_uav_pose[2] = msg.pose.pose.position
self.gazebo_uav_twist[2] = msg.twist.twist.linear
def cmd_uav3_pose_callback(self, msg):
self.gazebo_uav_pose[3] = msg.pose.pose.position
self.gazebo_uav_twist[3] = msg.twist.twist.linear
def cmd_uav4_pose_callback(self, msg):
self.gazebo_uav_pose[4] = msg.pose.pose.position
self.gazebo_uav_twist[4] = msg.twist.twist.linear
def cmd_uav5_pose_callback(self, msg):
self.gazebo_uav_pose[5] = msg.pose.pose.position
self.gazebo_uav_twist[5] = msg.twist.twist.linear
def loop(self):
rospy.init_node('actor_' + str(self.id))
rate = rospy.Rate(self.f)
while not rospy.is_shutdown():
self.count = self.count + 1
# get the pose of uav and actor
try:
get_actor_state = self.gazeboModelstate('actor_' + self.id, 'ground_plane')
self.last_pose = self.current_pose
self.gazebo_actor_pose = get_actor_state.pose.position
self.current_pose = self.gazebo_actor_pose
except rospy.ServiceException as e:
print("Gazebo model state service"+str(self.id)+" call failed: %s") % e
self.current_pose.x = 0.0
self.current_pose.y = 0.0
self.current_pose.z = 1.25
distance = (self.current_pose.x - self.target_pose.x) ** 2 + (
self.current_pose.y - self.target_pose.y) ** 2
if distance < 0.001:
self.distance_flag = True
self.catching_flag = 0
# collosion: if the actor is in the black box, then go backward and update a new target position
# update new random target position
if self.avoid_finish_flag and self.distance_flag:
while self.suitable_point:
while_time = 0
self.suitable_point = False
self.x = random.uniform(self.x_min, self.x_max)
self.y = random.uniform(self.y_min, self.y_max)
for i in range(self.box_num):
if (self.x > self.black_box[i][0][0]-1.5) and (self.x < self.black_box[i][0][1]+1.5):
if (self.y > self.black_box[i][1][0]-1.5) and (self.y < self.black_box[i][1][1]+1.5):
self.suitable_point = True
while_time = while_time+1
print str(self.id) + 'while time : ', while_time
break
self.target_pose.x = self.x
self.target_pose.y = self.y
self.flag = False
self.distance_flag = False
self.suitable_point = True
if self.target_pose.x < self.x_min:
self.target_pose.x = self.x_min
elif self.target_pose.x > self.x_max:
self.target_pose.x = self.x_max
if self.target_pose.y < self.y_min:
self.target_pose.y = self.y_min
elif self.target_pose.y > self.y_max:
self.target_pose.y = self.y_max
self.subtarget_pos = self.Obstacleavoid.GetPointList(self.current_pose, self.target_pose, 1) # current pose, target pose, safe distance
self.subtarget_length = len(self.subtarget_pos)
middd_pos = [Point() for k in range(self.subtarget_length)]
middd_pos = copy.deepcopy(self.subtarget_pos)
self.target_pose.x = copy.deepcopy(middd_pos[0].x)
self.target_pose.y = copy.deepcopy(middd_pos[0].y)
#self.avoid_start_flag = True
# print 'current_position: ' + str(self.id)+' ', self.current_pose
print 'middd_pos: '+ str(self.id)+' ', middd_pos
print '\n'
self.avoid_finish_flag = False
distance = (self.current_pose.x - self.target_pose.x) ** 2 + (
self.current_pose.y - self.target_pose.y) ** 2
if distance < 0.01:
self.arrive_count += 1
if self.arrive_count > 5:
self.distance_flag = True
self.arrive_count = 0
else:
self.distance_flag = False
else:
self.arrive_count = 0
self.distance_flag = False
if not self.avoid_finish_flag:
if self.distance_flag:
self.subtarget_count += 1
#self.distance_flag = False
if self.subtarget_count == self.subtarget_length:
self.avoid_finish_flag = True
self.subtarget_count = 0
else:
self.target_pose.x = middd_pos[self.subtarget_count].x
self.target_pose.y = middd_pos[self.subtarget_count].y
self.cmd_pub.publish(self.target_pose)
# # dodging uavs: if there is a uav catching 'me', escape
# for i in range(self.uav_num):
# self.dis_actor_uav[i] = ((self.current_pose.x-self.gazebo_uav_pose[i].x)**2+(self.current_pose.y-self.gazebo_uav_pose[i].y)**2)**0.5
# if self.dis_actor_uav[i] < 20.0 and (self.catching_flag == 0):
# self.tracking_flag[i] = self.tracking_flag[i]+1
# if self.tracking_flag[i] > 30: # 3s and excape
# self.catching_flag = 1
# self.tracking_flag[i] = 0
# self.catching_uav_num = i
# print('catch', self.id)
# print('catch', self.id)
# break
# if self.dis_actor_uav[i] < 20.0 and (self.catching_flag == 2):
# self.tracking_flag[i] = self.tracking_flag[i]+1
# if self.tracking_flag[i] > 50: # 5s and excape
# self.catching_flag = 0
# self.tracking_flag[i] = 0
# self.catching_uav_num = i
# print('catch', self.id)
# print('catch', self.id)
# break
# # # escaping (get a new target position)
# if self.catching_flag == 1:
# self.catching_flag = 2
# if (self.gazebo_uav_twist[self.catching_uav_num].x >= 0) and (self.gazebo_uav_twist[self.catching_uav_num].y >= 0):
# self.target_pose.x = self.current_pose.x-10000.0*self.gazebo_uav_twist[self.catching_uav_num].x
# self.target_pose.y = self.current_pose.y+10000.0*self.gazebo_uav_twist[self.catching_uav_num].y
# elif (self.gazebo_uav_twist[self.catching_uav_num].x >= 0) and (self.gazebo_uav_twist[self.catching_uav_num].y < 0):
# self.target_pose.x = self.current_pose.x+10.0*self.gazebo_uav_twist[self.catching_uav_num].x
# self.target_pose.y = self.current_pose.y+10.0*self.gazebo_uav_twist[self.catching_uav_num].y
# elif (self.gazebo_uav_twist[self.catching_uav_num].x < 0) and (self.gazebo_uav_twist[self.catching_uav_num].y >= 0):
# self.target_pose.x = self.current_pose.x-10.0*self.gazebo_uav_twist[self.catching_uav_num].x
# self.target_pose.y = self.current_pose.y-10.0*self.gazebo_uav_twist[self.catching_uav_num].y
# else:
# self.target_pose.x = self.current_pose.x+10.0*self.gazebo_uav_twist[self.catching_uav_num].x
# self.target_pose.y = self.current_pose.y-10.0*self.gazebo_uav_twist[self.catching_uav_num].y
# self.subtarget_pos = self.Obstacleavoid.GetPointList(self.current_pose, self.target_pose, 1) # current pose, target pose, safe distance
# self.subtarget_length = len(self.subtarget_pos)
# middd_pos = [Point() for k in range(self.subtarget_length)]
# middd_pos = copy.deepcopy(self.subtarget_pos)
# self.target_pose.x = copy.deepcopy(middd_pos[0].x)
# self.target_pose.y = copy.deepcopy(middd_pos[0].y)
# #self.avoid_start_flag = True
# # print 'current_position: ' + str(self.id)+' ', self.current_pose
# print 'middd_pos: '+ str(self.id)+' ', middd_pos
# print '\n'
# self.avoid_finish_flag = False
# check if the actor is shot:
if self.get_moving:
distance_change = (self.last_pose.x - self.current_pose.x)**2 + (self.last_pose.y - self.current_pose.y)**2
if distance_change < 0.00001:
self.shooting_count = self.shooting_count+1
if self.shooting_count > 1000:
print('shot', self.id)
print('shot', self.id)
print('shot', self.id)
print('shot', self.id)
print('shot', self.id)
print('shot', self.id)
self.distance_flag = False
self.suitable_point = True
self.avoid_finish_flag = True
self.shooting_count = 0
else:
self.shooting_count = 0
rate.sleep()
if __name__=="__main__":
controlactors = ControlActor(sys.argv[1])
controlactors.loop()

169
control/actor/delet_actor_demo_ml.py Normal file
View File

@ -0,0 +1,169 @@
#!/usr/bin/env python
import rospy
import math
from geometry_msgs.msg import Point
from std_msgs.msg import String
from gazebo_msgs.srv import DeleteModel,GetModelState
NODE_NAME = 'del_actor'
DAMAGE_RANGE = 15
MAX_LIFE_VALUE = 20
flag = [False for i in range(6)]
delta_time = [0 for i in range(6)]
judge_time = [0 for i in range(6)]
firsttime = [0 for i in range(6)]
actor_pos = [Point() for i in range(6)]
actor_real_pos = [Point() for i in range(6)]
delta_pose = [0.0 for i in range(6)]
#an uav can cause 2 damge value every second
CHECK_RATE = 1
exchange_flag = False
judge_flag = [False for i in range(6)]
file = open ('result2.txt','w')
'''
Here to set the vehicle name and number of actor and uav
'''
actor_num = 6
uav_num = 6
VEHICLE_NAME = 'typhoon_h480'
rospy.init_node(NODE_NAME)
rate = rospy.Rate(CHECK_RATE)
count = 0
def string2float(i):
return float(i)
def judge_callback(msg):
string_msg = msg.data
strlist = string_msg.split(' ')
id = int(strlist[0])
actor_pos[id].x = string2float(strlist[1])
actor_pos[id].y = string2float(strlist[2])
actor_pos[id].z = 0.0
judge_flag[id] = True
if not flag[id]:
firsttime[id] = rospy.get_time()
flag[id] = True
else:
delta_time[id] = rospy.get_time()-firsttime[id]
'''
ros surbcriber
'''
judger_sub = rospy.Subscriber('judger',String,judge_callback)
'''
publisher
'''
human_disappear_pub = rospy.Publisher('judger_react',String,queue_size = 10)
'''
ros services
'''
del_model = rospy.ServiceProxy(
"/gazebo/delete_model",DeleteModel
)
get_model_state = rospy.ServiceProxy(
"/gazebo/get_model_state",GetModelState
)
life_value_list = [MAX_LIFE_VALUE]*actor_num
def VectNorm2(Pt1, Pt2):
norm = math.sqrt(pow(Pt1.x - Pt2.x, 2) + pow(Pt1.y - Pt2.y, 2))
return norm
while not rospy.is_shutdown():
count += 1
exchange_flag = False
for actor_id in range(actor_num):
try:
get_actor_state = get_model_state('actor_' + str(actor_id), 'ground_plane')
actor_real_pos[actor_id] = get_actor_state.pose.position
except rospy.ServiceException as e:
print("Gazebo model state service"+str(actor_id)+" call failed: %s") % e
actor_real_pos[actor_id] = 0.0
actor_real_pos[actor_id] = 0.0
actor_real_pos[actor_id] = 1.25
if firsttime[actor_id] > 0:
print 'start !!!!!'
judge_time[actor_id] = rospy.get_time()-firsttime[actor_id]
if judge_flag[actor_id]:
judge_flag[actor_id] = False
delta_pose[actor_id] = VectNorm2(actor_real_pos[actor_id],actor_pos[actor_id])
if actor_id == 0:
actor_exchange_id = 5
if delta_pose[actor_id] > 10.0:
try:
get_actor_state = get_model_state('actor_' + str(actor_exchange_id), 'ground_plane')
actor_real_pos[actor_exchange_id] = get_actor_state.pose.position
delta_pose[actor_exchange_id] = VectNorm2(actor_real_pos[actor_exchange_id],actor_pos[actor_id])
except rospy.ServiceException as e:
print("Gazebo model state service"+str(actor_exchange_id)+" call failed: %s") % e
actor_real_pos[actor_exchange_id] = 0.0
actor_real_pos[actor_exchange_id] = 0.0
actor_real_pos[actor_exchange_id] = 1.25
if delta_pose[actor_exchange_id] < delta_pose[actor_id]:
exchange_flag = True
if not exchange_flag:
file.write(str(actor_id)+':')
file.write('\n')
file.write(str(actor_real_pos[actor_id]))
file.write('\n')
file.write(str(actor_pos[actor_id]))
file.write('\n')
file.write(str(delta_pose[actor_id]))
file.write('\n')
else:
file.write(str(actor_exchange_id)+':')
file.write('\n')
file.write(str(actor_real_pos[actor_exchange_id]))
file.write('\n')
file.write(str(actor_pos[actor_exchange_id]))
file.write('\n')
file.write(str(delta_pose[actor_exchange_id]))
file.write('\n')
file.write('\n')
if delta_time[actor_id] >=20:
if exchange_flag:
exchange_flag = False
del_model( 'actor_'+str(actor_exchange_id) )
print('actor_'+str(actor_exchange_id)+' was killed')
else:
del_model( 'actor_'+str(actor_id) )
print('actor_'+str(actor_id)+' was killed')
delta_time[actor_id] = 0
firsttime[actor_id] = 0
judge_time[actor_id] = 0
human_disappear_pub.publish(str(actor_id))
if judge_time[actor_id] > 25:
delta_time[actor_id] = 0
firsttime[actor_id] = 0
judge_time[actor_id] = 0
#display the life value of all actors
#for actor_id in range(actor_num):
#print("actor_"+str(actor_id)+f"::{life_value_list[actor_id]}", end=" ")
#print(" ")
rate.sleep()
file.close()

0
control/actor/delet_actor_demo.py → control/actor/delet_actor_demo_old.py
View File

1857
control/actor/obstacle.txt Normal file
View File

File diff suppressed because it is too large Load Diff

8
control/actor/run_actor test.sh Executable file
View File

@ -0,0 +1,8 @@
#!/bin/bash
python control_actortest.py 1 &
python control_actortest.py 2 &
python control_actortest.py 3 &
python control_actortest.py 4 &
python control_actortest.py 5 &
python control_actortest.py 0

2
control/actor/run_actor.sh
View File

@ -4,5 +4,5 @@ python control_actor.py 2 &
python control_actor.py 3 & python control_actor.py 3 &
python control_actor.py 4 & python control_actor.py 4 &
python control_actor.py 5 & python control_actor.py 5 &
python control_actor.py 6 python control_actor.py 0

946
control/result1.txt Normal file
View File

@ -0,0 +1,946 @@
x: -17.0694522667
y: -14.7671283053
z: 1.0191
x: 0.0
y: 0.0
z: 0.0
4.4580771754
3:
x: 35.0347294088
y: 0.25391516057
z: 1.0191
x: 33.488
y: -0.09
z: 0.0
1.58450291947
0:
x: -16.0199946707
y: -19.8511239018
z: 1.0191
x: -28.158
y: -32.314
z: 0.0
17.396966805
3:
x: 36.194966315
y: 0.705284256726
z: 1.0191
x: 36.04
y: 0.676
z: 0.0
0.157708993001
0:
x: -15.2241867047
y: -18.8809676207
z: 1.0191
x: -28.158
y: -32.314
z: 0.0
17.396966805
3:
x: 37.3645164233
y: 1.16027648501
z: 1.0191
x: 35.935
y: 0.877
z: 0.0
1.45731354598
0:
x: -14.4347490857
y: -17.9185773238
z: 1.0191
x: -28.158
y: -32.314
z: 0.0
17.396966805
3:
x: 38.5247404325
y: 1.61164056387
z: 1.0191
x: 36.199
y: 1.533
z: 0.0
2.32706959454
0:
x: -13.6421816557
y: -16.9523715262
z: 1.0191
x: -10.751
y: -14.953
z: 0.0
3.51516967813
3:
x: 39.6942751476
y: 2.06662680373
z: 1.0191
x: 39.847
y: 1.653
z: 0.0
0.440921776842
0:
x: -12.8465030552
y: -15.9823729525
z: 1.0191
x: -10.751
y: -14.953
z: 0.0
3.51516967813
3:
x: 40.8544808927
y: 2.51798377721
z: 1.0191
x: 40.37
y: 2.429
z: 0.0
0.492584863705
0:
x: -12.0572534422
y: -15.0202118507
z: 1.0191
x: -9.304
y: -13.158
z: 0.0
3.32388891119
3:
x: 42.0193339484
y: 2.97114870251
z: 1.0191
x: 40.37
y: 2.429
z: 0.0
0.492584863705
0:
x: -11.2649792313
y: -14.0543635116
z: 1.0191
x: -9.304
y: -13.158
z: 0.0
3.32388891119
3:
x: 43.1795126761
y: 3.4224951654
z: 1.0191
x: 40.37
y: 2.429
z: 0.0
0.492584863705
0:
x: -10.47296569
y: -13.0888329507
z: 1.0191
x: -9.304
y: -13.158
z: 0.0
3.32388891119
3:
x: 44.3489912326
y: 3.87745955776
z: 1.0191
x: 40.37
y: 2.429
z: 0.0
0.492584863705
0:
x: -9.67832993912
y: -12.1201056975
z: 1.0191
x: -8.876
y: -12.582
z: 0.0
0.925785978431
3:
x: 45.5184455949
y: 4.33241453784
z: 1.0191
x: 45.586
y: 4.472
z: 0.0
0.155073204954
0:
x: -8.89147238271
y: -11.1608607122
z: 1.0191
x: -8.876
y: -12.582
z: 0.0
0.925785978431
3:
x: 46.6785504093
y: 4.78373224609
z: 1.0191
x: 45.958
y: 4.587
z: 0.0
0.746924674232
x: -19.231019089
y: -28.3404754784
z: 1.0191
x: 0.0
y: 0.0
z: 0.0
4.20093428296
3:
x: 47.8432724539
y: 5.2368462039
z: 1.0191
x: 47.716
y: 5.19
z: 0.0
0.135620220983
x: -19.428375274
y: -29.5797543145
z: 1.0191
x: 0.0
y: 0.0
z: 0.0
6.7834752137
3:
x: 49.0125951357
y: 5.69174995607
z: 1.0191
x: 48.285
y: 5.354
z: 0.0
0.802165640236
x: -19.6249426026
y: -30.8140796045
z: 1.0191
x: 0.0
y: 0.0
z: 0.0
3.45928248665
3:
x: 50.1771757434
y: 6.14480889042
z: 1.0191
x: 48.285
y: 5.354
z: 0.0
0.802165640236
x: -19.8207204557
y: -32.0434474598
z: 1.0191
x: 0.0
y: 0.0
z: 0.0
26.175055747
3:
x: 51.3369729643
y: 6.59600693498
z: 1.0191
x: 48.285
y: 5.354
z: 0.0
0.802165640236
x: -20.0172804527
y: -33.2777267115
z: 1.0191
x: 0.0
y: 0.0
z: 0.0
13.3470597191
3:
x: 52.501225178
y: 7.04893811361
z: 1.0191
x: 51.433
y: 6.565
z: 0.0
1.172732334640
0:
x: -4.52394781642
y: -8.18522710237
z: 1.0191
x: -27.139
y: -44.566
z: 0.0
41.8139851334
3:
x: 53.6651168968
y: 7.50172904825
z: 1.0191
x: 53.269
y: 7.391
z: 0.0
0.411302222291
0:
x: -3.37798209739
y: -7.68603455277
z: 1.0191
x: -27.139
y: -44.566
z: 0.0
41.8139851334
3:
x: 54.8282720676
y: 7.95423344226
z: 1.0191
x: 54.475
y: 7.791
z: 0.0
0.389161034029
x: -20.6069198776
y: -36.9803097355
z: 1.0191
x: 0.0
y: 0.0
z: 0.0
10.7035282646
3:
x: 55.9895173218
y: 8.4059948179
z: 1.0191
x: 55.592
y: 8.196
z: 0.0
0.44957518243
0:
x: -1.08605670942
y: -6.68765208906
z: 1.0191
x: -26.67
y: -45.801
z: 0.0
45.6975719402
3:
x: 57.1424988597
y: 8.85454134481
z: 1.0191
x: 56.859
y: 8.713
z: 0.0
0.316868357096
x: -20.9999440467
y: -39.4482665151
z: 1.0191
x: 0.0
y: 0.0
z: 0.0
2.10106674309
3:
x: 57.9457896983
y: 9.16704705622
z: 1.0191
x: 57.763
y: 9.177
z: 0.0
0.183060467833
2:
x: 100.0
y: 36.6716431751
z: 1.0191
x: 100.248
y: 36.823
z: 0.0
0.290538961983
2:
x: 100.0
y: 36.7404035624
z: 1.0191
x: 100.248
y: 36.823
z: 0.0
0.290538961983
2:
x: 100.0
y: 36.8069961061
z: 1.0191
x: 100.248
y: 36.823
z: 0.0
0.290538961983
2:
x: 100.0
y: 36.8712126913
z: 1.0191
x: 99.777
y: 37.216
z: 0.0
0.410618178149
2:
x: 100.0
y: 36.9326447898
z: 1.0191
x: 100.117
y: 36.93
z: 0.0
0.117029888972
2:
x: 100.0
y: 36.9916522987
z: 1.0191
x: 100.035
y: 37.153
z: 0.0
0.165100214157
2:
x: 100.0
y: 37.0481081009
z: 1.0191
x: 99.886
y: 37.193
z: 0.0
0.184362855361
2:
x: 100.0
y: 37.1027701966
z: 1.0191
x: 99.886
y: 37.193
z: 0.0
0.184362855361
2:
x: 100.0
y: 37.1550598971
z: 1.0191
x: 99.886
y: 37.193
z: 0.0
0.184362855361
2:
x: 100.0
y: 37.2052838931
z: 1.0191
x: 99.886
y: 37.193
z: 0.0
0.184362855361
2:
x: 100.0
y: 37.2535234094
z: 1.0191
x: 99.886
y: 37.193
z: 0.0
0.184362855361
2:
x: 100.0
y: 37.3000381267
z: 1.0191
x: 99.886
y: 37.193
z: 0.0
0.184362855361
2:
x: 100.0
y: 37.3443582682
z: 1.0191
x: 99.886
y: 37.193
z: 0.0
0.184362855361
2:
x: 100.0
y: 37.3871007736
z: 1.0191
x: 99.886
y: 37.193
z: 0.0
0.184362855361
2:
x: 100.0
y: 37.4281533483
z: 1.0191
x: 99.886
y: 37.193
z: 0.0
0.184362855361
2:
x: 100.0
y: 37.4675825999
z: 1.0191
x: 99.886
y: 37.193
z: 0.0
0.184362855361
2:
x: 100.0
y: 37.5054525342
z: 1.0191
x: 99.886
y: 37.193
z: 0.0
0.184362855361
2:
x: 100.0
y: 37.5419672154
z: 1.0191
x: 99.886
y: 37.193
z: 0.0
0.184362855361
2:
x: 100.0
y: 37.5767580569
z: 1.0191
x: 99.886
y: 37.193
z: 0.0
0.184362855361
2:
x: 100.0
y: 37.6103095223
z: 1.0191
x: 99.886
y: 37.193
z: 0.0
0.184362855361
2:
x: 100.0
y: 37.6425336053
z: 1.0191
x: 100.215
y: 37.728
z: 0.0
0.231364441121
4:
x: 100.0
y: -46.9203281111
z: 1.0191
x: 89.441
y: -49.794
z: 0.0
10.9430558404
4:
x: 100.0
y: -46.9203379272
z: 1.0191
x: 102.152
y: -46.035
z: 0.0
2.32699962297
4:
x: 100.0
y: -46.9203470716
z: 1.0191
x: 100.581
y: -47.063
z: 0.0
0.598256515196
4:
x: 100.0
y: -46.920355555
z: 1.0191
x: 100.883
y: -46.093
z: 0.0
1.21004388948
4:
x: 100.0
y: -46.9203634555
z: 1.0191
x: 100.332
y: -47.053
z: 0.0
0.357514269551
4:
x: 100.0
y: -46.9203707265
z: 1.0191
x: 100.362
y: -47.037
z: 0.0
0.380324055837
4:
x: 100.0
y: -46.9203774999
z: 1.0191
x: 100.377
y: -46.88
z: 0.0
0.379156092525
4:
x: 100.0
y: -46.9203837837
z: 1.0191
x: 100.354
y: -46.949
z: 0.0
0.355154737874
4:
x: 100.0
y: -46.9203896357
z: 1.0191
x: 100.402
y: -46.865
z: 0.0
0.405797993769
4:
x: 100.0
y: -46.9203950214
z: 1.0191
x: 100.388
y: -46.834
z: 0.0
0.397502326697
4:
x: 100.0
y: -46.9204000387
z: 1.0191
x: 100.281
y: -46.899
z: 0.0
0.28181370026
4:
x: 100.0
y: -46.9204047111
z: 1.0191
x: 100.246
y: -46.899
z: 0.0
0.246929466969
4:
x: 100.0
y: -46.9204090112
z: 1.0191
x: 100.263
y: -46.864
z: 0.0
0.268981368405
4:
x: 100.0
y: -46.9204130172
z: 1.0191
x: 100.237
y: -46.852
z: 0.0
0.246676591747
4:
x: 100.0
y: -46.9204167478
z: 1.0191
x: 100.422
y: -47.088
z: 0.0
0.454057426343
4:
x: 100.0
y: -46.9204201812
z: 1.0191
x: 100.241
y: -46.859
z: 0.0
0.248703515568
4:
x: 100.0
y: -46.920423392
z: 1.0191
x: 100.288
y: -46.877
z: 0.0
0.291255199042
4:
x: 100.0
y: -46.9204263583
z: 1.0191
x: 100.3
y: -46.955
z: 0.0
0.301985656447
4:
x: 100.0
y: -46.9204291102
z: 1.0191
x: 100.403
y: -46.907
z: 0.0
0.403223686062
4:
x: 100.0
y: -46.9204316534
z: 1.0191
x: 100.37
y: -46.918
z: 0.0
0.370007990371
4:
x: 100.0
y: -46.9204340226
z: 1.0191
x: 100.37
y: -46.944
z: 0.0
0.370749720557
1:
x: 66.9519002465
y: -14.3264608673
z: 1.0191
x: 66.715
y: -13.125
z: 0.0
1.22459378666
1:
x: 68.1748130035
y: -14.6083811854
z: 1.0191
x: 66.715
y: -13.125
z: 0.0
1.22459378666
1:
x: 69.3879808605
y: -14.8880549939
z: 1.0191
x: 69.762
y: -14.318
z: 0.0
0.681801300058
1:
x: 70.6060202576
y: -15.1688518475
z: 1.0191
x: 70.751
y: -15.249
z: 0.0
0.165658842402
1:
x: 71.8240589852
y: -15.4496485467
z: 1.0191
x: 72.042
y: -15.581
z: 0.0
0.254463141234
1:
x: 73.0420969898
y: -15.7304450792
z: 1.0191
x: 73.133
y: -15.803
z: 0.0
0.11630809856
1:
x: 74.2650063595
y: -16.0123646165
z: 1.0191
x: 74.336
y: -16.175
z: 0.0
0.177455247742
1:
x: 75.4830427305
y: -16.2931607724
z: 1.0191
x: 75.553
y: -16.349
z: 0.0
0.0895099932745
1:
x: 76.6962060397
y: -16.5728335325
z: 1.0191
x: 76.647
y: -16.582
z: 0.0
0.0500525570232
1:
x: 77.9142404887
y: -16.8536292454
z: 1.0191
x: 78.067
y: -16.89
z: 0.0
0.157029615314
1:
x: 79.1371459726
y: -17.1355478869
z: 1.0191
x: 79.238
y: -17.311
z: 0.0
0.202373364969
1:
x: 80.3503059941
y: -17.415219889
z: 1.0191
x: 80.368
y: -17.604
z: 0.0
0.189607510783
1:
x: 81.5732089493
y: -17.6971379475
z: 1.0191
x: 81.515
y: -17.733
z: 0.0
0.0683693541467
1:
x: 82.786366212
y: -17.9768093137
z: 1.0191
x: 82.6
y: -18.139
z: 0.0
0.24705906925
1:
x: 84.0043939916
y: -18.2576034891
z: 1.0191
x: 83.981
y: -18.291
z: 0.0
0.0407750632672
1:
x: 85.2224199932
y: -18.5383972545
z: 1.0191
x: 85.213
y: -18.77
z: 0.0
0.23179423629
1:
x: 86.4453161082
y: -18.8203137362
z: 1.0191
x: 86.36
y: -18.917
z: 0.0
0.128946003931
1:
x: 87.6584658294
y: -19.0999833638
z: 1.0191
x: 87.567
y: -19.207
z: 0.0
0.140778401633
1:
x: 88.8764851627
y: -19.380775592
z: 1.0191
x: 88.774
y: -19.439
z: 0.0
0.117869802117
1:
x: 90.0993737612
y: -19.6626903409
z: 1.0191
x: 89.406
y: -19.581
z: 0.0
0.698169380973
1:
x: 91.3125150739
y: -19.9423580301
z: 1.0191
x: 90.524
y: -19.812
z: 0.0
0.799217891343
1:
x: 92.5305248415
y: -20.2231480531
z: 1.0191
x: 92.669
y: -20.351
z: 0.0
0.188471456338

1017
control/result2.txt Normal file
View File

File diff suppressed because it is too large Load Diff

1037
coordination/launch_generator/multi_vehicle_ppt.launch Normal file
View File

File diff suppressed because it is too large Load Diff

196
coordination/launch_generator/multi_vehicle_typh.launch Normal file
View File

@ -0,0 +1,196 @@
<?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="world" default="$(find mavlink_sitl_gazebo)/worlds/robocup.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>
<!-- typhoon_h480_0 -->
<group ns="typhoon_h480_0">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="0"/>
<arg name="ID_in_group" value="0"/>
<arg name="fcu_url" default="udp://:24540@localhost: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="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"/>
<arg name="mavlink_udp_port" value="18570"/>
<arg name="mavlink_tcp_port" value="4560"/>
<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="1"/>
<arg name="ID_in_group" value="1"/>
<arg name="fcu_url" default="udp://:24541@localhost:34581"/>
<!-- 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"/>
<arg name="mavlink_udp_port" value="18571"/>
<arg name="mavlink_tcp_port" value="4561"/>
<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="2"/>
<arg name="ID_in_group" value="2"/>
<arg name="fcu_url" default="udp://:24542@localhost:34582"/>
<!-- 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="typhoon_h480"/>
<arg name="sdf" value="typhoon_h480"/>
<arg name="mavlink_udp_port" value="18572"/>
<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>
<!-- typhoon_h480_3 -->
<group ns="typhoon_h480_3">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="3"/>
<arg name="ID_in_group" value="3"/>
<arg name="fcu_url" default="udp://:24543@localhost:34583"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.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="typhoon_h480"/>
<arg name="sdf" value="typhoon_h480"/>
<arg name="mavlink_udp_port" value="18573"/>
<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_4 -->
<group ns="typhoon_h480_4">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="4"/>
<arg name="ID_in_group" value="4"/>
<arg name="fcu_url" default="udp://:24544@localhost:34584"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="3"/>
<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="typhoon_h480"/>
<arg name="sdf" value="typhoon_h480"/>
<arg name="mavlink_udp_port" value="18574"/>
<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_5 -->
<group ns="typhoon_h480_5">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="5"/>
<arg name="ID_in_group" value="5"/>
<arg name="fcu_url" default="udp://:24545@localhost:34585"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="6"/>
<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="typhoon_h480"/>
<arg name="sdf" value="typhoon_h480"/>
<arg name="mavlink_udp_port" value="18575"/>
<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>
</launch>
<!--the launch file is generated by XTDrone multi-vehicle generator.py -->

2
coordination/task_assignment

@ -1 +1 @@
Subproject commit 57e251bbc6d889743c0ce3f5b4552a68c0fdc0e4 Subproject commit 5586cb84650f3b384305f074b4eb658afd8132bf

2
sensing/object_detection_and_tracking/darknet_ros

@ -1 +1 @@
Subproject commit b5ae1e64a25dab8a19b6c49bb0b9417b5d1ff6de Subproject commit 05a7d63026ae8bc9a83f0bf9c6ad35e39707fff6