jasder
/
XTDrone
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

new control_actors and uav catching plugin

This commit is contained in:
ma_lan 2020-09-18 10:49:23 +08:00
parent d34b7939bd
commit 262e398775
21 changed files with 649 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
control/actor/control_actor.py Executable file
View File

@ -0,0 +1,47 @@
import rospy
import random
from geometry_msgs.msg import Point
from gazebo_msgs.srv import GetModelState
import sys
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_max = 100.0
self.x_min = -50.0
self.y_max = 50.0
self.y_min = -50.0
self.id = actor_id
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")
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]
for i in range(5):
if self.x[i] > self.x_max:
self.x[i] = self.x_max
if self.x[i] < self.x_min:
self.x[i] = self.x_min
if self.y[i] > self.y_max:
self.y[i] = self.y_max
if self.y[i] < self.y_min:
self.y[i] = self.y_min
while not rospy.is_shutdown():
self.target_pose.x = self.x[int(self.id)-1]
self.target_pose.y = self.y[int(self.id)-1]
self.cmd_pub.publish(self.target_pose)
rate.sleep()
if __name__=="__main__":
controlactors = ControlActor(sys.argv[1])
controlactors.loop()

205
control/actor/control_actor_1.py Executable file
View File

@ -0,0 +1,205 @@
import rospy
import random
from geometry_msgs.msg import Point, Twist
from gazebo_msgs.srv import GetModelState
import sys
import numpy
from nav_msgs.msg import Odometry
class ControlActor:
def __init__(self, actor_id):
self.count = 0
self.die_count = 0
self.uav_num = 1
self.vehicle_type = 'iris'
self.f = 100
self.flag = True
self.distance_flag = False
self.suitable_point = True
self.get_moving = False
self.x = 0.0
self.y = 0.0
self.x_max = 100.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
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.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 call failed: %s") % e
self.current_pose.x = 0.0
self.current_pose.y = 0.0
self.current_pose.z = 1.25
# 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.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.cmd_pub.publish(self.target_pose)
if self.count % 20 == 0:
print('current_pose' + self.id+':', self.current_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
if self.suitable_point:
for i in range(13):
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)
self.suitable_point = False
self.get_moving = True
print('wall!!!!')
print('wall!!!!')
print('wall!!!!')
print('wall!!!!')
print('wall!!!!')
print('wall!!!!')
break
# 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
if self.dis_actor_uav[i] < 400.0 and (self.catching_flag == 0):
self.tracking_flag[i] = self.tracking_flag[i]+1
if self.tracking_flag[i] > 20:
self.catching_flag = 1
self.tracking_flag[i] = 0
self.catching_uav_num = i
print('catch you!', self.id)
print('catch you!', self.id)
print('catch you!', self.id)
print('catch you!', self.id)
print('catch you!', self.id)
print('catch you!', self.id)
print('catch you!', self.id)
break
# escaping
if self.catching_flag == 1:
self.catching_flag = 2
if self.suitable_point:
print('escaping')
print('escaping')
print('escaping')
print('escaping')
print('escaping')
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
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].x
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
if self.target_pose == self.current_pose:
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
self.target_pose.y = self.y
# check if the actor is died:
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.die_count = self.die_count+1
if self.die_count > 100:
print('you are died')
print('you are died')
print('you are died')
print('you are died')
print('you are died')
print('you are died')
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
self.target_pose.y = self.y
self.die_count = 0
else:
self.die_count = 0
rate.sleep()
if __name__=="__main__":
controlactors = ControlActor(sys.argv[1])
controlactors.loop()

150
control/actor/control_actortest.py
View File

