From 482eb88d96dd6ebbf336f2f5983410b67a47944a Mon Sep 17 00:00:00 2001 From: lan Date: Wed, 25 Mar 2020 08:36:56 -0700 Subject: [PATCH] wanshanbiandui --- control/follower_accel_control.py | 52 +++++++++++++++++--------- control/follower_vel_control.py | 8 ++-- control/formation_dict.pyc | Bin 1151 -> 1145 bytes control/keyboard_multi_control_new.py | 4 +- control/leader.py | 2 +- control/run.sh | 4 +- control/simple_3d_simulator_accel.py | 29 ++++++++------ control/stop.sh | 4 +- 8 files changed, 63 insertions(+), 40 deletions(-) diff --git a/control/follower_accel_control.py b/control/follower_accel_control.py index 879fe4e..f1b3b7c 100644 --- a/control/follower_accel_control.py +++ b/control/follower_accel_control.py @@ -4,12 +4,13 @@ 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 +from formation_dict import formation_dict_9 import time import math import numpy import sys import heapq +import copy class Follower: @@ -26,14 +27,16 @@ class Follower: self.following_ids = [] self.formation_config = 'waiting' self.following_count = 0 - self.Kp = 100 + self.Kp = 50 #100 self.kr = (4/int((self.uav_num-1)/3))**0.5 + #self.kr = 1 self.f = 50 - self.velxy_max = 0.5 + #self.f=1/0.01 + self.velxy_max = 0.5 #0.5 self.velz_max = 1 - self.following_local_pose = [None]*self.uav_num + self.following_local_pose = [PoseStamped()]*self.uav_num self.following_local_pose_sub = [None]*self.uav_num - self.following_local_velocity = [None]*self.uav_num + self.following_local_velocity = [TwistStamped()]*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) @@ -44,19 +47,22 @@ class Follower: 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) + def local_pose_callback(self, msg): self.local_pose = msg #print(self.arrive_count) def local_velocity_callback(self, msg): self.local_velocity = msg + #print(self.arrive_count) def following_local_pose_callback(self, msg, id): self.following_local_pose[id] = msg - #print('here') + def following_local_velocity_callback(self, msg, id): self.following_local_velocity[id] = msg + def cmd_vel_callback(self, msg): self.cmd_vel_enu = msg @@ -79,7 +85,7 @@ class Follower: def loop(self): rospy.init_node('follower'+str(self.id-1)) rate = rospy.Rate(self.f) - while True: + while not rospy.is_shutdown(): if self.arrive_count > 100: self.info_pub.publish("Arrived") #print("Arrived") @@ -87,7 +93,7 @@ class Follower: self.following_switch = False self.info_pub.publish("Received") print("Follower"+str(self.id-1)+": Switch to Formation "+self.formation_config) - self.L_matrix = self.get_L_matrix(formation_dict[self.formation_config]) + self.L_matrix = self.get_L_matrix(formation_dict_9[self.formation_config]) #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) @@ -107,32 +113,42 @@ class Follower: time.sleep(1) self.cmd_accel_enu = Vector3(0, 0, 0) + #self.cmd_vel_enu.linear = copy.deepcopy(self.avoid_vel) 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[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] + 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] + if not following_id[0] == 0: - 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] + 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_vel_enu.linear.x = self.local_velocity.twist.linear.x + self.avoid_vel.x + self.Kp * self.cmd_accel_enu.x / self.f self.cmd_vel_enu.linear.y = self.local_velocity.twist.linear.y +self.avoid_vel.y + self.Kp * self.cmd_accel_enu.y / self.f self.cmd_vel_enu.linear.z = self.local_velocity.twist.linear.z +self.avoid_vel.z + self.Kp * self.cmd_accel_enu.z / self.f + if self.cmd_vel_enu.linear.x > self.velxy_max: self.cmd_vel_enu.linear.x = self.velxy_max + elif self.cmd_vel_enu.linear.x < - self.velxy_max: + self.cmd_vel_enu.linear.x = - self.velxy_max if self.cmd_vel_enu.linear.y > self.velxy_max: self.cmd_vel_enu.linear.y = self.velxy_max + elif self.cmd_vel_enu.linear.y < - self.velxy_max: + self.cmd_vel_enu.linear.y = - self.velxy_max if self.cmd_vel_enu.linear.z > self.velz_max: self.cmd_vel_enu.linear.z = self.velz_max + elif self.cmd_vel_enu.linear.z < - self.velz_max: + self.cmd_vel_enu.linear.z = - self.velz_max if (self.cmd_vel_enu.linear.x)**2+(self.cmd_vel_enu.linear.y)**2+(self.cmd_vel_enu.linear.z)**2<0.2: self.arrive_count += 1 else: self.arrive_count = 0 - self.vel_enu_pub.publish(self.cmd_vel_enu) + if not self.formation_config == 'waiting': + self.vel_enu_pub.publish(self.cmd_vel_enu) rate.sleep() #函数输入为相对leader的位置矩阵和无人机数量，输出为L矩阵 @@ -323,5 +339,7 @@ class Follower: ''' if __name__ == '__main__': - follower = Follower(int(sys.argv[1]),9) + follower = Follower(int(sys.argv[1]),int(sys.argv[2])) + #follower = Follower(int(sys.argv[1]),9) + #follower = Follower(9,9) follower.loop() diff --git a/control/follower_vel_control.py b/control/follower_vel_control.py index 6a2ed92..5b3b374 100644 --- a/control/follower_vel_control.py +++ b/control/follower_vel_control.py @@ -77,9 +77,9 @@ class Follower: self.following_switch = False self.info_pub.publish("Received") print("Follower"+str(self.id-1)+": Switch to Formation "+self.formation_config) - #self.L_matrix = self.get_L_matrix(formation_dict_9[self.formation_config]) + self.L_matrix = self.get_L_matrix(formation_dict_9[self.formation_config]) #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.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) #if self.id == 2: #print(self.following_ids) @@ -92,13 +92,13 @@ class Follower: 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_count += 1 time.sleep(1) - print(self.avoid_vel) + #print(self.avoid_vel) self.cmd_vel_enu.linear = copy.deepcopy(self.avoid_vel) if self.cmd_vel_enu.linear.x == 0 and self.cmd_vel_enu.linear.y == 0 and self.cmd_vel_enu.linear.z == 0: for following_id in self.following_ids: self.cmd_vel_enu.linear.x += self.following_local_pose[following_id[0]].pose.position.x - self.local_pose.pose.position.x + formation_dict_9[self.formation_config][0, self.id-2] self.cmd_vel_enu.linear.y += self.following_local_pose[following_id[0]].pose.position.y - self.local_pose.pose.position.y + formation_dict_9[self.formation_config][1, self.id-2] - self.cmd_vel_enu.linear.z += self.following_local_pose[following_id[0]].pose.position.z - self.local_pose.pose.position.z+ formation_dict_9[self.formation_config][2, self.id-2] + self.cmd_vel_enu.linear.z += self.following_local_pose[following_id[0]].pose.position.z - self.local_pose.pose.position.z + formation_dict_9[self.formation_config][2, self.id-2] if not following_id[0] == 0: self.cmd_vel_enu.linear.x -= formation_dict_9[self.formation_config][0, following_id[0]-1] self.cmd_vel_enu.linear.y -= formation_dict_9[self.formation_config][1, following_id[0]-1] diff --git a/control/formation_dict.pyc b/control/formation_dict.pyc index 6b498c83961a31cc99c39be3b1ba8d1782eafe84..adef9d527d6c7ba9049b4f16389a533be1dc6175 100644 GIT binary patch delta 38 ucmey*@sop{`721r;Xx6(uQ?4Ou(@LaYt| diff --git a/control/keyboard_multi_control_new.py b/control/keyboard_multi_control_new.py index fd7c841..de4c674 100755 --- a/control/keyboard_multi_control_new.py +++ b/control/keyboard_multi_control_new.py @@ -12,8 +12,8 @@ ANG_VEL_STEP_SIZE = 0.01 cmd_vel_mask = False ctrl_leader = False - -uav_num = int(sys.argv[1]) +uav_num=9 +#uav_num = int(sys.argv[1]) if uav_num == 9: formation_configs = ['waiting', 'cube', 'pyramid', 'triangle'] if uav_num == 6: diff --git a/control/leader.py b/control/leader.py index a17f44c..d3458e5 100644 --- a/control/leader.py +++ b/control/leader.py @@ -61,7 +61,7 @@ class Leader: rospy.init_node('leader') rate = rospy.Rate(50) while True: - self.cmd_vel_enu = Twist() + #self.cmd_vel_enu = Twist() for follower_info in self.followers_info: if follower_info == "Arrived": self.follower_arrived_num += 1 diff --git a/control/run.sh b/control/run.sh index b1b181d..7fbff98 100755 --- a/control/run.sh +++ b/control/run.sh @@ -3,7 +3,7 @@ python leader.py $1 & uav_num=2 while(( $uav_num<= $1 )) do - python follower_vel_control.py $uav_num $1& + python follower_accel_control.py $uav_num $1& #echo $uav_num let "uav_num++" -done \ No newline at end of file +done diff --git a/control/simple_3d_simulator_accel.py b/control/simple_3d_simulator_accel.py index 7f18915..4a8765a 100644 --- a/control/simple_3d_simulator_accel.py +++ b/control/simple_3d_simulator_accel.py @@ -9,10 +9,10 @@ import sys use_1_8 = 1 +uav_num=9 +#uav_num = int(sys.argv[1]) -uav_num = int(sys.argv[1]) - -step_time=0.001 +step_time=0.01 pose_puber=[None]*uav_num vel_puber=[None]*uav_num @@ -20,6 +20,7 @@ vel_puber=[None]*uav_num plot_x=[0]*(uav_num) plot_y=[0]*(uav_num) plot_z=[0]*(uav_num) +local_vel = [TwistStamped()]*(uav_num) for i in range(uav_num): uav_id=i+use_1_8 @@ -51,20 +52,24 @@ def init(): ax.set_zlim3d(-label_lim, label_lim) -#def cmd_accel_callback(msg,id): - accel=msg - plot_x[id]+=step_time*accel.linear.x - plot_y[id]+=step_time*accel.linear.y - plot_z[id]+=step_time*accel.linear.z +def cmd_vel_callback(msg,id): + # accel=msg + # plot_x[id]+=step_time*accel.linear.x + # plot_y[id]+=step_time*accel.linear.y + # plot_z[id]+=step_time*accel.linear.z + local_vel[id].twist=msg + plot_x[id]+=step_time*local_vel[id].twist.linear.x + plot_y[id]+=step_time*local_vel[id].twist.linear.y + plot_z[id]+=step_time*local_vel[id].twist.linear.z rospy.init_node('simple_3d_simulator') -rate = rospy.Rate(1000) +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_accel_flu', Twist, cmd_accel_callback,i) - rospy.Subscriber('/xtdrone/uav'+str(uav_id)+'/cmd_accel_enu', Twist, cmd_accel_callback,i) + 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) try: while not rospy.is_shutdown(): @@ -74,7 +79,7 @@ try: local_pose.pose.position.y=plot_y[i] local_pose.pose.position.z=plot_z[i] pose_puber[i].publish(local_pose) - pose_puber[i].publish(local_pose) + vel_puber[i].publish(local_vel[i]) init() ax.scatter(plot_x,plot_y,plot_z,marker="x") plt.pause(step_time) diff --git a/control/stop.sh b/control/stop.sh index 7c5ea01..838629a 100755 --- a/control/stop.sh +++ b/control/stop.sh @@ -1,2 +1,2 @@ -kill -9 $(ps -ef|grep follower_vel_control.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ') -kill -9 $(ps -ef|grep leader.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ') \ No newline at end of file +kill -9 $(ps -ef|grep follower_accel_control.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ') +kill -9 $(ps -ef|grep leader.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ')