From 30326783ef5f0d6e10e24c4f6757543c1d06cdff Mon Sep 17 00:00:00 2001 From: MALAN Date: Sat, 12 Jun 2021 15:51:58 +0800 Subject: [PATCH] modify cformation demo based on the paper --- communication/multi_vehicle_communication.sh | 6 +- communication/multirotor_communication.py | 2 +- .../my_multirotor_keyboard_control.py | 280 ++++++++++++++++++ coordination/formation_demo/follower.py | 8 +- .../launch_generator/multi_vehicle.launch | 109 ------- sensing/pose_ground_truth/get_local_pose.py | 2 +- .../get_multi_vehcle_local_pose.sh | 16 +- 7 files changed, 298 insertions(+), 125 deletions(-) create mode 100644 control/keyboard/my_multirotor_keyboard_control.py delete mode 100644 coordination/launch_generator/multi_vehicle.launch diff --git a/communication/multi_vehicle_communication.sh b/communication/multi_vehicle_communication.sh index 6f9a9b1..3f24a86 100644 --- a/communication/multi_vehicle_communication.sh +++ b/communication/multi_vehicle_communication.sh @@ -1,5 +1,5 @@ #!/bin/bash -iris_num=3 +iris_num=9 typhoon_h480_num=0 solo_num=0 plane_num=0 @@ -11,14 +11,14 @@ tailsitter_num=0 vehicle_num=0 while(( $vehicle_num< iris_num)) do - python multirotor_communication.py iris $vehicle_num& + python2.7 multirotor_communication.py iris $vehicle_num& let "vehicle_num++" done vehicle_num=0 while(( $vehicle_num< typhoon_h480_num)) do - python multirotor_communication.py typhoon_h480 $vehicle_num& + python2.7 multirotor_communication.py typhoon_h480 $vehicle_num& let "vehicle_num++" done diff --git a/communication/multirotor_communication.py b/communication/multirotor_communication.py index 6843d0b..dd33d46 100644 --- a/communication/multirotor_communication.py +++ b/communication/multirotor_communication.py @@ -53,7 +53,7 @@ class Communication: def start(self): rospy.init_node(self.vehicle_type+'_'+self.vehicle_id+"_communication") - rate = rospy.Rate(100) + rate = rospy.Rate(30) ''' main ROS thread ''' diff --git a/control/keyboard/my_multirotor_keyboard_control.py b/control/keyboard/my_multirotor_keyboard_control.py new file mode 100644 index 0000000..a0d91f4 --- /dev/null +++ b/control/keyboard/my_multirotor_keyboard_control.py @@ -0,0 +1,280 @@ +# encoding: utf-8 +import rospy +from geometry_msgs.msg import Twist +import sys, select, os +import tty, termios +from std_msgs.msg import String + + +MAX_LINEAR = 20 +MAX_ANG_VEL = 3 +LINEAR_STEP_SIZE = 0.1 +ANG_VEL_STEP_SIZE = 0.1 + +cmd_vel_mask = False +ctrl_leader = False #初始 控制所有无人机 + +msg2all = """ +Control Your XTDrone! +To all drones (press g to control the leader) +--------------------------- + 1 2 3 4 5 6 7 8 9 0 + w r t y i + a s d g j k l + x v b n , + +w/x : increase/decrease forward velocity +a/d : increase/decrease leftward velocity +i/, : increase/decrease upward velocity +j/l : increase/decrease orientation +r : return home +t/y : arm/disarm +v/n : takeoff/land +b : offboard +s/k : hover and remove the mask of keyboard control +0~9 : extendable mission(eg.different formation configuration) + this will mask the keyboard control +g : control the leader +CTRL-C to quit +""" + +msg2leader = """ +Control Your XTDrone! +To the leader (press g to control all drones) +--------------------------- + 1 2 3 4 5 6 7 8 9 0 + w r t y i + a s d g j k l + x v b n , + +w/x : increase/decrease forward velocity +a/d : increase/decrease leftward velocity +i/, : increase/decrease upward velocity +j/l : increase/decrease orientation +r : return home +t/y : arm/disarm +v/n : takeoff(disenabled now)/land +b : offboard +s/k : hover and remove the mask of keyboard control +0~9 : extendable mission(eg.different formation configuration) + this will mask the keyboard control +g : control all drones +CTRL-C to quit +""" + +e = """ +Communications Failed +""" + +def getKey(): # 获得键盘按键 + tty.setraw(sys.stdin.fileno()) + rlist, _, _ = select.select([sys.stdin], [], [], 0.1) + if rlist: + key = sys.stdin.read(1) + else: + key = '' + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) + return key + +def print_msg(): # 打印提示信息 + if ctrl_leader: + print(msg2leader) + else: + print(msg2all) + +if __name__=="__main__": + + settings = termios.tcgetattr(sys.stdin) + + multirotor_type = sys.argv[1] + multirotor_num = int(sys.argv[2]) + control_type = sys.argv[3] #vel 速度控制 + ''' + if multirotor_num == 18: #多无人机编队 + formation_configs = ['waiting', 'cuboid', 'sphere', 'diamond'] # 0-9的参数 + elif multirotor_num == 9: + formation_configs = ['waiting', 'cube', 'pyramid', 'triangle'] + elif multirotor_num == 6: + formation_configs = ['waiting', 'T', 'diamond', 'triangle'] + ''' # 0 1 2 3 + if multirotor_num == 21: + formation_configs = ['waiting', 'cube21', 'circle21', 'pyramid21', 'diamond21'] + elif multirotor_num == 34: + formation_configs = ['waiting', 'CUBE', 'HEART', '520', 'NUDT', '八一'] + + cmd= String() + twist = Twist() + + rospy.init_node('multirotor_keyboard_multi_control') + + if control_type == 'vel': # 速度控制 + multi_cmd_vel_flu_pub = [None]*multirotor_num + multi_cmd_pub = [None]*multirotor_num + for i in range(multirotor_num): + 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) + + else: # 加速度控制 + multi_cmd_accel_flu_pub = [None]*multirotor_num + multi_cmd_pub = [None]*multirotor_num + for i in range(multirotor_num): + 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) + + forward = 0.0 + leftward = 0.0 + upward = 0.0 + angular = 0.0 + + print_msg() + while(1): + key = getKey() + if key == 'w' : + forward = forward + LINEAR_STEP_SIZE + print_msg() + if control_type == 'vel': + print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) + else: + print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) + + elif key == 'x' : + forward = forward - LINEAR_STEP_SIZE + print_msg() + if control_type == 'vel': + print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) + else: + print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) + + elif key == 'a' : + + leftward = leftward + LINEAR_STEP_SIZE + print_msg() + if control_type == 'vel': + print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) + else: + print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) + + elif key == 'd' : + leftward = leftward - LINEAR_STEP_SIZE + print_msg() + if control_type == 'vel': + print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) + else: + print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) + + elif key == 'i' : + upward = upward + LINEAR_STEP_SIZE + print_msg() + if control_type == 'vel': + print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) + else: + print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) + + elif key == ',' : + upward = upward - LINEAR_STEP_SIZE + print_msg() + if control_type == 'vel': + print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) + else: + print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) + + elif key == 'j': + angular = angular + ANG_VEL_STEP_SIZE + print_msg() + print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) + + elif key == 'l': + angular = angular - ANG_VEL_STEP_SIZE + print_msg() + print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) + + elif key == 'r': # 自动返回起飞点 + cmd = 'AUTO.RTL' + print_msg() + print('Returning home') + elif key == 't': #飞机解锁 + cmd = 'ARM' + print_msg() + print('Arming') + elif key == 'y': # 飞机上锁 + cmd = 'DISARM' + print_msg() + print('Disarming') + elif key == 'v': + cmd = 'AUTO.TAKEOFF' + cmd = '' + print_msg() + #print('Takeoff mode is disenabled now') + elif key == 'b': + cmd = 'OFFBOARD' + print_msg() + print('Offboard') + elif key == 'n': # 自动着陆 + cmd = 'AUTO.LAND' + print_msg() + print('Landing') + elif key == 'g': # 开启或结束编队控制 + ctrl_leader = not ctrl_leader + print_msg() + elif key in ['k', 's']: # 悬停，全部参数归零 + cmd_vel_mask = False + forward = 0.0 + leftward = 0.0 + upward = 0.0 + angular = 0.0 + cmd = 'HOVER' + print_msg() + print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) + print('Hover') + else: + for i in range(10): # 按键数字 + if key == str(i): + cmd = formation_configs[i] # 编队的队形 + print_msg() + print(cmd) + cmd_vel_mask = True # 有掩模，其他按键无效 + if (key == '\x03'): + break + + if forward > MAX_LINEAR: + forward = MAX_LINEAR + elif forward < -MAX_LINEAR: + forward = -MAX_LINEAR + if leftward > MAX_LINEAR: + leftward = MAX_LINEAR + elif leftward < -MAX_LINEAR: + leftward = -MAX_LINEAR + if upward > MAX_LINEAR: + upward = MAX_LINEAR + elif upward < -MAX_LINEAR: + upward = -MAX_LINEAR + if angular > MAX_ANG_VEL: + angular = MAX_ANG_VEL + elif angular < -MAX_ANG_VEL: + angular = - MAX_ANG_VEL + + twist.linear.x = forward; twist.linear.y = leftward ; twist.linear.z = upward + twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = angular + + for i in range(multirotor_num): # 无人机数量 + if ctrl_leader: + if control_type == 'vel': + leader_cmd_vel_flu_pub.publish(twist) + else: + leader_cmd_aceel_flu_pub.publish(twist) #无定义 + leader_cmd_pub.publish(cmd) + else: + if not cmd_vel_mask: #只有在输入0-9时，mask为True，其余情况为F 。这里是非数字时 + if control_type == 'vel': + multi_cmd_vel_flu_pub[i].publish(twist) + else: + multi_cmd_accel_flu_pub[i].publish(twist) + multi_cmd_pub[i].publish(cmd) + + cmd = '' + + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) diff --git a/coordination/formation_demo/follower.py b/coordination/formation_demo/follower.py index c1b8893..e484660 100644 --- a/coordination/formation_demo/follower.py +++ b/coordination/formation_demo/follower.py @@ -119,8 +119,10 @@ class Follower: self.change_id = self.KM() # Get a new formation pattern of UAVs based on KM. + #self.L_matrix = self.get_L_matrix(self.orig_formation) 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.L_matrix = self.get_L_matrix(self.new_formation) + self.L_matrix = self.get_L_central_matrix() self.orig_formation=self.new_formation if self.id == 3: print(self.L_matrix) @@ -238,7 +240,7 @@ class Follower: def get_L_matrix(self, rel_posi): #假设无论多少UAV，都假设尽可能3层通信（叶子节点除外） - c_num=int((self.uav_num-1)/3) + c_num=int((self.uav_num+1)/3) comm=[[]for i in range (self.uav_num)] w=numpy.ones((self.uav_num,self.uav_num))*0 # 定义邻接矩阵 @@ -272,7 +274,7 @@ class Follower: if i==next_node or i in node_flag: - rel_d[i]=2000 #这个2000是根据相对位置和整个地图的大小决定的，要比最大可能相对距离大 + rel_d[i]=1e10 #要比最大可能相对距离大 else: rel_d[i]=pow((rel_posi[0][i]-rel_posi[0][next_node]),2)+pow((rel_posi[1][i]-rel_posi[1][next_node]),2)+pow((rel_posi[2][i]-rel_posi[2][next_node]),2) diff --git a/coordination/launch_generator/multi_vehicle.launch b/coordination/launch_generator/multi_vehicle.launch deleted file mode 100644 index 43e06cc..0000000 --- a/coordination/launch_generator/multi_vehicle.launch +++ /dev/null @@ -1,109 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/sensing/pose_ground_truth/get_local_pose.py b/sensing/pose_ground_truth/get_local_pose.py index f807dab..b24b707 100644 --- a/sensing/pose_ground_truth/get_local_pose.py +++ b/sensing/pose_ground_truth/get_local_pose.py @@ -25,7 +25,7 @@ if __name__ == '__main__': multi_odom_groundtruth_sub[i] = rospy.Subscriber('/xtdrone/'+vehicle_type+'_'+str(i)+'/ground_truth/odom', Odometry, odm_groundtruth_callback, i) multi_pose_pub[i] = rospy.Publisher(vehicle_type+'_'+str(i)+'/mavros/vision_pose/pose', PoseStamped, queue_size=2) multi_speed_pub[i] = rospy.Publisher(vehicle_type+'_'+str(i)+'/mavros/vision_speed/speed', Vector3Stamped, queue_size=2) - rate = rospy.Rate(100) + rate = rospy.Rate(50) while True: for i in range(vehicle_num): multi_pose_pub[i].publish(multi_local_pose[i]) diff --git a/sensing/pose_ground_truth/get_multi_vehcle_local_pose.sh b/sensing/pose_ground_truth/get_multi_vehcle_local_pose.sh index 196cea3..926edc4 100644 --- a/sensing/pose_ground_truth/get_multi_vehcle_local_pose.sh +++ b/sensing/pose_ground_truth/get_multi_vehcle_local_pose.sh @@ -9,11 +9,11 @@ tiltrotor_num=0 tailsitter_num=0 -python get_local_pose.py iris $iris_num& -python get_local_pose.py typhoon_h480 $typhoon_h480_num& -python get_local_pose.py solo $solo_num& -python get_local_pose.py plane $plane_num& -python get_local_pose.py rover $rover_num& -python get_local_pose.py standard_vtol $standard_vtol_num& -python get_local_pose.py tiltrotor $tiltrotor_num& -python get_local_pose.py tailsitter $tailsitter_num \ No newline at end of file +python2.7 get_local_pose.py iris $iris_num& +python2.7 get_local_pose.py typhoon_h480 $typhoon_h480_num& +python2.7 get_local_pose.py solo $solo_num& +python2.7 get_local_pose.py plane $plane_num& +python2.7 get_local_pose.py rover $rover_num& +python2.7 get_local_pose.py standard_vtol $standard_vtol_num& +python2.7 get_local_pose.py tiltrotor $tiltrotor_num& +python2.7 get_local_pose.py tailsitter $tailsitter_num