@ -1,17 +1,22 @@
import rospy
import random
from geometry_msgs.msg import Point
from geometry_msgs.msg import Point, Twist
from gazebo_msgs.srv import GetModelState
import sys
import numpy
from nav_msgs.msg import Odometry
class ControlActor:
def __init__(self, actor_id):
self.count = 0
self.die_count = 0
self.uav_num = 6
self.vehicle_type = 'iris'
self.f = 100
self.flag = True
self.distance_flag = False
self.suitable_point = True
self.get_moving = False
self.x = 0.0
self.y = 0.0
self.x_max = 100.0
@ -25,31 +30,69 @@ class ControlActor:
self.target_pose = Point()
self.target_pose.z = 1.25
self.gazebo_actor_pose = Point()
#self.black_box = numpy.array([[[-33, -20], [17, 33]], [[6, 19], [11, 27]], [[54, 67], [14, 30]], [[71, 83], [9, 19]], [[87, 101], [11, 17]], [[78, 95], [23, 34]], [[53, 70], [-35, -26]], [[-5, 5], [-34, -21]], [[13, 39], [-19, -9]], [[-6, 7], [-20, -10]], [[-28, -23], [-15, -28]], [[-36, -31], [-26, -13]], [[-37, -23], [-35, -30]]])
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.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.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_state = self.gazeboModelstate('actor_' + self.id, 'ground_plane')
get_actor_state = self.gazeboModelstate('actor_' + self.id, 'ground_plane')
self.last_pose = self.current_pose
self.gazebo_actor_pose = get_state.pose.position
self.gazebo_actor_pose = get_actor_state.pose.position
self.current_pose = self.gazebo_actor_pose
except rospy.ServiceException, e:
print "Gazebo model state service call failed: %s" % e
except rospy.ServiceException as e:
print("Gazebo model state service call failed: %s") % e
self.current_pose.x = 0.0
self.current_pose.y = 0.0
self.current_pose.z = 1.25
# 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)
@ -58,21 +101,102 @@ class ControlActor:
self.target_pose.y = self.y
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.cmd_pub.publish(self.target_pose)
if self.count % 20 == 0:
print('current_pose' + self.id+':', self.current_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
if self.suitable_point:
for i in range(13):
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 + 100*(self.last_pose.x-self.current_pose.x)
self.target_pose.y = self.current_pose.y + 100*(self.last_pose.y-self.current_pose.y)
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)
self.suitable_point = False
self.get_moving = True
print('wall!!!!')
print('wall!!!!')
print('wall!!!!')
print('wall!!!!')
print('wall!!!!')
print('wall!!!!')
break
# 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
if self.dis_actor_uav[i] < 400.0 and (self.catching_flag == 0):
self.tracking_flag[i] = self.tracking_flag[i]+1
if self.tracking_flag[i] > 20:
self.catching_flag = 1
self.tracking_flag[i] = 0
self.catching_uav_num = i
print('catch you!', self.id)
print('catch you!', self.id)
print('catch you!', self.id)
print('catch you!', self.id)
print('catch you!', self.id)
print('catch you!', self.id)
print('catch you!', self.id)
break
# escaping
if self.catching_flag == 1:
self.catching_flag = 2
if self.suitable_point:
print('escaping')
print('escaping')
print('escaping')
print('escaping')
print('escaping')
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
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].x
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
if self.target_pose == self.current_pose:
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
self.target_pose.y = self.y
# check if the actor is died:
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.die_count = self.die_count+1
if self.die_count > 100:
print('you are died')
print('you are died')
print('you are died')
print('you are died')
print('you are died')
print('you are died')
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
self.target_pose.y = self.y
self.die_count = 0
else:
self.die_count = 0
rate.sleep()

8
control/actor/gazebo_ros_actor_plugin/gazebo_ros_actor_cmd_plugin/src/ActorPluginRos.cpp
View File

@ -277,9 +277,9 @@ void ActorPluginRos::OnUpdate(const common::UpdateInfo &_info)
(distanceTraveled * 5.0));
this->lastUpdate = _info.simTime;
std::cout << "[XTDrone_Actor_Plugin]: Publish topic actor_pose_pub" << std::endl;
std::cout << "Target_Position: " << target[0] << "," << target[1] << "," << target[2] << std::endl;
std::cout << "Actor_Position: " << std::dec << pose.Pos().X() << "," << pose.Pos().Y() << "," << pose.Pos().Z() << std::endl;
std::cout << "init_pose: " << std::dec << init_pose << "vel: " << this->velocity << std::endl;
//std::cout << "[XTDrone_Actor_Plugin]: Publish topic actor_pose_pub" << std::endl;
//std::cout << "Target_Position: " << target[0] << "," << target[1] << "," << target[2] << std::endl;
//std::cout << "Actor_Position: " << std::dec << pose.Pos().X() << "," << pose.Pos().Y() << "," << pose.Pos().Z() << std::endl;
//std::cout << "init_pose: " << std::dec << init_pose << "vel: " << this->velocity << std::endl;
}
}

10
control/actor/run_actor.sh
View File

@ -1,7 +1,7 @@
#!/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_actor.py 1 &
python control_actor.py 2 &
python control_actor.py 3 &
python control_actor.py 4 &
python control_actor.py 5

7
control/actor/run_actortest.sh Executable file
View File

@ -0,0 +1,7 @@
#!/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

2
control/actor/stop_actor.sh
View File

