forked from xtdrone/XTDrone
modify code of actor control
This commit is contained in:
parent
5500f67495
commit
d280b2f7bd
|
@ -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
|
|
@ -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
|
|
@ -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)
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -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()
|
|
@ -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()
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
File diff suppressed because it is too large
Load Diff
|
@ -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
|
||||
|
|
@ -4,5 +4,5 @@ python control_actor.py 2 &
|
|||
python control_actor.py 3 &
|
||||
python control_actor.py 4 &
|
||||
python control_actor.py 5 &
|
||||
python control_actor.py 6
|
||||
python control_actor.py 0
|
||||
|
||||
|
|
|
@ -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
|
||||
|
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
|
@ -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 -->
|
|
@ -1 +1 @@
|
|||
Subproject commit 57e251bbc6d889743c0ce3f5b4552a68c0fdc0e4
|
||||
Subproject commit 5586cb84650f3b384305f074b4eb658afd8132bf
|
|
@ -1 +1 @@
|
|||
Subproject commit b5ae1e64a25dab8a19b6c49bb0b9417b5d1ff6de
|
||||
Subproject commit 05a7d63026ae8bc9a83f0bf9c6ad35e39707fff6
|
Loading…
Reference in New Issue