From 2a03e671c460fde149a51f1bb7b1355a7f85be61 Mon Sep 17 00:00:00 2001 From: Robin Shaun Date: Tue, 19 May 2020 20:46:55 +0800 Subject: [PATCH] =?UTF-8?q?=E5=AE=8C=E5=96=84=E7=BC=96=E9=98=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- communication/multi_vehicle_communication.sh | 2 +- communication/plane_communication.py | 2 +- communication/vtol_communication.py | 2 +- control/multirotor_keyboard_control.py | 13 +- coordination/formation_demo/avoid.py | 25 +- .../formation_demo/follower_accel_control.py | 58 +-- coordination/formation_demo/leader.py | 29 +- coordination/formation_demo/run.sh | 9 +- coordination/launch/generator.py | 1 - coordination/launch/multi_vehicle.launch | 369 ++---------------- .../simple_3d_simulator_accel.py | 16 +- coordination/simple_simulator/visualize.py | 5 +- 12 files changed, 113 insertions(+), 418 deletions(-) diff --git a/communication/multi_vehicle_communication.sh b/communication/multi_vehicle_communication.sh index 7bfde91..1350a4d 100644 --- a/communication/multi_vehicle_communication.sh +++ b/communication/multi_vehicle_communication.sh @@ -1,5 +1,5 @@ #!/bin/bash -iris_num=9 +iris_num=6 typhoon_h480_num=0 solo_num=0 plane_num=0 diff --git a/communication/plane_communication.py b/communication/plane_communication.py index d94a82a..608dc06 100644 --- a/communication/plane_communication.py +++ b/communication/plane_communication.py @@ -119,7 +119,7 @@ class Communication: self.arm_state = not self.disarm() print(self.vehicle_type+'_'+self.vehicle_id+": Armed "+str(self.arm_state)) - elif msg.data == 'takeoff' or msg.data == 'land' or msg.data == 'loiter' or msg.data == 'idle': + elif msg.data in ['takeoff', 'land', 'loiter', 'idle']: self.mission = msg.data print(self.mission) else: diff --git a/communication/vtol_communication.py b/communication/vtol_communication.py index efc6fc7..ed0a05e 100644 --- a/communication/vtol_communication.py +++ b/communication/vtol_communication.py @@ -206,7 +206,7 @@ class Communication: print(self.vehicle_type+'_'+self.vehicle_id+':'+msg.data) print(self.transition(state=4)) - elif msg.data == 'loiter' or msg.data == 'idle': + elif msg.data in ['loiter', 'idle']: self.plane_mission = msg.data print(self.vehicle_type+'_'+self.vehicle_id+':'+self.plane_mission) diff --git a/control/multirotor_keyboard_control.py b/control/multirotor_keyboard_control.py index 1c8b0ed..a58e29e 100755 --- a/control/multirotor_keyboard_control.py +++ b/control/multirotor_keyboard_control.py @@ -89,6 +89,11 @@ if __name__=="__main__": multirotor_num = int(sys.argv[2]) control_type = sys.argv[3] + if multirotor_num == 9: + formation_configs = ['waiting', 'cube', 'pyramid', 'triangle'] + if multirotor_num == 6: + formation_configs = ['waiting', 'T', 'diamond', 'triangle'] + cmd= String() twist = Twist() @@ -101,7 +106,7 @@ if __name__=="__main__": multi_cmd_vel_flu_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd_vel_flu', Twist, queue_size=10) multi_cmd_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd',String,queue_size=10) leader_cmd_vel_flu_pub = rospy.Publisher("/xtdrone/leader/cmd_vel_flu", Twist, queue_size=10) - leader_cmd_pub = rospy.Publisher("/xtdrone/leader_cmd", String, queue_size=10) + leader_cmd_pub = rospy.Publisher("/xtdrone/leader/cmd", String, queue_size=10) else: multi_cmd_accel_flu_pub = [None]*multirotor_num @@ -110,7 +115,7 @@ if __name__=="__main__": multi_cmd_accel_flu_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd_accel_flu', Twist, queue_size=10) multi_cmd_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd',String,queue_size=10) leader_cmd_accel_flu_pub = rospy.Publisher("/xtdrone/leader/cmd_accel_flu", Twist, queue_size=10) - leader_cmd_pub = rospy.Publisher("/xtdrone/leader_cmd", String, queue_size=10) + leader_cmd_pub = rospy.Publisher("/xtdrone/leader/cmd", String, queue_size=10) forward = 0.0 leftward = 0.0 @@ -207,7 +212,7 @@ if __name__=="__main__": elif key == 'g': ctrl_leader = not ctrl_leader print_msg() - elif key == 'k' or key == 's': + elif key in ['k', 's']: cmd_vel_mask = False forward = 0.0 leftward = 0.0 @@ -220,7 +225,7 @@ if __name__=="__main__": else: for i in range(10): if key == str(i): - cmd = 'mission'+key + cmd = formation_configs[i] print_msg() print(cmd) cmd_vel_mask = True diff --git a/coordination/formation_demo/avoid.py b/coordination/formation_demo/avoid.py index cc94309..30655b1 100644 --- a/coordination/formation_demo/avoid.py +++ b/coordination/formation_demo/avoid.py @@ -6,30 +6,31 @@ import math import numpy import sys -uav_num = int(sys.argv[1]) -pose = [None]*uav_num -pose_sub = [None]*uav_num -avoid_accel_pub = [None]*uav_num +vehicle_type = sys.argv[1] +vehicle_num = int(sys.argv[2]) +pose = [None]*vehicle_num +pose_sub = [None]*vehicle_num +avoid_accel_pub = [None]*vehicle_num avoid_kp = 0.2 avoid_radius = 1 aid_vec1 = [1, 0, 0] aid_vec2 = [0, 1, 0] -uavs_avoid_accel = [Vector3()] * uav_num +uavs_avoid_accel = [Vector3()] * vehicle_num def pose_callback(msg, id): pose[id] = msg rospy.init_node('avoid') -for i in range(uav_num): - pose_sub[i] = rospy.Subscriber('/uav'+str(i+1)+'/mavros/local_position/pose',PoseStamped,pose_callback,i) - avoid_accel_pub[i] = rospy.Publisher("/xtdrone/uav"+str(i+1)+"/avoid_accel", Vector3,queue_size=10) +for i in range(vehicle_num): + pose_sub[i] = rospy.Subscriber(vehicle_type+'_'+str(i)+'/mavros/local_position/pose',PoseStamped,pose_callback,i) + avoid_accel_pub[i] = rospy.Publisher("/xtdrone/"+vehicle_type+'_'+str(i+1)+"/avoid_accel", Vector3,queue_size=10) time.sleep(1) rate = rospy.Rate(50) while not rospy.is_shutdown(): - for i in range(uav_num): + for i in range(vehicle_num): position1 = numpy.array([pose[i].pose.position.x, pose[i].pose.position.y, pose[i].pose.position.z]) - for j in range(1, uav_num-i): + for j in range(1, vehicle_num-i): position2 = numpy.array([pose[i+j].pose.position.x, pose[i+j].pose.position.y, pose[i+j].pose.position.z]) dir_vec = position1-position2 if numpy.linalg.norm(dir_vec) < avoid_radius: @@ -41,9 +42,9 @@ while not rospy.is_shutdown(): avoid_accel = avoid_kp * numpy.cross(dir_vec, aid_vec2)/numpy.linalg.norm(numpy.cross(dir_vec, aid_vec2)) uavs_avoid_accel[i] = Vector3(uavs_avoid_accel[i].x+avoid_accel[0],uavs_avoid_accel[i].y+avoid_accel[1],uavs_avoid_accel[i].z+avoid_accel[2]) uavs_avoid_accel[i+j] = Vector3(uavs_avoid_accel[i+j].x-avoid_accel[0],uavs_avoid_accel[i+j].y-avoid_accel[1],uavs_avoid_accel[i+j].z-avoid_accel[2]) - for i in range(uav_num): + for i in range(vehicle_num): avoid_accel_pub[i].publish(uavs_avoid_accel[i]) - uavs_avoid_accel = [Vector3()] * uav_num + uavs_avoid_accel = [Vector3()] * vehicle_num rate.sleep() diff --git a/coordination/formation_demo/follower_accel_control.py b/coordination/formation_demo/follower_accel_control.py index 7641433..21e8986 100644 --- a/coordination/formation_demo/follower_accel_control.py +++ b/coordination/formation_demo/follower_accel_control.py @@ -4,11 +4,14 @@ import rospy from geometry_msgs.msg import Twist, Vector3, PoseStamped, TwistStamped from std_msgs.msg import String from pyquaternion import Quaternion -from formation_dict import formation_dict_9 +import sys +if sys.argv[3] == '6': + from formation_dict import formation_dict_6 as formation_dict +elif sys.argv[3] == '9': + from formation_dict import formation_dict_9 as formation_dict import time import math import numpy -import sys import heapq import copy import Queue @@ -16,9 +19,10 @@ from itertools import permutations class Follower: - def __init__(self, uav_id, uav_num): + def __init__(self, uav_type, uav_id, uav_num): self.hover = "HOVER" self.offboard = "OFFBOARD" + self.uav_type = uav_type self.id = uav_id self.uav_num = uav_num self.f = 100 @@ -35,7 +39,7 @@ class Follower: self.following_ids = [] self.formation_config = 'waiting' self.following_count = 0 - self.Kp = 100 #100 + self.Kp = 1000 #100 #self.kr = (4/int((self.uav_num-1)/2))**0.5 self.kr = 1 self.velxy_max = 2 @@ -45,13 +49,13 @@ class Follower: self.following_local_velocity = [TwistStamped() for i in range(self.uav_num)] self.following_local_velocity_sub = [None]*self.uav_num self.arrive_count = 0 - self.local_pose_sub = rospy.Subscriber("/uav"+str(self.id)+"/mavros/local_position/pose", PoseStamped, self.local_pose_callback) - self.local_velocity_sub = rospy.Subscriber("/uav"+str(self.id)+"/mavros/local_position/velocity_local", TwistStamped, self.local_velocity_callback) - self.avoid_accel_sub = rospy.Subscriber("/xtdrone/uav"+str(self.id)+"/avoid_accel", Vector3, self.avoid_accel_callback) + self.local_pose_sub = rospy.Subscriber(self.uav_type+'_'+str(self.id)+"/mavros/local_position/pose", PoseStamped, self.local_pose_callback) + self.local_velocity_sub = rospy.Subscriber(self.uav_type+'_'+str(self.id)+"/mavros/local_position/velocity_local", TwistStamped, self.local_velocity_callback) + self.avoid_accel_sub = rospy.Subscriber("/xtdrone/"+self.uav_type+'_'+str(self.id)+"/avoid_accel", Vector3, self.avoid_accel_callback) self.formation_switch_sub = rospy.Subscriber("/xtdrone/formation_switch",String, self.formation_switch_callback) - self.vel_enu_pub = rospy.Publisher('/xtdrone/uav'+str(self.id)+'/cmd_vel_enu', Twist, queue_size=10) - self.info_pub = rospy.Publisher('/xtdrone/uav'+str(self.id)+'/info', String, queue_size=10) - self.cmd_pub = rospy.Publisher('/xtdrone/uav'+str(self.id)+'/cmd',String,queue_size=10) + self.vel_enu_pub = rospy.Publisher('/xtdrone/'+self.uav_type+'_'+str(self.id)+'/cmd_vel_enu', Twist, queue_size=10) + self.info_pub = rospy.Publisher('/xtdrone/'+self.uav_type+'_'+str(self.id)+'/info', String, queue_size=10) + self.cmd_pub = rospy.Publisher('/xtdrone/'+self.uav_type+'_'+str(self.id)+'/cmd',String,queue_size=10) self.first_formation = True self.orig_formation = None self.new_formation = None @@ -112,15 +116,15 @@ class Follower: self.info_pub.publish("Received") print("Follower"+str(self.id-1)+": Switch to Formation "+self.formation_config) if self.formation_config=='waiting': - self.L_matrix = self.get_L_matrix(formation_dict_9[self.formation_config]) + self.L_matrix = self.get_L_matrix(formation_dict[self.formation_config]) else: if self.first_formation: self.first_formation=False - self.orig_formation=formation_dict_9[self.formation_config] - self.L_matrix = self.get_L_matrix(formation_dict_9[self.formation_config]) + self.orig_formation=formation_dict[self.formation_config] + self.L_matrix = self.get_L_matrix(formation_dict[self.formation_config]) else: - #self.new_formation=self.get_new_formation(self.orig_formation,formation_dict_9[self.formation_config]) - self.adj_matrix = self.build_graph(self.orig_formation,formation_dict_9[self.formation_config]) + #self.new_formation=self.get_new_formation(self.orig_formation,formation_dict[self.formation_config]) + self.adj_matrix = self.build_graph(self.orig_formation,formation_dict[self.formation_config]) self.label_left = numpy.max(self.adj_matrix, axis=1) # init label for the left self.label_right = numpy.array([0]*(self.uav_num-1)) # init label for the right set @@ -129,15 +133,15 @@ class Follower: self.visit_right = numpy.array([0]*(self.uav_num-1)) self.slack_right = numpy.array([100]*(self.uav_num-1)) self.change_id = self.KM() - self.new_formation=self.get_new_formation(self.change_id,formation_dict_9[self.formation_config]) + self.new_formation=self.get_new_formation(self.change_id,formation_dict[self.formation_config]) self.L_matrix = self.get_L_matrix(self.new_formation) self.orig_formation=self.new_formation - #self.L_matrix = self.get_L_matrix(formation_dict_9[self.formation_config]) + #self.L_matrix = self.get_L_matrix(formation_dict[self.formation_config]) if self.id==2: print(self.L_matrix) #self.L_matrix = numpy.array([[0,0,0,0,0,0,0,0,0],[1,-1,0,0,0,0,0,0,0],[1,0,-1,0,0,0,0,0,0],[1,0,0,-1,0,0,0,0,0],[1,0,0,0,-1,0,0,0,0],[1,0,0,0,0,-1,0,0,0],[1,0,0,0,0,0,-1,0,0],[1,0,0,0,0,0,0,-1,0],[1,0,0,0,0,0,0,0,-1]]) - self.following_ids = numpy.argwhere(self.L_matrix[self.id-1,:] == 1) + self.following_ids = numpy.argwhere(self.L_matrix[self.id,:] == 1) #if self.id == 2: #print(self.following_ids) self.following_count = 0 @@ -148,8 +152,8 @@ class Follower: self.following_local_velocity_sub[i].unregister() for following_id in self.following_ids: #print('here') - self.following_local_pose_sub[following_id[0]] = rospy.Subscriber("/uav"+str(following_id[0]+1)+"/mavros/local_position/pose", PoseStamped , self.following_local_pose_callback,following_id[0]) - self.following_local_velocity_sub[following_id[0]] = rospy.Subscriber("/uav"+str(following_id[0]+1)+"/mavros/local_position/velocity_local", TwistStamped , self.following_local_velocity_callback,following_id[0]) + self.following_local_pose_sub[following_id[0]] = rospy.Subscriber(self.uav_type+'_'+str(following_id[0])+"/mavros/local_position/pose", PoseStamped , self.following_local_pose_callback,following_id[0]) + self.following_local_velocity_sub[following_id[0]] = rospy.Subscriber(self.uav_type+'_'+str(following_id[0])+"/mavros/local_position/velocity_local", TwistStamped , self.following_local_velocity_callback,following_id[0]) self.following_count += 1 @@ -158,14 +162,14 @@ class Follower: for following_id in self.following_ids: #if self.following_local_pose[following_id[0]] == None and self.following_local_velocity[following_id[0]] == None: #print(following_id) - self.cmd_accel_enu.x += self.following_local_pose[following_id[0]].pose.position.x + self.kr * self.following_local_velocity[following_id[0]].twist.linear.x - self.local_pose.pose.position.x - self.kr * self.local_velocity.twist.linear.x + formation_dict_9[self.formation_config][0, self.id-2] - self.cmd_accel_enu.y += self.following_local_pose[following_id[0]].pose.position.y + self.kr * self.following_local_velocity[following_id[0]].twist.linear.y - self.local_pose.pose.position.y - self.kr * self.local_velocity.twist.linear.y + formation_dict_9[self.formation_config][1, self.id-2] - self.cmd_accel_enu.z += self.following_local_pose[following_id[0]].pose.position.z + self.kr * self.following_local_velocity[following_id[0]].twist.linear.z - self.local_pose.pose.position.z - self.kr * self.local_velocity.twist.linear.z + formation_dict_9[self.formation_config][2, self.id-2] + self.cmd_accel_enu.x += self.following_local_pose[following_id[0]].pose.position.x + self.kr * self.following_local_velocity[following_id[0]].twist.linear.x - self.local_pose.pose.position.x - self.kr * self.local_velocity.twist.linear.x + formation_dict[self.formation_config][0, self.id-2] + self.cmd_accel_enu.y += self.following_local_pose[following_id[0]].pose.position.y + self.kr * self.following_local_velocity[following_id[0]].twist.linear.y - self.local_pose.pose.position.y - self.kr * self.local_velocity.twist.linear.y + formation_dict[self.formation_config][1, self.id-2] + self.cmd_accel_enu.z += self.following_local_pose[following_id[0]].pose.position.z + self.kr * self.following_local_velocity[following_id[0]].twist.linear.z - self.local_pose.pose.position.z - self.kr * self.local_velocity.twist.linear.z + formation_dict[self.formation_config][2, self.id-2] if not following_id[0] == 0: - self.cmd_accel_enu.x -= formation_dict_9[self.formation_config][0, following_id[0]-1] - self.cmd_accel_enu.y -= formation_dict_9[self.formation_config][1, following_id[0]-1] - self.cmd_accel_enu.z -= formation_dict_9[self.formation_config][2, following_id[0]-1] + self.cmd_accel_enu.x -= formation_dict[self.formation_config][0, following_id[0]-1] + self.cmd_accel_enu.y -= formation_dict[self.formation_config][1, following_id[0]-1] + self.cmd_accel_enu.z -= formation_dict[self.formation_config][2, following_id[0]-1] if self.arrive_count <= 2000: self.cmd_vel_enu.linear.x = self.local_velocity.twist.linear.x + self.Kp * (self.avoid_accel.x + self.cmd_accel_enu.x / self.f) self.cmd_vel_enu.linear.y = self.local_velocity.twist.linear.y + self.Kp * (self.avoid_accel.y + self.cmd_accel_enu.y / self.f) @@ -495,5 +499,5 @@ class Follower: ''' if __name__ == '__main__': - follower = Follower(int(sys.argv[1]),int(sys.argv[2])) + follower = Follower(sys.argv[1],int(sys.argv[2]),int(sys.argv[3])) follower.loop() \ No newline at end of file diff --git a/coordination/formation_demo/leader.py b/coordination/formation_demo/leader.py index 93a40de..980cbb0 100644 --- a/coordination/formation_demo/leader.py +++ b/coordination/formation_demo/leader.py @@ -8,10 +8,14 @@ import time import math import numpy import sys +if sys.argv[2] == '6': + from formation_dict import formation_dict_6 as formation_dict +elif sys.argv[2] == '9': + from formation_dict import formation_dict_9 as formation_dict class Leader: - def __init__(self, leader_id, uav_num): + def __init__(self, uav_type, leader_id, uav_num): self.hover = True self.id = leader_id self.local_pose = PoseStamped() @@ -23,17 +27,20 @@ class Leader: self.avoid_accel = Vector3(0,0,0) self.formation_config = 'waiting' self.target_height_recorded = False + self.cmd = String() self.f = 200 self.Kz = 0.5 - self.local_pose_sub = rospy.Subscriber("/uav"+str(self.id)+"/mavros/local_position/pose", PoseStamped , self.local_pose_callback) - self.cmd_vel_sub = rospy.Subscriber("/xtdrone/leader/cmd_vel", Twist, self.cmd_vel_callback) - self.avoid_vel_sub = rospy.Subscriber("/xtdrone/uav"+str(self.id)+"/avoid_accel", Vector3, self.avoid_accel_callback) - self.formation_switch_sub = rospy.Subscriber("/gcs/formation_switch",String, self.cmd_callback) + self.local_pose_sub = rospy.Subscriber(uav_type+'_'+str(self.id)+"/mavros/local_position/pose", PoseStamped , self.local_pose_callback) + self.cmd_vel_sub = rospy.Subscriber("/xtdrone/leader/cmd_vel_flu", Twist, self.cmd_vel_callback) + self.avoid_vel_sub = rospy.Subscriber("/xtdrone/"+uav_type+'_'+str(self.id)+"/avoid_accel", Vector3, self.avoid_accel_callback) + self.leader_cmd_sub = rospy.Subscriber("/xtdrone/leader/cmd",String, self.cmd_callback) + for i in range(self.follower_num): - rospy.Subscriber('/xtdrone/uav'+str(i+1)+'/info',String,self.followers_info_callback,i) + rospy.Subscriber('/xtdrone/'+uav_type+'_'+str(i+1)+'/info',String,self.followers_info_callback,i) self.local_pose_pub = rospy.Publisher("/xtdrone/leader/pose", PoseStamped , queue_size=10) self.formation_switch_pub = rospy.Publisher("/xtdrone/formation_switch",String, queue_size=10) - self.vel_enu_pub = rospy.Publisher('/xtdrone/uav'+str(self.id)+'/cmd_vel_enu', Twist, queue_size=10) + self.vel_enu_pub = rospy.Publisher('/xtdrone/'+uav_type+'_'+str(self.id)+'/cmd_vel_enu', Twist, queue_size=10) + self.cmd_pub = rospy.Publisher('/xtdrone/'+uav_type+'_'+str(self.id)+'/cmd', String, queue_size=10) def local_pose_callback(self, msg): self.local_pose = msg @@ -46,9 +53,10 @@ class Leader: self.hover = False def cmd_callback(self, msg): - if not msg.data == '': + if msg.data in formation_dict.keys(): self.formation_config = msg.data - #print("Switch to Formation"+self.formation_config) + else: + self.cmd = msg.data def avoid_accel_callback(self, msg): self.avoid_accel = msg @@ -79,8 +87,9 @@ class Leader: self.cmd_vel_enu.linear.z += self.avoid_accel.z self.vel_enu_pub.publish(self.cmd_vel_enu) self.local_pose_pub.publish(self.local_pose) + self.cmd_pub.publish(self.cmd) rate.sleep() if __name__ == '__main__': - leader = Leader(1,int(sys.argv[1])) + leader = Leader(sys.argv[1], 0, int(sys.argv[2])) leader.loop() diff --git a/coordination/formation_demo/run.sh b/coordination/formation_demo/run.sh index 7fbff98..186b4a0 100755 --- a/coordination/formation_demo/run.sh +++ b/coordination/formation_demo/run.sh @@ -1,9 +1,8 @@ #!/bin/bash -python leader.py $1 & -uav_num=2 -while(( $uav_num<= $1 )) +python leader.py $1 $2 & +uav_num=1 +while(( $uav_num< $2 )) do - python follower_accel_control.py $uav_num $1& - #echo $uav_num + python follower_accel_control.py $1 $uav_num $2& let "uav_num++" done diff --git a/coordination/launch/generator.py b/coordination/launch/generator.py index 8cb6389..2b58fda 100644 --- a/coordination/launch/generator.py +++ b/coordination/launch/generator.py @@ -1,4 +1,3 @@ -#-*-coding：gbk-*- import sys welcome_msg = """ diff --git a/coordination/launch/multi_vehicle.launch b/coordination/launch/multi_vehicle.launch index 2914d43..5f1658c 100644 --- a/coordination/launch/multi_vehicle.launch +++ b/coordination/launch/multi_vehicle.launch @@ -32,8 +32,8 @@ - - + + @@ -47,8 +47,8 @@ - - + + @@ -61,8 +61,8 @@ - - + + @@ -76,22 +76,22 @@ - - + + - - + + - - + + @@ -105,8 +105,8 @@ - - + + @@ -114,13 +114,13 @@ - + - - + + @@ -134,22 +134,22 @@ - - + + - + - + - - + + @@ -162,238 +162,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -403,8 +171,8 @@ - - + + @@ -424,92 +192,5 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/coordination/simple_simulator/simple_3d_simulator_accel.py b/coordination/simple_simulator/simple_3d_simulator_accel.py index 0f68086..20c7d00 100644 --- a/coordination/simple_simulator/simple_3d_simulator_accel.py +++ b/coordination/simple_simulator/simple_3d_simulator_accel.py @@ -7,10 +7,8 @@ from geometry_msgs.msg import Twist,Pose,PoseStamped,TwistStamped from gazebo_msgs.srv import GetModelState import sys -use_1_8 = 1 - -#uav_num=9 -uav_num = int(sys.argv[1]) +uav_type = sys.argv[1] +uav_num = int(sys.argv[2]) step_time=0.005 @@ -23,11 +21,10 @@ plot_z=[0]*(uav_num) local_vel = [TwistStamped() for i in range (uav_num)] for i in range(uav_num): - uav_id=i+use_1_8 plot_x[i]= i//3 plot_y[i]= i%3 - pose_puber[i]=rospy.Publisher('/uav'+str(uav_id)+'/mavros/local_position/pose', PoseStamped, queue_size=10) - vel_puber[i]=rospy.Publisher('/uav'+str(uav_id)+'/mavros/local_position/velocity_local', TwistStamped, queue_size=10) + pose_puber[i]=rospy.Publisher(uav_type+'_'+str(i)+'/mavros/local_position/pose', PoseStamped, queue_size=2) + vel_puber[i]=rospy.Publisher(uav_type+'_'+str(i)+'/mavros/local_position/velocity_local', TwistStamped, queue_size=2) def cmd_vel_callback(msg,id): @@ -41,9 +38,8 @@ rospy.init_node('simple_3d_simulator') rate = rospy.Rate(1/step_time) for i in range(uav_num): - uav_id=i+use_1_8 - rospy.Subscriber('/xtdrone/uav'+str(uav_id)+'/cmd_vel_flu', Twist, cmd_vel_callback,i) - rospy.Subscriber('/xtdrone/uav'+str(uav_id)+'/cmd_vel_enu', Twist, cmd_vel_callback,i) + rospy.Subscriber('/xtdrone/'+uav_type+'_'+str(i)+'/cmd_vel_flu', Twist, cmd_vel_callback,i) + rospy.Subscriber('/xtdrone/'+uav_type+'_'+str(i)+'/cmd_vel_enu', Twist, cmd_vel_callback,i) while not rospy.is_shutdown(): diff --git a/coordination/simple_simulator/visualize.py b/coordination/simple_simulator/visualize.py index e1c035d..6d4ff03 100644 --- a/coordination/simple_simulator/visualize.py +++ b/coordination/simple_simulator/visualize.py @@ -7,7 +7,8 @@ from geometry_msgs.msg import Twist,Pose,PoseStamped,TwistStamped from gazebo_msgs.srv import GetModelState import sys -uav_num = int(sys.argv[1]) +uav_type = sys.argv[1] +uav_num = int(sys.argv[2]) step_time=0.005 @@ -51,7 +52,7 @@ rospy.init_node('visualize') rate = rospy.Rate(1/step_time) for i in range(uav_num): - rospy.Subscriber('/uav'+str(i+1)+'/mavros/local_position/pose', PoseStamped, pose_sub_callback,i) + rospy.Subscriber(uav_type+'_'+str(i)+'/mavros/local_position/pose', PoseStamped, pose_sub_callback,i) try: while not rospy.is_shutdown():