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 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
|
||||||
|
|
||||||
|
|
|
@ -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