modify cformation demo based on the paper

This commit is contained in:
MALAN 2021-06-12 15:51:58 +08:00
parent 8b21fe21b5
commit 30326783ef
7 changed files with 298 additions and 125 deletions

View File

@ -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

View File

@ -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
'''

View File

@ -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)

View File

@ -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)

View File

@ -1,109 +0,0 @@
<?xml version="1.0"?>
<launch>
<!-- MAVROS posix SITL environment launch script -->
<!-- launches Gazebo environment and 2x: MAVROS, PX4 SITL, and spawns vehicle -->
<!-- vehicle model and world -->
<arg name="est" default="ekf2"/>
<arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/outdoor3.world"/>
<!-- gazebo configs -->
<arg name="gui" default="true"/>
<arg name="debug" default="false"/>
<arg name="verbose" default="false"/>
<arg name="paused" default="false"/>
<!-- Gazebo sim -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="gui" value="$(arg gui)"/>
<arg name="world_name" value="$(arg world)"/>
<arg name="debug" value="$(arg debug)"/>
<arg name="verbose" value="$(arg verbose)"/>
<arg name="paused" value="$(arg paused)"/>
</include>
<!-- rover_0 -->
<group ns="rover_0">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="0"/>
<arg name="ID_in_group" value="0"/>
<arg name="fcu_url" default="udp://:24540@localhost:34580"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" value="3"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="rover"/>
<arg name="sdf" value="rover_with_lidar_stereo"/>
<arg name="mavlink_udp_port" value="18570"/>
<arg name="mavlink_tcp_port" value="4560"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- rover_1 -->
<group ns="rover_1">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="1"/>
<arg name="ID_in_group" value="1"/>
<arg name="fcu_url" default="udp://:24541@localhost:34581"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<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"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="rover"/>
<arg name="sdf" value="rover_with_lidar_stereo"/>
<arg name="mavlink_udp_port" value="18571"/>
<arg name="mavlink_tcp_port" value="4561"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<!-- rover_2 -->
<group ns="rover_2">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="2"/>
<arg name="ID_in_group" value="2"/>
<arg name="fcu_url" default="udp://:24542@localhost:34582"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<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"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="rover"/>
<arg name="sdf" value="rover_with_lidar_stereo"/>
<arg name="mavlink_udp_port" value="18572"/>
<arg name="mavlink_tcp_port" value="4562"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
</launch>
<!--the launch file is generated by XTDrone multi-vehicle generator.py -->

View File

@ -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])

View File

@ -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
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