forked from xtdrone/XTDrone
update robocup.world; add an actor; update controlactortest
This commit is contained in:
parent
78008359b5
commit
da226e6ed5
|
|
@ -8,8 +8,8 @@ import numpy
|
|||
class ControlActor:
|
||||
def __init__(self, actor_id):
|
||||
self.f = 100
|
||||
self.x = [0.0 for i in range(5)]
|
||||
self.y = [0.0 for i in range(5)]
|
||||
self.x = [0.0 for i in range(6)]
|
||||
self.y = [0.0 for i in range(6)]
|
||||
self.x_max = 100.0
|
||||
self.x_min = -50.0
|
||||
self.y_max = 50.0
|
||||
|
|
@ -18,15 +18,20 @@ class ControlActor:
|
|||
self.target_pose = Point()
|
||||
self.target_pose.z = 1.25
|
||||
self.cmd_pub = rospy.Publisher('/actor_' + str(self.id) + '/cmd_pose', Point, queue_size=10)
|
||||
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]]])
|
||||
print('actor_' + self.id + ": " + "communication initialized")
|
||||
#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 = numpy.array(
|
||||
[[[-33, -21], [18, 33]], [[5, 20], [11, 25]], [[54, 68], [15, 31]], [[71, 83], [9, 17]],
|
||||
[[88, 100], [11, 16]], [[78, 95], [22, 35]], [[53, 70], [-35, -27]], [[-6, 7], [-18, -10]],
|
||||
[[20, 38], [-18, -10]], [[-5, 5], [-34, -21]], [[-28, -23], [-28, -15]], [[-36, -23], [-35, -30]],
|
||||
[[-36, -31], [-26, -13]], [[15, 18], [-16, -12]]])
|
||||
print('actor' + self.id + ": " + "communication initialized")
|
||||
|
||||
def loop(self):
|
||||
rospy.init_node('actor_' + str(self.id))
|
||||
rate = rospy.Rate(self.f)
|
||||
#target_pose needs to meet x_max,x_min,y_max,y_min and make sure that the target is not in the black box.
|
||||
self.x = [0.0, 2.0, 2.0, -2.0, -2.0]
|
||||
self.y = [0.0, 2.0, -2.0, 2.0, -2.0]
|
||||
self.x = [-34.0, -38.0, 2.0, -2.0, -2.0, 0.0]
|
||||
self.y = [34.0, -36.0, -2.0, 2.0, -2.0, 0.0]
|
||||
for i in range(5):
|
||||
if self.x[i] > self.x_max:
|
||||
self.x[i] = self.x_max
|
||||
|
|
@ -44,4 +49,4 @@ class ControlActor:
|
|||
|
||||
if __name__=="__main__":
|
||||
controlactors = ControlActor(sys.argv[1])
|
||||
controlactors.loop()
|
||||
controlactors.loop()
|
||||
|
|
|
|||
|
|
@ -36,7 +36,13 @@ class ControlActor:
|
|||
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 = 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 = numpy.array(
|
||||
[[[-33, -21], [18, 33]], [[5, 20], [11, 25]], [[54, 68], [15, 31]], [[71, 83], [9, 17]],
|
||||
[[88, 100], [11, 16]], [[78, 95], [22, 35]], [[53, 70], [-35, -27]], [[-6, 7], [-18, -10]],
|
||||
[[20, 38], [-18, -10]], [[-5, 5], [-34, -21]], [[-28, -23], [-28, -15]], [[-36, -23], [-35, -30]],
|
||||
[[-36, -31], [-26, -13]], [[15, 18], [-16, -12]]])
|
||||
self.box_num = 14;
|
||||
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]],
|
||||
|
|
@ -92,15 +98,22 @@ class ControlActor:
|
|||
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.flag or self.distance_flag:
|
||||
self.x = random.uniform(self.x_min, self.x_max)
|
||||
self.y = random.uniform(self.y_min, self.y_max)
|
||||
self.flag = False
|
||||
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:
|
||||
|
|
@ -110,30 +123,48 @@ class ControlActor:
|
|||
elif self.target_pose.y > self.y_max:
|
||||
self.target_pose.y = self.y_max
|
||||
self.cmd_pub.publish(self.target_pose)
|
||||
if self.count % 20 == 0:
|
||||
if self.count % 100 == 0:
|
||||
print('current_pose' + self.id+':', self.current_pose)
|
||||
#print('last_pose' + self.id + ':', self.last_pose)
|
||||
print('target_pose' + self.id+':', self.target_pose)
|
||||
print('uav_pose:', self.gazebo_uav_pose[0])
|
||||
print('uav_vel:', self.gazebo_uav_twist[0])
|
||||
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
|
||||
#print('uav_pose:', self.gazebo_uav_pose[0])
|
||||
#print('uav_vel:', self.gazebo_uav_twist[0])
|
||||
|
||||
if self.suitable_point:
|
||||
for i in range(13):
|
||||
for i in range(self.box_num):
|
||||
if (self.current_pose.x > self.black_box[i][0][0]) and (self.current_pose.x < self.black_box[i][0][1]):
|
||||
if (self.current_pose.y > self.black_box[i][1][0]) and (self.current_pose.y < self.black_box[i][1][1]):
|
||||
self.target_pose.x = self.current_pose.x + 200*(self.last_pose.x-self.current_pose.x)
|
||||
self.target_pose.y = self.current_pose.y + 200*(self.last_pose.y-self.current_pose.y)
|
||||
delta_x = self.last_pose.x - self.current_pose.x
|
||||
delta_y = self.last_pose.y - self.current_pose.y
|
||||
if (delta_x >= 0) and (delta_y >= 0):
|
||||
self.target_pose.x = self.current_pose.x + 100.0 * delta_x
|
||||
self.target_pose.y = self.current_pose.y - 100.0 * delta_y+1
|
||||
elif (delta_x >= 0) and (delta_y < 0):
|
||||
self.target_pose.x = self.current_pose.x - 100.0 * delta_x
|
||||
self.target_pose.y = self.current_pose.y + 100.0 * delta_y+1
|
||||
elif (delta_x < 0) and (delta_y >= 0):
|
||||
self.target_pose.x = self.current_pose.x - 100.0 * delta_x
|
||||
self.target_pose.y = self.current_pose.y + 100.0 * delta_y+1
|
||||
else:
|
||||
self.target_pose.x = self.current_pose.x + 100.0 * delta_x
|
||||
self.target_pose.y = self.current_pose.y - 100.0 * delta_y+1
|
||||
for j in range(self.box_num):
|
||||
if (self.target_pose.x > self.black_box[j][0][0]) and (
|
||||
self.target_pose.x < self.black_box[j][0][1]):
|
||||
if (self.target_pose.y > self.black_box[j][1][0]) and (
|
||||
self.target_pose.y < self.black_box[j][1][1]):
|
||||
self.target_pose.x = self.current_pose.x + 100.0 * delta_x
|
||||
self.target_pose.y = self.current_pose.y + 100.0 * delta_y
|
||||
print('nonononono')
|
||||
break
|
||||
self.suitable_point = False
|
||||
self.get_moving = True
|
||||
print('wall!!!!')
|
||||
print('wall!!!!')
|
||||
print('wall!!!!')
|
||||
print('wall!!!!')
|
||||
print('wall!!!!')
|
||||
print('wall!!!!')
|
||||
print('wall!!!!', self.id)
|
||||
print('wall!!!!', self.id)
|
||||
print('wall!!!!', self.id)
|
||||
print('wall!!!!', self.id)
|
||||
print('wall!!!!', self.id)
|
||||
print('wall!!!!', self.id)
|
||||
break
|
||||
# dodging uavs: if there is a uav catching 'me', escape
|
||||
for i in range(self.uav_num):
|
||||
|
|
@ -156,11 +187,11 @@ class ControlActor:
|
|||
if self.catching_flag == 1:
|
||||
self.catching_flag = 2
|
||||
if self.suitable_point:
|
||||
print('escaping')
|
||||
print('escaping')
|
||||
print('escaping')
|
||||
print('escaping')
|
||||
print('escaping')
|
||||
print('escaping', self.id)
|
||||
print('escaping', self.id)
|
||||
print('escaping', self.id)
|
||||
print('escaping', self.id)
|
||||
print('escaping', self.id)
|
||||
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-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
|
||||
|
|
@ -169,7 +200,7 @@ class ControlActor:
|
|||
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].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
|
||||
|
|
@ -183,13 +214,13 @@ class ControlActor:
|
|||
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 > 100:
|
||||
print('shot')
|
||||
print('shot')
|
||||
print('shot')
|
||||
print('shot')
|
||||
print('shot')
|
||||
print('shot')
|
||||
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.x = random.uniform(self.x_min, self.x_max)
|
||||
self.y = random.uniform(self.y_min, self.y_max)
|
||||
self.target_pose.x = self.x
|
||||
|
|
|
|||
|
|
@ -3,5 +3,6 @@ python control_actor.py 1 &
|
|||
python control_actor.py 2 &
|
||||
python control_actor.py 3 &
|
||||
python control_actor.py 4 &
|
||||
python control_actor.py 5
|
||||
python control_actor.py 5 &
|
||||
python control_actor.py 6
|
||||
|
||||
|
|
|
|||
|
|
@ -3,5 +3,6 @@ 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 5 &
|
||||
python control_actortest.py 6
|
||||
|
||||
|
|
|
|||
|
|
@ -1,7 +0,0 @@
|
|||
#!/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
|
||||
|
||||
|
|
@ -1 +0,0 @@
|
|||
kill -9 $(ps -ef|grep control_actortest.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ')
|
||||
File diff suppressed because it is too large
Load Diff
Loading…
Reference in New Issue