From 262e398775be19df0835cbdc37671946772ca7a0 Mon Sep 17 00:00:00 2001 From: ma_lan Date: Fri, 18 Sep 2020 10:49:23 +0800 Subject: [PATCH] new control_actors and uav catching plugin --- control/actor/control_actor.py | 47 ++++ control/actor/control_actor_1.py | 205 ++++++++++++++++++ control/actor/control_actortest.py | 152 +++++++++++-- .../src/ActorPluginRos.cpp | 8 +- control/actor/run_actor.sh | 10 +- control/actor/run_actortest.sh | 7 + control/actor/stop_actor.sh | 2 +- control/actor/stop_actortest.sh | 1 + control/run_actor.sh | 7 + control/stop_actor.sh | 1 + coordination/.idea/.gitignore | 3 + coordination/.idea/coordination.iml | 11 + .../inspectionProfiles/profiles_settings.xml | 6 + coordination/.idea/misc.xml | 4 + coordination/.idea/modules.xml | 8 + coordination/.idea/vcs.xml | 6 + coordination/catch_actors/catching_uavs.py | 171 +++++++++++++++ coordination/catch_actors/run_catching.sh | 7 + coordination/catch_actors/stop_actor.sh | 1 + .../launch_generator/multi_vehicle.launch | 25 +-- sitl_config/worlds/robocup.world | 10 +- 21 files changed, 649 insertions(+), 43 deletions(-) create mode 100755 control/actor/control_actor.py create mode 100755 control/actor/control_actor_1.py create mode 100755 control/actor/run_actortest.sh create mode 100755 control/actor/stop_actortest.sh create mode 100755 control/run_actor.sh create mode 100755 control/stop_actor.sh create mode 100644 coordination/.idea/.gitignore create mode 100644 coordination/.idea/coordination.iml create mode 100644 coordination/.idea/inspectionProfiles/profiles_settings.xml create mode 100644 coordination/.idea/misc.xml create mode 100644 coordination/.idea/modules.xml create mode 100644 coordination/.idea/vcs.xml create mode 100644 coordination/catch_actors/catching_uavs.py create mode 100755 coordination/catch_actors/run_catching.sh create mode 100755 coordination/catch_actors/stop_actor.sh diff --git a/control/actor/control_actor.py b/control/actor/control_actor.py new file mode 100755 index 0000000..01d7ad9 --- /dev/null +++ b/control/actor/control_actor.py @@ -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() \ No newline at end of file diff --git a/control/actor/control_actor_1.py b/control/actor/control_actor_1.py new file mode 100755 index 0000000..6d55a80 --- /dev/null +++ b/control/actor/control_actor_1.py @@ -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() diff --git a/control/actor/control_actortest.py b/control/actor/control_actortest.py index eb4230a..c181307 100755 --- a/control/actor/control_actortest.py +++ b/control/actor/control_actortest.py @@ -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,24 +101,105 @@ 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() if __name__=="__main__": controlactors = ControlActor(sys.argv[1]) - controlactors.loop() \ No newline at end of file + controlactors.loop() diff --git a/control/actor/gazebo_ros_actor_plugin/gazebo_ros_actor_cmd_plugin/src/ActorPluginRos.cpp b/control/actor/gazebo_ros_actor_plugin/gazebo_ros_actor_cmd_plugin/src/ActorPluginRos.cpp index be653ba..d3ee0a1 100644 --- a/control/actor/gazebo_ros_actor_plugin/gazebo_ros_actor_cmd_plugin/src/ActorPluginRos.cpp +++ b/control/actor/gazebo_ros_actor_plugin/gazebo_ros_actor_cmd_plugin/src/ActorPluginRos.cpp @@ -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; } } diff --git a/control/actor/run_actor.sh b/control/actor/run_actor.sh index 930ff68..61d29ab 100755 --- a/control/actor/run_actor.sh +++ b/control/actor/run_actor.sh @@ -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 diff --git a/control/actor/run_actortest.sh b/control/actor/run_actortest.sh new file mode 100755 index 0000000..930ff68 --- /dev/null +++ b/control/actor/run_actortest.sh @@ -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 + diff --git a/control/actor/stop_actor.sh b/control/actor/stop_actor.sh index 8a82a4c..1e35233 100755 --- a/control/actor/stop_actor.sh +++ b/control/actor/stop_actor.sh @@ -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' ' ') diff --git a/control/actor/stop_actortest.sh b/control/actor/stop_actortest.sh new file mode 100755 index 0000000..8a82a4c --- /dev/null +++ b/control/actor/stop_actortest.sh @@ -0,0 +1 @@ +kill -9 $(ps -ef|grep control_actortest.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ') diff --git a/control/run_actor.sh b/control/run_actor.sh new file mode 100755 index 0000000..930ff68 --- /dev/null +++ b/control/run_actor.sh @@ -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 + diff --git a/control/stop_actor.sh b/control/stop_actor.sh new file mode 100755 index 0000000..8a82a4c --- /dev/null +++ b/control/stop_actor.sh @@ -0,0 +1 @@ +kill -9 $(ps -ef|grep control_actortest.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ') diff --git a/coordination/.idea/.gitignore b/coordination/.idea/.gitignore new file mode 100644 index 0000000..26d3352 --- /dev/null +++ b/coordination/.idea/.gitignore @@ -0,0 +1,3 @@ +# Default ignored files +/shelf/ +/workspace.xml diff --git a/coordination/.idea/coordination.iml b/coordination/.idea/coordination.iml new file mode 100644 index 0000000..2c1d26d --- /dev/null +++ b/coordination/.idea/coordination.iml @@ -0,0 +1,11 @@ + + + + + + + + + + \ No newline at end of file diff --git a/coordination/.idea/inspectionProfiles/profiles_settings.xml b/coordination/.idea/inspectionProfiles/profiles_settings.xml new file mode 100644 index 0000000..105ce2d --- /dev/null +++ b/coordination/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/coordination/.idea/misc.xml b/coordination/.idea/misc.xml new file mode 100644 index 0000000..7ba73c2 --- /dev/null +++ b/coordination/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/coordination/.idea/modules.xml b/coordination/.idea/modules.xml new file mode 100644 index 0000000..427b307 --- /dev/null +++ b/coordination/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/coordination/.idea/vcs.xml b/coordination/.idea/vcs.xml new file mode 100644 index 0000000..6c0b863 --- /dev/null +++ b/coordination/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/coordination/catch_actors/catching_uavs.py b/coordination/catch_actors/catching_uavs.py new file mode 100644 index 0000000..b744f42 --- /dev/null +++ b/coordination/catch_actors/catching_uavs.py @@ -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() \ No newline at end of file diff --git a/coordination/catch_actors/run_catching.sh b/coordination/catch_actors/run_catching.sh new file mode 100755 index 0000000..e0e02ba --- /dev/null +++ b/coordination/catch_actors/run_catching.sh @@ -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 + diff --git a/coordination/catch_actors/stop_actor.sh b/coordination/catch_actors/stop_actor.sh new file mode 100755 index 0000000..f8664bc --- /dev/null +++ b/coordination/catch_actors/stop_actor.sh @@ -0,0 +1 @@ +kill -9 $(ps -ef|grep catching_uavs.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ') diff --git a/coordination/launch_generator/multi_vehicle.launch b/coordination/launch_generator/multi_vehicle.launch index 6e527b6..a9b0a87 100644 --- a/coordination/launch_generator/multi_vehicle.launch +++ b/coordination/launch_generator/multi_vehicle.launch @@ -18,8 +18,8 @@ - - + + @@ -32,8 +32,8 @@ - - + + @@ -47,8 +47,6 @@ -<<<<<<< HEAD -======= @@ -86,8 +84,8 @@ - - + + @@ -115,7 +113,7 @@ - + @@ -144,8 +142,8 @@ - - + + @@ -173,8 +171,8 @@ - - + + @@ -194,6 +192,5 @@ ->>>>>>> 06e8933d2ed98cccbb3aad2f36a90ab6231520e1 \ No newline at end of file diff --git a/sitl_config/worlds/robocup.world b/sitl_config/worlds/robocup.world index cf16f1a..027b232 100755 --- a/sitl_config/worlds/robocup.world +++ b/sitl_config/worlds/robocup.world @@ -98,7 +98,7 @@ true - 1 1 1.1 1.57 0 0 + 0 0 1.25 1.57 0 0 5.1 @@ -115,7 +115,7 @@ true - 1 5 1.1 1.57 0 0 + 2 2 1.25 1.57 0 0 5.1 @@ -132,7 +132,7 @@ true - 3 1 1.1 1.57 0 0 + 2 -2 1.25 1.57 0 0 5.1 @@ -149,7 +149,7 @@ true - 2 1 1.1 1.57 0 0 + -2 2 1.25 1.57 0 0 5.1 @@ -166,7 +166,7 @@ true - 1 4 1.1 1.57 0 0 + -2 -2 1.25 1.57 0 0 5.1