@ -1 +1 @@
kill -9 $(ps -ef|grep control_actortest.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ')
kill -9 $(ps -ef|grep control_actor.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ')

1
control/actor/stop_actortest.sh Executable file
View File

@ -0,0 +1 @@
kill -9 $(ps -ef|grep control_actortest.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ')

7
control/run_actor.sh Executable file
View File

@ -0,0 +1,7 @@
#!/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
control/stop_actor.sh Executable file
View File

@ -0,0 +1 @@
kill -9 $(ps -ef|grep control_actortest.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ')

3
coordination/.idea/.gitignore vendored Normal file
View File

@ -0,0 +1,3 @@
# Default ignored files
/shelf/
/workspace.xml

11
coordination/.idea/coordination.iml Normal file
View File

@ -0,0 +1,11 @@
<?xml version="1.0" encoding="UTF-8"?>
<module type="PYTHON_MODULE" version="4">
<component name="NewModuleRootManager">
<content url="file://$MODULE_DIR$" />
<orderEntry type="inheritedJdk" />
<orderEntry type="sourceFolder" forTests="false" />
</component>
<component name="TestRunnerService">
<option name="PROJECT_TEST_RUNNER" value="Nosetests" />
</component>
</module>

6
coordination/.idea/inspectionProfiles/profiles_settings.xml Normal file
View File

@ -0,0 +1,6 @@
<component name="InspectionProjectProfileManager">
<settings>
<option name="USE_PROJECT_PROFILE" value="false" />
<version value="1.0" />
</settings>
</component>

4
coordination/.idea/misc.xml Normal file
View File

@ -0,0 +1,4 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectRootManager" version="2" project-jdk-name="Python 2.7" project-jdk-type="Python SDK" />
</project>

8
coordination/.idea/modules.xml Normal file
View File

@ -0,0 +1,8 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectModuleManager">
<modules>
<module fileurl="file://$PROJECT_DIR$/.idea/coordination.iml" filepath="$PROJECT_DIR$/.idea/coordination.iml" />
</modules>
</component>
</project>

6
coordination/.idea/vcs.xml Normal file
View File

@ -0,0 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="VcsDirectoryMappings">
<mapping directory="$PROJECT_DIR$/.." vcs="Git" />
</component>
</project>

171
coordination/catch_actors/catching_uavs.py Normal file
View File

@ -0,0 +1,171 @@
#!/usr/bin/python
# -*- coding: UTF-8 -*-
import rospy
from geometry_msgs.msg import Twist, Point, PoseStamped, TwistStamped
from std_msgs.msg import String
import sys
import math
class CatchingPlan:
def __init__(self, uav_id):
self.uav_type = 'iris'
self.id = int(uav_id)
print(self.id)
self.uav_num = 6
self.f = 100
self.count = 0
self.local_pose = PoseStamped()
self.uav_current_pose = Point()
self.uav_current_yaw = 0.0
self.uav_vel = Twist()
self.arrive_print = False
self.flag = 0
self.Kp = 0.5
self.Kpy = 0.1
self.velxy_max = 1
self.velz_max = 1
self.angz_max = 0.51
self.target_position = Twist()
self.target_yaw = 0.0
self.arrive_count = 0
self.local_pose_sub = rospy.Subscriber(self.uav_type + '_' + str(self.id) + "/mavros/local_position/pose",
PoseStamped, self.local_pose_callback)
self.vel_enu_pub = rospy.Publisher('/xtdrone/'+self.uav_type+'_'+str(self.id)+'/cmd_vel_enu', Twist, queue_size=10)
def local_pose_callback(self, msg):
self.local_pose = msg
self.uav_current_pose = self.local_pose.pose.position
# change Quaternion to TF:
x = self.local_pose.pose.orientation.x
y = self.local_pose.pose.orientation.y
z = self.local_pose.pose.orientation.z
w = self.local_pose.pose.orientation.w
r = math.atan2(2 * (w * z + x * y), 1 - 2 * (z * z + y * y))
self.uav_current_yaw = r * 180 / math.pi
def loop(self):
rospy.init_node('uav'+str(self.id))
rate = rospy.Rate(self.f)
while not rospy.is_shutdown():
self.count += 1
if self.flag == 0:
self.target_position.linear.z = 9.0
self.target_position.linear.x = self.uav_current_pose.x
self.target_position.linear.y = self.uav_current_pose.y
self.uav_vel.angular.x = 0.0
self.uav_vel.angular.y = 0.0
if self.flag == 1:
if self.id == 0:
self.target_position.linear.x = -25.0
self.target_position.linear.y = 25.0
elif self.id == 1:
self.target_position.linear.x = -25.0
self.target_position.linear.y = -25.0
elif self.id == 2:
self.target_position.linear.x = 25.0
self.target_position.linear.y = 25.0
elif self.id == 3:
self.target_position.linear.x = 25.0
self.target_position.linear.y = -25.0
elif self.id == 4:
self.target_position.linear.x = 75.0
self.target_position.linear.y = 25.0
elif self.id == 5:
self.target_position.linear.x = 75.0
self.target_position.linear.y = -25.0
self.flag = 2
self.uav_vel.linear.x = self.Kp * (self.target_position.linear.x - self.uav_current_pose.x)
self.uav_vel.linear.y = self.Kp * (self.target_position.linear.y - self.uav_current_pose.y)
self.uav_vel.linear.z = self.Kp * (self.target_position.linear.z - self.uav_current_pose.z)
if self.uav_vel.linear.x > self.velxy_max:
self.uav_vel.linear.x = self.velxy_max
elif self.uav_vel.linear.x < - self.velxy_max:
self.uav_vel.linear.x = - self.velxy_max
if self.uav_vel.linear.y > self.velxy_max:
self.uav_vel.linear.y = self.velxy_max
elif self.uav_vel.linear.y < - self.velxy_max:
self.uav_vel.linear.y = - self.velxy_max
if self.uav_vel.linear.z > self.velz_max:
self.uav_vel.linear.z = self.velz_max
elif self.uav_vel.linear.z < - self.velz_max:
self.uav_vel.linear.z = - self.velz_max
if self.uav_vel.linear.x == 0.0:
if self.uav_vel.linear.y >= 0.0:
self.target_yaw = 0.0
else:
self.target_yaw = math.pi
else:
self.target_yaw = math.atan2(self.uav_vel.linear.y, self.uav_vel.linear.x)
self.uav_vel.angular.z = self.Kpy * (self.target_yaw - self.uav_current_yaw)
if self.uav_vel.angular.z > self.angz_max:
self.uav_vel.angular.z = self.angz_max
elif self.uav_vel.angular.z < -self.angz_max:
self.uav_vel.angular.z = -self.angz_max
if self.count%20 == 0:
print('target_position:'+str(self.id),self.target_position)
print('current_position:'+str(self.id), self.uav_current_pose)
print('target_yaw:'+str(self.id), self.target_yaw)
print('uav_current_yaw:'+str(self.id), self.uav_current_yaw)
self.vel_enu_pub.publish(self.uav_vel)
if (self.uav_vel.linear.x ** 2 + self.uav_vel.linear.y ** 2 + self.uav_vel.linear.z ** 2) < 0.2:
self.arrive_count += 1
if self.arrive_count > 10:
self.arrive_print = True
self.arrive_count = 0
else:
self.arrive_count = 0
self.arrive_print = False
if (self.flag == 0) and self.arrive_print:
self.flag = 1
elif (self.flag == 2) and self.arrive_print:
self.target_position.linear.x = self.target_position.linear.x
self.target_position.linear.y = self.target_position.linear.y - 20.0
self.flag = 3
elif (self.flag == 3) and self.arrive_print:
self.target_position.linear.x = self.target_position.linear.x - 20.0
self.target_position.linear.y = self.target_position.linear.y
self.flag = 4
elif (self.flag == 4) and self.arrive_print:
self.target_position.linear.x = self.target_position.linear.x
self.target_position.linear.y = self.target_position.linear.y + 40.0
self.flag = 5
elif (self.flag == 5) and self.arrive_print:
self.target_position.linear.x = self.target_position.linear.x + 40.0
self.target_position.linear.y = self.target_position.linear.y
self.flag = 6
elif (self.flag == 6) and self.arrive_print:
self.target_position.linear.x = self.target_position.linear.x
self.target_position.linear.y = self.target_position.linear.y - 30.0
self.flag = 7
if (self.flag == 7) and self.arrive_print:
self.target_position.linear.x = self.target_position.linear.x - 30.0
self.target_position.linear.y = self.target_position.linear.y
self.flag = 8
if (self.flag == 8) and self.arrive_print:
self.target_position.linear.x = self.target_position.linear.x
self.target_position.linear.y = self.target_position.linear.y + 20.0
self.flag = 9
if (self.flag == 9) and self.arrive_print:
self.target_position.linear.x = self.target_position.linear.x + 20.0
self.target_position.linear.y = self.target_position.linear.y
self.flag = 10
if (self.flag == 10) and self.arrive_print:
self.target_position.linear.x = self.target_position.linear.x
self.target_position.linear.y = self.target_position.linear.y + 10.0
self.flag = 11
if (self.flag == 11) and self.arrive_print:
self.target_position.linear.x = self.target_position.linear.x - 10.0
self.target_position.linear.y = self.target_position.linear.y
self.flag = 2
rate.sleep()
if __name__ == '__main__':
catchplan = CatchingPlan(sys.argv[1])
catchplan.loop()

7
coordination/catch_actors/run_catching.sh Executable file
View File

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

1
coordination/catch_actors/stop_actor.sh Executable file
View File

@ -0,0 +1 @@
kill -9 $(ps -ef|grep catching_uavs.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ')

25
coordination/launch_generator/multi_vehicle.launch
View File

@ -18,8 +18,8 @@
<arg name="verbose" value="$(arg verbose)"/>
<arg name="paused" value="$(arg paused)"/>
</include>
<!-- typhoon_h480_0 -->
<group ns="typhoon_h480_0">
<!-- iris_0 -->
<group ns="iris_0">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="1"/>
<arg name="ID_in_group" value="0"/>
@ -32,8 +32,8 @@
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="typhoon_h480"/>
<arg name="sdf" value="typhoon_h480_stereo"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_stereo_camera"/>
<arg name="mavlink_udp_port" value="24562"/>
<arg name="mavlink_tcp_port" value="4561"/>
<arg name="ID" value="$(arg ID)"/>
@ -47,8 +47,6 @@
<arg name="tgt_component" value="1"/>
</include>
</group>
<<<<<<< HEAD
=======
<!-- iris_1 -->
<group ns="iris_1">
<!-- MAVROS and vehicle configs -->
@ -86,8 +84,8 @@
<arg name="fcu_url" default="udp://:14543@127.0.0.1:34576"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="6"/>
<arg name="y" value="3"/>
<arg name="x" value="0"/>
<arg name="y" value="6"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
@ -115,7 +113,7 @@
<arg name="fcu_url" default="udp://:14544@127.0.0.1:34578"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="x" value="3"/>
<arg name="y" value="6"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
@ -144,8 +142,8 @@
<arg name="fcu_url" default="udp://:14545@127.0.0.1:34580"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="3"/>
<arg name="y" value="6"/>
<arg name="x" value="0"/>
<arg name="y" value="9"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
@ -173,8 +171,8 @@
<arg name="fcu_url" default="udp://:14546@127.0.0.1:34582"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="6"/>
<arg name="y" value="6"/>
<arg name="x" value="3"/>
<arg name="y" value="9"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
@ -194,6 +192,5 @@
<arg name="tgt_component" value="1"/>
</include>
</group>
>>>>>>> 06e8933d2ed98cccbb3aad2f36a90ab6231520e1
</launch>
<!--the launch file is generated by XTDrone multi-vehicle generator.py -->

10
sitl_config/worlds/robocup.world
View File

@ -98,7 +98,7 @@
<interpolate_x>true</interpolate_x>
</animation>
<plugin name="actor1_plugin" filename="libros_actor_cmd_pose_plugin.so">
<init_pose>1 1 1.1 1.57 0 0</init_pose>
<init_pose>0 0 1.25 1.57 0 0</init_pose>
<animation_factor>5.1</animation_factor>
</plugin>
</actor>
@ -115,7 +115,7 @@
<interpolate_x>true</interpolate_x>
</animation>
<plugin name="actor2_plugin" filename="libros_actor_cmd_pose_plugin.so">
<init_pose>1 5 1.1 1.57 0 0</init_pose>
<init_pose>2 2 1.25 1.57 0 0</init_pose>
<animation_factor>5.1</animation_factor>
</plugin>
</actor>
@ -132,7 +132,7 @@
<interpolate_x>true</interpolate_x>
</animation>
<plugin name="actor3_plugin" filename="libros_actor_cmd_pose_plugin.so">
<init_pose>3 1 1.1 1.57 0 0</init_pose>
<init_pose>2 -2 1.25 1.57 0 0</init_pose>
<animation_factor>5.1</animation_factor>
</plugin>
</actor>
@ -149,7 +149,7 @@
<interpolate_x>true</interpolate_x>
</animation>
<plugin name="actor4_plugin" filename="libros_actor_cmd_pose_plugin.so">
<init_pose>2 1 1.1 1.57 0 0</init_pose>
<init_pose>-2 2 1.25 1.57 0 0</init_pose>
<animation_factor>5.1</animation_factor>
</plugin>
</actor>
@ -166,7 +166,7 @@
<interpolate_x>true</interpolate_x>
</animation>
<plugin name="actor5_plugin" filename="libros_actor_cmd_pose_plugin.so">
<init_pose>1 4 1.1 1.57 0 0</init_pose>
<init_pose>-2 -2 1.25 1.57 0 0</init_pose>
<animation_factor>5.1</animation_factor>
</plugin>
</actor>