forked from xtdrone/XTDrone
完善编队
This commit is contained in:
parent
587d396f8c
commit
b1d71f2392
|
|
@ -0,0 +1,238 @@
|
|||
import rospy
|
||||
import tf
|
||||
from mavros_msgs.msg import GlobalPositionTarget, State, PositionTarget
|
||||
from mavros_msgs.srv import CommandBool, CommandTOL, SetMode
|
||||
from geometry_msgs.msg import PoseStamped, Pose, TwistStamped, Twist, Vector3Stamped, Vector3
|
||||
from sensor_msgs.msg import Imu, NavSatFix
|
||||
from std_msgs.msg import String
|
||||
import time
|
||||
from pyquaternion import Quaternion
|
||||
import math
|
||||
from multiprocessing import Process
|
||||
import sys
|
||||
|
||||
class PX4Communication:
|
||||
|
||||
def __init__(self,id):
|
||||
|
||||
self.id = id
|
||||
self.imu = None
|
||||
self.local_pose = None
|
||||
self.current_state = None
|
||||
self.current_heading = None
|
||||
self.takeoff_height = 1
|
||||
|
||||
self.target_motion = PositionTarget()
|
||||
self.global_target = None
|
||||
|
||||
self.arm_state = False
|
||||
self.offboard_state = False
|
||||
self.flag = 0
|
||||
self.flight_mode = None
|
||||
self.mission = None
|
||||
|
||||
'''
|
||||
ros subscribers
|
||||
'''
|
||||
self.local_pose_sub = rospy.Subscriber("/uav"+str(self.id)+"/mavros/local_position/pose", PoseStamped, self.local_pose_callback)
|
||||
self.mavros_sub = rospy.Subscriber("/uav"+str(self.id)+"/mavros/state", State, self.mavros_state_callback)
|
||||
self.imu_sub = rospy.Subscriber("/uav"+str(self.id)+"/mavros/imu/data", Imu, self.imu_callback)
|
||||
self.cmd_pose_flu_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd_pose_flu", Pose, self.cmd_pose_flu_callback)
|
||||
self.cmd_pose_enu_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd_pose_enu", Pose, self.cmd_pose_enu_callback)
|
||||
self.cmd_vel_flu_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd_vel_flu", Twist, self.cmd_vel_flu_callback)
|
||||
self.cmd_vel_enu_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd_vel_enu", Twist, self.cmd_vel_enu_callback)
|
||||
self.cmd_vel_flu_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd_accel_flu", Vector3, self.cmd_accel_flu_callback)
|
||||
self.cmd_vel_enu_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd_accel_enu", Vector3, self.cmd_accel_enu_callback)
|
||||
self.cmd_sub = rospy.Subscriber("/xtdrone/"+"uav"+str(self.id)+"/cmd",String,self.cmd_callback)
|
||||
|
||||
'''
|
||||
ros publishers
|
||||
'''
|
||||
self.target_motion_pub = rospy.Publisher("/uav"+str(self.id)+"/mavros/setpoint_raw/local", PositionTarget, queue_size=10)
|
||||
|
||||
'''
|
||||
ros services
|
||||
'''
|
||||
self.armService = rospy.ServiceProxy("/uav"+str(self.id)+"/mavros/cmd/arming", CommandBool)
|
||||
self.flightModeService = rospy.ServiceProxy("/uav"+str(self.id)+"/mavros/set_mode", SetMode)
|
||||
|
||||
|
||||
print("UAV"+str(self.id)+": "+"PX4 Communication Initialized!")
|
||||
|
||||
|
||||
def start(self):
|
||||
rospy.init_node("px4_uav"+str(self.id)+"_communication")
|
||||
rate = rospy.Rate(100)
|
||||
'''
|
||||
main ROS thread
|
||||
'''
|
||||
while(rospy.is_shutdown):
|
||||
self.target_motion_pub.publish(self.target_motion)
|
||||
if (self.flight_mode is "LAND") and (self.local_pose.pose.position.z < 0.15):
|
||||
|
||||
if(self.disarm()):
|
||||
|
||||
self.flight_mode = "DISARMED"
|
||||
|
||||
rate.sleep()
|
||||
|
||||
def local_pose_callback(self, msg):
|
||||
self.local_pose = msg
|
||||
|
||||
def mavros_state_callback(self, msg):
|
||||
self.mavros_state = msg.mode
|
||||
|
||||
def imu_callback(self, msg):
|
||||
self.current_heading = self.q2yaw(msg.orientation)
|
||||
|
||||
def construct_target(self, x=0, y=0, z=0, vx=0, vy=0, vz=0, afx=0, afy=0, afz=0, yaw=0, yaw_rate=0):
|
||||
target_raw_pose = PositionTarget()
|
||||
target_raw_pose.coordinate_frame = 1
|
||||
|
||||
target_raw_pose.position.x = x
|
||||
target_raw_pose.position.y = y
|
||||
target_raw_pose.position.z = z
|
||||
|
||||
target_raw_pose.velocity.x = vx
|
||||
target_raw_pose.velocity.y = vy
|
||||
target_raw_pose.velocity.z = vz
|
||||
|
||||
target_raw_pose.acceleration_or_force.x = afx
|
||||
target_raw_pose.acceleration_or_force.y = afy
|
||||
target_raw_pose.acceleration_or_force.z = afz
|
||||
|
||||
target_raw_pose.yaw = yaw
|
||||
target_raw_pose.yaw_rate = yaw_rate
|
||||
|
||||
if(self.flag == 0):
|
||||
target_raw_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \
|
||||
+ PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
|
||||
+ PositionTarget.IGNORE_YAW
|
||||
if(self.flag == 1):
|
||||
target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \
|
||||
+ PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
|
||||
+ PositionTarget.IGNORE_YAW
|
||||
if(self.flag == 2):
|
||||
target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \
|
||||
+ PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \
|
||||
+ PositionTarget.IGNORE_YAW
|
||||
|
||||
return target_raw_pose
|
||||
|
||||
def cmd_pose_flu_callback(self, msg):
|
||||
self.flag = 0
|
||||
target_x_enu, target_y_enu = self.flu2enu(msg.position.x, msg.position.y)
|
||||
target_z_enu = msg.position.z
|
||||
#target_yaw = self.q2yaw(msg.orientation)+self.current_heading
|
||||
self.target_motion = self.construct_target(x=target_x_enu,y=target_y_enu,z=target_z_enu)
|
||||
|
||||
def cmd_pose_enu_callback(self, msg):
|
||||
self.flag = 0
|
||||
#target_yaw = self.q2yaw(msg.orientation)+self.current_heading
|
||||
self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z)
|
||||
|
||||
def cmd_vel_flu_callback(self, msg):
|
||||
self.flag = 1
|
||||
target_vx_enu, target_vy_enu = self.flu2enu(msg.linear.x, msg.linear.y)
|
||||
target_vz_enu = msg.linear.z
|
||||
target_yaw_rate = msg.angular.z
|
||||
self.target_motion = self.construct_target(vx=target_vx_enu,vy=target_vy_enu,vz=target_vz_enu,yaw_rate=target_yaw_rate)
|
||||
|
||||
def cmd_vel_enu_callback(self, msg):
|
||||
self.flag = 1
|
||||
self.target_motion = self.construct_target(vx=msg.linear.x,vy=msg.linear.y,vz=msg.linear.z,yaw_rate=msg.angular.z)
|
||||
|
||||
def cmd_accel_flu_callback(self, msg):
|
||||
target_afx_enu, target_afy_enu = self.flu2enu(msg.x, msg.y)
|
||||
target_afz_enu = msg.z
|
||||
self.target_motion = self.construct_target(afx=target_afx_enu,afy=target_afy_enu,afz=target_afz_enu)
|
||||
def cmd_accel_enu_callback(self, msg):
|
||||
self.flag = 2
|
||||
self.target_motion = self.construct_target(afx=msg.x,afy=msg.y,afz=msg.z)
|
||||
|
||||
def cmd_callback(self, msg):
|
||||
if msg.data == '':
|
||||
return
|
||||
|
||||
elif msg.data == 'ARM':
|
||||
self.arm_state =self.arm()
|
||||
print("UAV"+str(self.id)+": "+'Armed'+str(self.arm_state))
|
||||
|
||||
elif msg.data == 'DISARM':
|
||||
disarm_state =self.disarm()
|
||||
if disarm_state:
|
||||
self.arm_state = False
|
||||
print("UAV"+str(self.id)+": "+'Armed'+str(self.arm_state))
|
||||
|
||||
elif msg.data[:-1] == "mission" and not msg.data == self.mission:
|
||||
self.mission = msg.data
|
||||
print("UAV"+str(self.id)+": "+msg.data)
|
||||
|
||||
elif not msg.data == self.flight_mode:
|
||||
self.flight_mode = msg.data
|
||||
self.flight_mode_switch()
|
||||
|
||||
|
||||
def q2yaw(self, q):
|
||||
if isinstance(q, Quaternion):
|
||||
rotate_z_rad = q.yaw_pitch_roll[0]
|
||||
else:
|
||||
q_ = Quaternion(q.w, q.x, q.y, q.z)
|
||||
rotate_z_rad = q_.yaw_pitch_roll[0]
|
||||
|
||||
return rotate_z_rad
|
||||
|
||||
def flu2enu(self, x_flu, y_flu):
|
||||
x_enu = x_flu*math.cos(self.current_heading)-y_flu*math.sin(self.current_heading)
|
||||
y_enu = x_flu*math.sin(self.current_heading)+y_flu*math.cos(self.current_heading)
|
||||
return x_enu, y_enu
|
||||
|
||||
def cmd_yaw(self, yaw):
|
||||
quaternion = tf.transformations.quaternion_from_euler(0, 0, yaw)
|
||||
self.target_pose.pose.orientation.x = quaternion[0]
|
||||
self.target_pose.pose.orientation.y = quaternion[1]
|
||||
self.target_pose.pose.orientation.z = quaternion[2]
|
||||
self.target_pose.pose.orientation.w = quaternion[3]
|
||||
|
||||
def arm(self):
|
||||
if self.armService(True):
|
||||
return True
|
||||
else:
|
||||
print("UAV"+str(self.id)+": Vehicle Arming Failed!")
|
||||
return False
|
||||
|
||||
def disarm(self):
|
||||
if self.armService(False):
|
||||
return True
|
||||
else:
|
||||
print("UAV"+str(self.id)+": Vehicle Disarming Failed!")
|
||||
return False
|
||||
|
||||
def hover(self):
|
||||
self.flag = 0
|
||||
self.target_motion = self.construct_target(x=self.local_pose.pose.position.x,y=self.local_pose.pose.position.y,z=self.local_pose.pose.position.z)
|
||||
|
||||
def flight_mode_switch(self):
|
||||
if self.flight_mode == 'HOVER':
|
||||
self.hover()
|
||||
print("UAV"+str(self.id)+": Hover")
|
||||
elif self.flightModeService(custom_mode=self.flight_mode):
|
||||
print("UAV"+str(self.id)+": "+self.flight_mode)
|
||||
return True
|
||||
else:
|
||||
print("UAV"+str(self.id)+": "+self.flight_mode+"Failed")
|
||||
return False
|
||||
|
||||
def takeoff_detection(self):
|
||||
if self.local_pose.pose.position.z > 0.3 and self.arm_state:
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
uav_num = int(sys.argv[1])
|
||||
for i in range(uav_num):
|
||||
px4_com = PX4Communication(i+1)
|
||||
p = Process(target=px4_com.start)
|
||||
p.start()
|
||||
|
|
@ -205,10 +205,10 @@ if __name__=="__main__":
|
|||
ctrl_leader = not ctrl_leader
|
||||
print_msg()
|
||||
elif key == 'k':
|
||||
cmd = 'AUTO.LOITER'
|
||||
cmd = 'HOVER'
|
||||
cmd_vel_mask = False
|
||||
print_msg()
|
||||
print('Loiter')
|
||||
print('Hover')
|
||||
elif key == 's' :
|
||||
cmd_vel_mask = False
|
||||
target_forward_vel = 0.0
|
||||
|
|
|
|||
|
|
@ -9,12 +9,12 @@ import sys
|
|||
uav_num = int(sys.argv[1])
|
||||
pose = [None]*uav_num
|
||||
pose_sub = [None]*uav_num
|
||||
avoid_vel_pub = [None]*uav_num
|
||||
avoid_accel_pub = [None]*uav_num
|
||||
avoid_kp = 1
|
||||
avoid_radius = 1
|
||||
aid_vec1 = [1, 0, 0]
|
||||
aid_vec2 = [0, 1, 0]
|
||||
uavs_avoid_vel = [Vector3()] * uav_num
|
||||
uavs_avoid_accel = [Vector3()] * uav_num
|
||||
|
||||
def pose_callback(msg, id):
|
||||
pose[id] = msg
|
||||
|
|
@ -22,7 +22,7 @@ def pose_callback(msg, id):
|
|||
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_vel_pub[i] = rospy.Publisher("/xtdrone/uav"+str(i+1)+"/avoid_vel", Vector3,queue_size=10)
|
||||
avoid_accel_pub[i] = rospy.Publisher("/xtdrone/uav"+str(i+1)+"/avoid_accel", Vector3,queue_size=10)
|
||||
|
||||
time.sleep(1)
|
||||
rate = rospy.Rate(50)
|
||||
|
|
@ -36,14 +36,14 @@ while not rospy.is_shutdown():
|
|||
cos1 = dir_vec.dot(aid_vec1)/(numpy.linalg.norm(dir_vec) * numpy.linalg.norm(aid_vec1))
|
||||
cos2 = dir_vec.dot(aid_vec2)/(numpy.linalg.norm(dir_vec) * numpy.linalg.norm(aid_vec2))
|
||||
if abs(cos1) < abs(cos2):
|
||||
avoid_vel = avoid_kp * numpy.cross(dir_vec, aid_vec1)/numpy.linalg.norm(numpy.cross(dir_vec, aid_vec1))
|
||||
avoid_accel = avoid_kp * numpy.cross(dir_vec, aid_vec1)/numpy.linalg.norm(numpy.cross(dir_vec, aid_vec1))
|
||||
else:
|
||||
avoid_vel = avoid_kp * numpy.cross(dir_vec, aid_vec2)/numpy.linalg.norm(numpy.cross(dir_vec, aid_vec2))
|
||||
uavs_avoid_vel[i] = Vector3(uavs_avoid_vel[i].x+avoid_vel[0],uavs_avoid_vel[i].y+avoid_vel[1],uavs_avoid_vel[i].z+avoid_vel[2])
|
||||
uavs_avoid_vel[i+j] = Vector3(uavs_avoid_vel[i+j].x-avoid_vel[0],uavs_avoid_vel[i+j].y-avoid_vel[1],uavs_avoid_vel[i+j].z-avoid_vel[2])
|
||||
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):
|
||||
avoid_vel_pub[i].publish(uavs_avoid_vel[i])
|
||||
uavs_avoid_vel = [Vector3()] * uav_num
|
||||
avoid_accel_pub[i].publish(uavs_avoid_accel[i])
|
||||
uavs_avoid_accel = [Vector3()] * uav_num
|
||||
rate.sleep()
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@ 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
|
||||
from formation_dict import formation_dict_6
|
||||
import time
|
||||
import math
|
||||
import numpy
|
||||
|
|
@ -15,37 +15,39 @@ import copy
|
|||
class Follower:
|
||||
|
||||
def __init__(self, uav_id, uav_num):
|
||||
self.hover = True
|
||||
self.hover = "HOVER"
|
||||
self.offboard = "OFFBOARD"
|
||||
self.id = uav_id
|
||||
self.uav_num = uav_num
|
||||
self.local_pose = PoseStamped()
|
||||
self.local_velocity = TwistStamped()
|
||||
self.cmd_vel_enu = Twist()
|
||||
self.cmd_accel_enu = Vector3()
|
||||
self.avoid_vel = Vector3()
|
||||
self.avoid_accel = Vector3()
|
||||
self.following_switch = False
|
||||
self.arrive_print = True
|
||||
self.following_ids = []
|
||||
self.formation_config = 'waiting'
|
||||
self.following_count = 0
|
||||
self.Kp = 100 #100
|
||||
self.kr = (4/int((self.uav_num-1)/3))**0.5
|
||||
#self.kr = 1
|
||||
self.f = 500
|
||||
#self.f=1/0.01
|
||||
self.velxy_max = 0.5 #0.5
|
||||
self.Kp = 100 #100
|
||||
#self.kr = (4/int((self.uav_num-1)/2))**0.5
|
||||
self.kr = 1
|
||||
self.f = 100
|
||||
self.velxy_max = 1
|
||||
self.velz_max = 1
|
||||
self.following_local_pose = [PoseStamped()]*self.uav_num
|
||||
self.following_local_pose = [PoseStamped() for i in range (self.uav_num)]
|
||||
self.following_local_pose_sub = [None]*self.uav_num
|
||||
self.following_local_velocity = [TwistStamped()]*self.uav_num
|
||||
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.cmd_vel_sub = rospy.Subscriber("/xtdrone/follower/cmd_vel", Twist, self.cmd_vel_callback)
|
||||
self.avoid_vel_sub = rospy.Subscriber("/xtdrone/uav"+str(self.id)+"/avoid_vel", Vector3, self.avoid_vel_callback)
|
||||
self.avoid_accel_sub = rospy.Subscriber("/xtdrone/uav"+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)
|
||||
|
||||
|
||||
def local_pose_callback(self, msg):
|
||||
|
|
@ -66,70 +68,69 @@ class Follower:
|
|||
|
||||
def cmd_vel_callback(self, msg):
|
||||
self.cmd_vel_enu = msg
|
||||
if msg.linear.z == 0:
|
||||
self.hover = True
|
||||
else:
|
||||
self.hover = False
|
||||
|
||||
def formation_switch_callback(self, msg):
|
||||
if not self.formation_config == msg.data:
|
||||
self.following_switch = True
|
||||
else:
|
||||
self.following_switch = False
|
||||
self.formation_config = msg.data
|
||||
|
||||
|
||||
def avoid_vel_callback(self, msg):
|
||||
self.avoid_vel = msg
|
||||
def avoid_accel_callback(self, msg):
|
||||
self.avoid_accel = msg
|
||||
|
||||
def loop(self):
|
||||
rospy.init_node('follower'+str(self.id-1))
|
||||
rate = rospy.Rate(self.f)
|
||||
while not rospy.is_shutdown():
|
||||
if self.arrive_count > 100:
|
||||
self.info_pub.publish("Arrived")
|
||||
#print("Arrived")
|
||||
if self.arrive_count > 100 and self.arrive_print:
|
||||
print("Follower"+str(self.id-1)+":Arrived")
|
||||
self.arrive_print = False
|
||||
if self.following_switch:
|
||||
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])
|
||||
#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)
|
||||
#if self.id == 2:
|
||||
#print(self.following_ids)
|
||||
self.following_count = 0
|
||||
for i in range(self.uav_num):
|
||||
if not self.following_local_pose_sub[i] == None:
|
||||
self.following_local_pose_sub[i].unregister()
|
||||
if not self.following_local_velocity_sub[i] == None:
|
||||
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_count += 1
|
||||
time.sleep(1)
|
||||
self.arrive_print = True
|
||||
self.arrive_count = 0
|
||||
for i in range(self.f/10):
|
||||
self.cmd_pub.publish(self.offboard)
|
||||
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_6[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)
|
||||
#if self.id == 2:
|
||||
#print(self.following_ids)
|
||||
self.following_count = 0
|
||||
for i in range(self.uav_num):
|
||||
if not self.following_local_pose_sub[i] == None:
|
||||
self.following_local_pose_sub[i].unregister()
|
||||
if not self.following_local_velocity_sub[i] == None:
|
||||
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_count += 1
|
||||
|
||||
|
||||
self.cmd_accel_enu = Vector3(0, 0, 0)
|
||||
#self.cmd_vel_enu.linear = copy.deepcopy(self.avoid_vel)
|
||||
#self.cmd_vel_enu.linear = copy.deepcopy(self.avoid_accel)
|
||||
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_6[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_6[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_6[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_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
|
||||
|
||||
self.cmd_accel_enu.x -= formation_dict_6[self.formation_config][0, following_id[0]-1]
|
||||
self.cmd_accel_enu.y -= formation_dict_6[self.formation_config][1, following_id[0]-1]
|
||||
self.cmd_accel_enu.z -= formation_dict_6[self.formation_config][2, following_id[0]-1]
|
||||
if self.arrive_count <= 100:
|
||||
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)
|
||||
self.cmd_vel_enu.linear.z = self.local_velocity.twist.linear.z + self.Kp * (self.avoid_accel.z + self.cmd_accel_enu.z / self.f)
|
||||
else:
|
||||
self.cmd_pub.publish(self.hover)
|
||||
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:
|
||||
|
|
@ -152,41 +153,45 @@ class Follower:
|
|||
rate.sleep()
|
||||
|
||||
#函数输入为相对leader的位置矩阵和无人机数量，输出为L矩阵
|
||||
def get_L_matrix(self, rel_posi):
|
||||
|
||||
#假设无论多少UAV，都假设尽可能3层通信（叶子节点除外）
|
||||
c_num=int((self.uav_num-1)/3)
|
||||
def get_L_matrix(self,rel_posi):
|
||||
|
||||
c_num=int((self.uav_num-1)/2)
|
||||
min_num_index_list = [0]*c_num
|
||||
|
||||
comm=[[]for i in range (self.uav_num)]
|
||||
w=numpy.ones((self.uav_num,self.uav_num))*0 # 定义邻接矩阵
|
||||
w=numpy.ones((self.uav_num,self.uav_num))*0
|
||||
nodes_next=[]
|
||||
node_flag = [self.uav_num-1]
|
||||
node_mid_flag=[]
|
||||
|
||||
rel_d=[0]*(self.uav_num-1)
|
||||
# 规定每个无人机可以随机连接三台距离自己最近的无人机,且不能连接在flag中的无人机(即已经判断过连接点的无人机)。
|
||||
# 计算第一层通信（leader）：获得离自己最近的三台无人机编号
|
||||
|
||||
|
||||
for i in range(0,self.uav_num-1):
|
||||
|
||||
rel_d[i]=pow(rel_posi[0][i],2)+pow(rel_posi[1][i],2)+pow(rel_posi[2][i],2)
|
||||
|
||||
min_num_index_list = map(rel_d.index, heapq.nsmallest(c_num, rel_d))
|
||||
min_num_index_list=list(min_num_index_list)
|
||||
c=numpy.copy(rel_d)
|
||||
c.sort()
|
||||
count=0
|
||||
|
||||
#leader 连接的无人机编号：
|
||||
for j in range(0,c_num):
|
||||
for i in range(0,self.uav_num-1):
|
||||
if rel_d[i]==c[j]:
|
||||
if not i in node_mid_flag:
|
||||
min_num_index_list[count]=i
|
||||
node_mid_flag.append(i)
|
||||
count=count+1
|
||||
if count==c_num:
|
||||
break
|
||||
if count==c_num:
|
||||
break
|
||||
|
||||
if min_num_index_list[j] in node_flag:
|
||||
|
||||
nodes_next=nodes_next
|
||||
|
||||
else:
|
||||
if min_num_index_list[j] in nodes_next:
|
||||
nodes_next=nodes_next
|
||||
else:
|
||||
nodes_next.append(min_num_index_list[j])
|
||||
for j in range(0,c_num):
|
||||
nodes_next.append(min_num_index_list[j])
|
||||
|
||||
comm[self.uav_num-1].append(min_num_index_list[j])
|
||||
comm[self.uav_num-1].append(min_num_index_list[j])
|
||||
|
||||
size_=len(node_flag)
|
||||
|
||||
|
|
@ -194,19 +199,33 @@ class Follower:
|
|||
|
||||
next_node= nodes_next[0]
|
||||
nodes_next=nodes_next[1:]
|
||||
|
||||
min_num_index_list = [0]*c_num
|
||||
node_mid_flag=[]
|
||||
rel_d=[0]*(self.uav_num-1)
|
||||
for i in range(0,self.uav_num-1):
|
||||
|
||||
if i==next_node or i in node_flag:
|
||||
|
||||
rel_d[i]=2000 #这个2000是根据相对位置和整个地图的大小决定的，要比最大可能相对距离大
|
||||
rel_d[i]=2000
|
||||
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)
|
||||
|
||||
min_num_index_list = map(rel_d.index, heapq.nsmallest(c_num, rel_d))
|
||||
min_num_index_list=list(min_num_index_list)
|
||||
c=numpy.copy(rel_d)
|
||||
c.sort()
|
||||
count=0
|
||||
|
||||
for j in range(0,c_num):
|
||||
for i in range(0,self.uav_num-1):
|
||||
if rel_d[i]==c[j]:
|
||||
if not i in node_mid_flag:
|
||||
min_num_index_list[count]=i
|
||||
node_mid_flag.append(i)
|
||||
count=count+1
|
||||
if count==c_num:
|
||||
break
|
||||
if count==c_num:
|
||||
break
|
||||
node_flag.append(next_node)
|
||||
|
||||
size_=len(node_flag)
|
||||
|
|
@ -224,9 +243,6 @@ class Follower:
|
|||
nodes_next.append(min_num_index_list[j])
|
||||
|
||||
comm[next_node].append(min_num_index_list[j])
|
||||
# comm为每个uav连接其他uav的编号，其中数组的最后一行为leader
|
||||
#leader是拉普拉斯矩阵的第一行，为0
|
||||
#第0号飞机（相对位置矩阵中的第一个位置）为第二行，以此类推
|
||||
|
||||
for i in range (0,self.uav_num):
|
||||
for j in range(0,self.uav_num-1):
|
||||
|
|
@ -242,7 +258,7 @@ class Follower:
|
|||
else:
|
||||
w[j+1][i]=0
|
||||
|
||||
L=w #定义拉普拉斯矩阵
|
||||
L=w
|
||||
for i in range (0,self.uav_num):
|
||||
|
||||
L[i][i]=-sum(w[i])
|
||||
|
|
@ -342,4 +358,4 @@ if __name__ == '__main__':
|
|||
follower = Follower(int(sys.argv[1]),int(sys.argv[2]))
|
||||
#follower = Follower(int(sys.argv[1]),9)
|
||||
#follower = Follower(9,9)
|
||||
follower.loop()
|
||||
follower.loop()
|
||||
|
|
@ -0,0 +1,341 @@
|
|||
#!/usr/bin/python
|
||||
# -*- coding: UTF-8 -*-
|
||||
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_6
|
||||
import time
|
||||
import math
|
||||
import numpy
|
||||
import sys
|
||||
import heapq
|
||||
import copy
|
||||
|
||||
class Follower:
|
||||
|
||||
def __init__(self, uav_id, uav_num):
|
||||
self.hover = True
|
||||
self.id = uav_id
|
||||
self.uav_num = uav_num
|
||||
self.local_pose = PoseStamped()
|
||||
self.local_velocity = TwistStamped()
|
||||
self.cmd_vel_enu = Twist()
|
||||
self.cmd_accel_enu = Vector3()
|
||||
self.avoid_vel = Vector3()
|
||||
self.following_switch = False
|
||||
self.following_ids = []
|
||||
self.formation_config = 'waiting'
|
||||
self.following_count = 0
|
||||
self.Kp = 20
|
||||
#self.kr = (4/int((self.uav_num-1)/2))**0.5
|
||||
self.kr = 0.1
|
||||
self.f = 100
|
||||
self.first_x=True
|
||||
self.first_y=True
|
||||
self.first_z=True
|
||||
self.following_local_pose = [PoseStamped() for i in range (self.uav_num)]
|
||||
self.following_local_pose_sub = [None]*self.uav_num
|
||||
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.cmd_vel_sub = rospy.Subscriber("/xtdrone/follower/cmd_vel", Twist, self.cmd_vel_callback)
|
||||
#self.avoid_vel_sub = rospy.Subscriber("/xtdrone/uav"+str(self.id)+"/avoid_vel", Vector3, self.avoid_vel_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.accel_enu_pub = rospy.Publisher('/xtdrone/uav'+str(self.id)+'/cmd_accel_enu', Vector3, 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
|
||||
|
||||
|
||||
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
|
||||
if msg.linear.z == 0:
|
||||
self.hover = True
|
||||
else:
|
||||
self.hover = False
|
||||
|
||||
def formation_switch_callback(self, msg):
|
||||
if not self.formation_config == msg.data:
|
||||
self.following_switch = True
|
||||
self.formation_config = msg.data
|
||||
|
||||
|
||||
#def avoid_vel_callback(self, msg):
|
||||
#self.avoid_vel = msg
|
||||
|
||||
def loop(self):
|
||||
rospy.init_node('follower'+str(self.id-1))
|
||||
rate = rospy.Rate(self.f)
|
||||
while not rospy.is_shutdown():
|
||||
if self.arrive_count > 100 and self.arrive_print:
|
||||
print("Follower"+str(self.id-1)+":Arrived")
|
||||
self.arrive_print = False
|
||||
if self.following_switch:
|
||||
self.following_switch = False
|
||||
for i in range(self.f/10):
|
||||
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_6[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)
|
||||
#if self.id == 2:
|
||||
#print(self.following_ids)
|
||||
self.following_count = 0
|
||||
for i in range(self.uav_num):
|
||||
if not self.following_local_pose_sub[i] == None:
|
||||
self.following_local_pose_sub[i].unregister()
|
||||
if not self.following_local_velocity_sub[i] == None:
|
||||
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_count += 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_6[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_6[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_6[self.formation_config][2, self.id-2]
|
||||
|
||||
if not following_id[0] == 0:
|
||||
self.cmd_accel_enu.x -= formation_dict_6[self.formation_config][0, following_id[0]-1]
|
||||
self.cmd_accel_enu.y -= formation_dict_6[self.formation_config][1, following_id[0]-1]
|
||||
self.cmd_accel_enu.z -= formation_dict_6[self.formation_config][2, following_id[0]-1]
|
||||
|
||||
if self.arrive_count > 100:
|
||||
self.cmd_accel_enu = Vector3(0,0,0)
|
||||
if not self.formation_config == 'waiting':
|
||||
self.accel_enu_pub.publish(self.cmd_accel_enu)
|
||||
rate.sleep()
|
||||
|
||||
|
||||
#函数输入为相对leader的位置矩阵和无人机数量，输出为L矩阵
|
||||
def get_L_matrix(self,rel_posi):
|
||||
|
||||
c_num=int((self.uav_num-1)/2)
|
||||
min_num_index_list = [0]*c_num
|
||||
|
||||
comm=[[]for i in range (self.uav_num)]
|
||||
w=numpy.ones((self.uav_num,self.uav_num))*0
|
||||
nodes_next=[]
|
||||
node_flag = [self.uav_num-1]
|
||||
node_mid_flag=[]
|
||||
|
||||
rel_d=[0]*(self.uav_num-1)
|
||||
|
||||
|
||||
for i in range(0,self.uav_num-1):
|
||||
|
||||
rel_d[i]=pow(rel_posi[0][i],2)+pow(rel_posi[1][i],2)+pow(rel_posi[2][i],2)
|
||||
|
||||
c=numpy.copy(rel_d)
|
||||
c.sort()
|
||||
count=0
|
||||
|
||||
for j in range(0,c_num):
|
||||
for i in range(0,self.uav_num-1):
|
||||
if rel_d[i]==c[j]:
|
||||
if not i in node_mid_flag:
|
||||
min_num_index_list[count]=i
|
||||
node_mid_flag.append(i)
|
||||
count=count+1
|
||||
if count==c_num:
|
||||
break
|
||||
if count==c_num:
|
||||
break
|
||||
|
||||
|
||||
for j in range(0,c_num):
|
||||
nodes_next.append(min_num_index_list[j])
|
||||
|
||||
comm[self.uav_num-1].append(min_num_index_list[j])
|
||||
|
||||
size_=len(node_flag)
|
||||
|
||||
while (nodes_next!=[]) and (size_<(self.uav_num-1)):
|
||||
|
||||
next_node= nodes_next[0]
|
||||
nodes_next=nodes_next[1:]
|
||||
min_num_index_list = [0]*c_num
|
||||
node_mid_flag=[]
|
||||
rel_d=[0]*(self.uav_num-1)
|
||||
for i in range(0,self.uav_num-1):
|
||||
|
||||
if i==next_node or i in node_flag:
|
||||
|
||||
rel_d[i]=2000
|
||||
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)
|
||||
|
||||
c=numpy.copy(rel_d)
|
||||
c.sort()
|
||||
count=0
|
||||
|
||||
for j in range(0,c_num):
|
||||
for i in range(0,self.uav_num-1):
|
||||
if rel_d[i]==c[j]:
|
||||
if not i in node_mid_flag:
|
||||
min_num_index_list[count]=i
|
||||
node_mid_flag.append(i)
|
||||
count=count+1
|
||||
if count==c_num:
|
||||
break
|
||||
if count==c_num:
|
||||
break
|
||||
node_flag.append(next_node)
|
||||
|
||||
size_=len(node_flag)
|
||||
|
||||
for j in range(0,c_num):
|
||||
|
||||
if min_num_index_list[j] in node_flag:
|
||||
|
||||
nodes_next=nodes_next
|
||||
|
||||
else:
|
||||
if min_num_index_list[j] in nodes_next:
|
||||
nodes_next=nodes_next
|
||||
else:
|
||||
nodes_next.append(min_num_index_list[j])
|
||||
|
||||
comm[next_node].append(min_num_index_list[j])
|
||||
|
||||
for i in range (0,self.uav_num):
|
||||
for j in range(0,self.uav_num-1):
|
||||
|
||||
if i==0:
|
||||
if j in comm[self.uav_num-1]:
|
||||
w[j+1][i]=1
|
||||
else:
|
||||
w[j+1][i]=0
|
||||
else:
|
||||
if j in comm[i-1]:
|
||||
w[j+1][i]=1
|
||||
else:
|
||||
w[j+1][i]=0
|
||||
|
||||
L=w
|
||||
for i in range (0,self.uav_num):
|
||||
|
||||
L[i][i]=-sum(w[i])
|
||||
|
||||
return L
|
||||
'''
|
||||
def get_L_matrix(self, rel_posi):
|
||||
|
||||
#假设无论多少UAV，都假设尽可能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 # 定义邻接矩阵
|
||||
nodes_next=[]
|
||||
node_flag = [self.uav_num-1]
|
||||
|
||||
rel_d=[0]*(self.uav_num-1)
|
||||
# 规定每个无人机可以随机连接三台距离自己最近的无人机,且不能连接在flag中的无人机(即已经判断过连接点的无人机)。
|
||||
# 计算第一层通信（leader）：获得离自己最近的三台无人机编号
|
||||
|
||||
for i in range(0,self.uav_num-1):
|
||||
|
||||
rel_d[i]=pow(rel_posi[0][i],2)+pow(rel_posi[1][i],2)+pow(rel_posi[2][i],2)
|
||||
|
||||
min_num_index_list = map(rel_d.index, heapq.nsmallest(c_num, rel_d))
|
||||
min_num_index_list=list(min_num_index_list)
|
||||
#leader 连接的无人机编号：
|
||||
comm[self.uav_num-1]=min_num_index_list
|
||||
|
||||
nodes_next.extend(comm[self.uav_num-1])
|
||||
|
||||
size_=len(node_flag)
|
||||
|
||||
while (nodes_next!=[]) and (size_<(self.uav_num-1)):
|
||||
|
||||
next_node= nodes_next[0]
|
||||
nodes_next=nodes_next[1:]
|
||||
|
||||
rel_d=[0]*(self.uav_num-1)
|
||||
for i in range(0,self.uav_num-1):
|
||||
|
||||
if i==next_node or i in node_flag:
|
||||
|
||||
rel_d[i]=2000 #这个2000是根据相对位置和整个地图的大小决定的，要比最大可能相对距离大
|
||||
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)
|
||||
|
||||
min_num_index_list = map(rel_d.index, heapq.nsmallest(c_num, rel_d))
|
||||
min_num_index_list=list(min_num_index_list)
|
||||
node_flag.append(next_node)
|
||||
|
||||
size_=len(node_flag)
|
||||
|
||||
for j in range(0,c_num):
|
||||
|
||||
if min_num_index_list[j] in node_flag:
|
||||
|
||||
nodes_next=nodes_next
|
||||
|
||||
else:
|
||||
if min_num_index_list[j] in nodes_next:
|
||||
nodes_next=nodes_next
|
||||
else:
|
||||
nodes_next.append(min_num_index_list[j])
|
||||
|
||||
comm[next_node].append(min_num_index_list[j])
|
||||
# comm为每个uav连接其他uav的编号，其中数组的最后一行为leader
|
||||
#print (comm)
|
||||
#leader是拉普拉斯矩阵的第一行，为0
|
||||
#第0号飞机（相对位置矩阵中的第一个位置）为第二行，以此类推
|
||||
|
||||
for i in range (0,self.uav_num):
|
||||
for j in range(0,self.uav_num-1):
|
||||
|
||||
if i==0:
|
||||
if j in comm[self.uav_num-1]:
|
||||
w[j+1][i]=1
|
||||
else:
|
||||
w[j+1][i]=0
|
||||
else:
|
||||
if j in comm[i-1]:
|
||||
w[j+1][i]=1
|
||||
else:
|
||||
w[j+1][i]=0
|
||||
|
||||
L=w #定义拉普拉斯矩阵
|
||||
for i in range (0,self.uav_num):
|
||||
|
||||
L[i][i]=-sum(w[i])
|
||||
|
||||
#print (L)
|
||||
return L
|
||||
'''
|
||||
|
||||
if __name__ == '__main__':
|
||||
follower = Follower(int(sys.argv[1]),int(sys.argv[2]))
|
||||
#follower = Follower(int(sys.argv[1]),9)
|
||||
#follower = Follower(9,9)
|
||||
follower.loop()
|
||||
|
|
@ -1,11 +1,19 @@
|
|||
import numpy as np
|
||||
|
||||
formation_dict_6 = {"T":np.array([[2,0,2],[-2,0,2],[0,0,2],[0,0,-2],[0,0,-4]]) , "diamond": np.array([[2,2,-2],[2,-2,-2],[-2,-2,-2],[-2,2,-2],[0,0,-4]]), "triangle": np.array([[-3,0,0],[3,0,0],[-1.5,0,1.5],[1.5,0,1.5],[0,0,3]]),"waiting":np.zeros([3,5])}
|
||||
formation_dict_6["T"] = np.transpose(formation_dict_6["T"])
|
||||
formation_dict_6["diamond"] = np.transpose(formation_dict_6["diamond"])
|
||||
formation_dict_6["triangle"] = np.transpose(formation_dict_6["triangle"])
|
||||
formation_dict_6["T"] = np.transpose(formation_dict_6["T"]*1)
|
||||
formation_dict_6["diamond"] = np.transpose(formation_dict_6["diamond"]*1)
|
||||
formation_dict_6["triangle"] = np.transpose(formation_dict_6["triangle"]*1)
|
||||
|
||||
formation_dict_9 = {"cube": np.array([[2,2,2],[-2,2,2],[-2,-2,2],[2,-2,2],[2,2,-2],[-2,2,-2],[-2,-2,-2],[2,-2,-2]]), "pyramid": np.array([[2,2,-2],[-2,2,-2],[-2,-2,-2],[2,-2,-2],[4,4,-4],[-4,4,-4],[-4,-4,-4],[4,-4,-4]]), "triangle": np.array([[0,4,-4],[0,0,-2],[0,0,-4],[0,-2,-2],[0,2,-4],[0,2,-2],[0,-4,-4],[0,-2,-4]]),"waiting":np.zeros([3,8])}
|
||||
formation_dict_9["cube"] = np.transpose(formation_dict_9["cube"])
|
||||
formation_dict_9["pyramid"] = np.transpose(formation_dict_9["pyramid"])
|
||||
formation_dict_9["triangle"] = np.transpose(formation_dict_9["triangle"])
|
||||
formation_dict_9["cube"] = np.transpose(formation_dict_9["cube"]*1)
|
||||
formation_dict_9["pyramid"] = np.transpose(formation_dict_9["pyramid"]*1)
|
||||
formation_dict_9["triangle"] = np.transpose(formation_dict_9["triangle"]*1)
|
||||
|
||||
formation_dict_18 = {"cube":np.array([[0, 2, 0], [2, 0, 0], [2, 2, 0], [4, 0, 0], [4, 2, 0], [0, 0, 2], [0, 2, 2], [2, 0, 2], [2, 2, 2], [4, 0, 2], [4, 2, 2], [0, 0, 4], [0, 2, 4], [2, 0, 4], [2, 2, 4], [4, 0, 4], [4, 2, 4]]
|
||||
) , "sphere": np.array([[0.5857864376269, -1.4142135623731, 0.0], [2.0, -2.0, 0.0], [3.4142135623731003, -1.4142135623731, 0.0], [4.0, 0.0, 0.0], [3.4142135623731003, 1.4142135623731, 0.0], [2.0, 2.0, 0.0], [0.5857864376269, 1.4142135623731, 0.0], [1.0, -1.0, -1.4142135623731], [3.0, -1.0, -1.4142135623731], [3.0, 1.0, -1.4142135623731], [1.0, 1.0, -1.4142135623731], [1.0, -1.0, 1.4142135623731], [3.0, -1.0, 1.4142135623731], [3.0, 1.0, 1.4142135623731], [1.0, 1.0, 1.4142135623731], [2.0, 0.0, 2.0], [2.0, 0.0, -2.0]]
|
||||
), "diamond": np.array([[2.0, 0.0, 0.0], [4.0, 0.0, 0.0], [4.0, 2.0, 0.0], [4.0, 4.0, 0.0], [2.0, 4.0, 0.0], [0.0, 4.0, 0.0], [0.0, 2.0, 0.0], [1.0, 1.0, 1.0], [3.0, 1.0, 1.0], [3.0, 3.0, 1.0], [1.0, 3.0, 1.0], [1.0, 1.0, -1.0], [3.0, 1.0, -1.0], [3.0, 3.0, -1.0], [1.0, 3.0, -1.0], [2.0, 2.0, 2.0], [2.0, 2.0, -2.0]]
|
||||
),"waiting":np.zeros([3,5])}
|
||||
formation_dict_18["cube"] = np.transpose(formation_dict_18["cube"])
|
||||
formation_dict_18["sphere"] = np.transpose(formation_dict_18["sphere"])
|
||||
formation_dict_18["diamond"] = np.transpose(formation_dict_18["diamond"])
|
||||
Binary file not shown.
|
|
@ -0,0 +1,195 @@
|
|||
import rospy
|
||||
from geometry_msgs.msg import Twist
|
||||
import sys, select, os
|
||||
import tty, termios
|
||||
from std_msgs.msg import String
|
||||
|
||||
MAX_LIN_accel = 1
|
||||
MAX_ANG_accel = 0.1
|
||||
LIN_accel_STEP_SIZE = 0.02
|
||||
ANG_accel_STEP_SIZE = 0.01
|
||||
|
||||
msg = """
|
||||
Control Your XTDrone!
|
||||
---------------------------
|
||||
1 2 3 4 5 6 7 8 9 0
|
||||
w r t y i
|
||||
a s d j k l
|
||||
x v b n ,
|
||||
|
||||
w/x : increase/decrease forward accelocity (-1~1)
|
||||
a/d : increase/decrease leftward accelocity (-1~1)
|
||||
i/, : increase/decrease upward accelocity (-1~1)
|
||||
j/l : increase/decrease angular accelocity (-0.1~0.1)
|
||||
r : return home
|
||||
t/y : arm/disarm
|
||||
v/n : takeoff(disenabled now)/land
|
||||
b : offboard
|
||||
s or k : hover
|
||||
0~9 : extendable mission
|
||||
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 accels(target_forward_accel, target_leftward_accel, target_upward_accel,target_angular_accel):
|
||||
return "currently:\t forward x %s\t leftward y %s\t upward z %s\t angular %s " % (target_forward_accel, target_leftward_accel, target_upward_accel, target_angular_accel)
|
||||
|
||||
def makeSimpleProfile(output, input, slop):
|
||||
if input > output:
|
||||
output = min( input, output + slop )
|
||||
elif input < output:
|
||||
output = max( input, output - slop )
|
||||
else:
|
||||
output = input
|
||||
return output
|
||||
|
||||
def constrain(input, low, high):
|
||||
if input < low:
|
||||
input = low
|
||||
elif input > high:
|
||||
input = high
|
||||
else:
|
||||
input = input
|
||||
|
||||
return input
|
||||
|
||||
def checkLinearLimitaccelocity(accel):
|
||||
accel = constrain(accel, -MAX_LIN_accel, MAX_LIN_accel)
|
||||
return accel
|
||||
|
||||
def checkAngularLimitaccelocity(accel):
|
||||
accel = constrain(accel, -MAX_ANG_accel, MAX_ANG_accel)
|
||||
return accel
|
||||
|
||||
if __name__=="__main__":
|
||||
settings = termios.tcgetattr(sys.stdin)
|
||||
|
||||
rospy.init_node('keyboard_control')
|
||||
cmd_accel_flu_pub = rospy.Publisher('/xtdrone/cmd_accel_flu', Twist, queue_size=10)
|
||||
cmd_pub = rospy.Publisher('/xtdrone/cmd',String,queue_size=10)
|
||||
cmd = String()
|
||||
|
||||
target_forward_accel = 0.0
|
||||
target_leftward_accel = 0.0
|
||||
target_upward_accel = 0.0
|
||||
target_angular_accel = 0.0
|
||||
control_forward_accel = 0.0
|
||||
control_leftward_accel = 0.0
|
||||
control_upward_accel = 0.0
|
||||
control_angular_accel = 0.0
|
||||
|
||||
count = 0
|
||||
|
||||
try:
|
||||
print(msg)
|
||||
while(1):
|
||||
key = getKey()
|
||||
if key == 'w' :
|
||||
target_forward_accel = checkLinearLimitaccelocity(target_forward_accel + LIN_accel_STEP_SIZE)
|
||||
print(msg)
|
||||
print(accels(target_forward_accel,target_leftward_accel,target_upward_accel,target_angular_accel))
|
||||
elif key == 'x' :
|
||||
target_forward_accel = checkLinearLimitaccelocity(target_forward_accel - LIN_accel_STEP_SIZE)
|
||||
print(msg)
|
||||
print(accels(target_forward_accel,target_leftward_accel,target_upward_accel,target_angular_accel))
|
||||
elif key == 'a' :
|
||||
target_leftward_accel = checkLinearLimitaccelocity(target_leftward_accel + LIN_accel_STEP_SIZE)
|
||||
print(msg)
|
||||
print(accels(target_forward_accel,target_leftward_accel,target_upward_accel,target_angular_accel))
|
||||
elif key == 'd' :
|
||||
target_leftward_accel = checkLinearLimitaccelocity(target_leftward_accel - LIN_accel_STEP_SIZE)
|
||||
print(msg)
|
||||
print(accels(target_forward_accel,target_leftward_accel,target_upward_accel,target_angular_accel))
|
||||
elif key == 'i' :
|
||||
target_upward_accel = checkLinearLimitaccelocity(target_upward_accel + LIN_accel_STEP_SIZE)
|
||||
print(msg)
|
||||
print(accels(target_forward_accel,target_leftward_accel,target_upward_accel,target_angular_accel))
|
||||
elif key == ',' :
|
||||
target_upward_accel = checkLinearLimitaccelocity(target_upward_accel - LIN_accel_STEP_SIZE)
|
||||
print(msg)
|
||||
print(accels(target_forward_accel,target_leftward_accel,target_upward_accel,target_angular_accel))
|
||||
elif key == 'j':
|
||||
target_angular_accel = checkAngularLimitaccelocity(target_angular_accel + ANG_accel_STEP_SIZE)
|
||||
print(msg)
|
||||
print(accels(target_forward_accel,target_leftward_accel,target_upward_accel,target_angular_accel))
|
||||
elif key == 'l':
|
||||
target_angular_accel = checkAngularLimitaccelocity(target_angular_accel - ANG_accel_STEP_SIZE)
|
||||
print(msg)
|
||||
print(accels(target_forward_accel,target_leftward_accel,target_upward_accel,target_angular_accel))
|
||||
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 == 's' or key == 'k' :
|
||||
target_forward_accel = 0.0
|
||||
target_leftward_accel = 0.0
|
||||
target_upward_accel = 0.0
|
||||
target_angular_accel = 0.0
|
||||
control_forward_accel = 0.0
|
||||
control_leftward_accel = 0.0
|
||||
control_upward_accel = 0.0
|
||||
control_angular_accel = 0.0
|
||||
print(msg)
|
||||
print(accels(target_forward_accel,-target_leftward_accel,target_upward_accel,target_angular_accel))
|
||||
else:
|
||||
if (key == '\x03'):
|
||||
break
|
||||
|
||||
twist = Twist()
|
||||
|
||||
control_forward_accel = makeSimpleProfile(control_forward_accel, target_forward_accel, (LIN_accel_STEP_SIZE/2.0))
|
||||
control_leftward_accel = makeSimpleProfile(control_leftward_accel, target_leftward_accel, (LIN_accel_STEP_SIZE/2.0))
|
||||
control_upward_accel = makeSimpleProfile(control_upward_accel, target_upward_accel, (LIN_accel_STEP_SIZE/2.0))
|
||||
twist.linear.x = control_forward_accel; twist.linear.y = control_leftward_accel; twist.linear.z = control_upward_accel
|
||||
|
||||
control_angular_accel = makeSimpleProfile(control_angular_accel, target_angular_accel, (ANG_accel_STEP_SIZE/2.0))
|
||||
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = control_angular_accel
|
||||
|
||||
cmd_accel_flu_pub.publish(twist)
|
||||
cmd_pub.publish(cmd)
|
||||
cmd = ''
|
||||
except:
|
||||
print(e)
|
||||
|
||||
finally:
|
||||
twist = Twist()
|
||||
twist.linear.x = 0.0; twist.linear.y = 0.0; twist.linear.z = 0.0
|
||||
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = 0.0
|
||||
cmd_accel_flu_pub.publish(twist)
|
||||
cmd_pub.publish(cmd)
|
||||
|
||||
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
|
||||
|
|
@ -205,10 +205,10 @@ if __name__=="__main__":
|
|||
ctrl_leader = not ctrl_leader
|
||||
print_msg()
|
||||
elif key == 'k':
|
||||
cmd = 'AUTO.LOITER'
|
||||
cmd = 'HOVER'
|
||||
cmd_vel_mask = False
|
||||
print_msg()
|
||||
print('Loiter')
|
||||
print('Hover')
|
||||
elif key == 's' :
|
||||
cmd_vel_mask = False
|
||||
target_forward_vel = 0.0
|
||||
|
|
|
|||
|
|
@ -0,0 +1,273 @@
|
|||
import rospy
|
||||
from geometry_msgs.msg import Twist, Vector3
|
||||
import sys, select, os
|
||||
import tty, termios
|
||||
from std_msgs.msg import String
|
||||
|
||||
|
||||
MAX_LIN_VEL = 1
|
||||
MAX_ANG_VEL = 0.1
|
||||
LIN_VEL_STEP_SIZE = 0.02
|
||||
ANG_VEL_STEP_SIZE = 0.01
|
||||
|
||||
cmd_vel_mask = False
|
||||
ctrl_leader = False
|
||||
uav_num = int(sys.argv[1])
|
||||
if uav_num == 9:
|
||||
formation_configs = ['waiting', 'cube', 'pyramid', 'triangle']
|
||||
if uav_num == 6:
|
||||
formation_configs = ['waiting', 'T', 'diamond', 'triangle']
|
||||
|
||||
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 (-1~1)
|
||||
a/d : increase/decrease leftward velocity (-1~1)
|
||||
i/, : increase/decrease upward velocity (-1~1)
|
||||
j/l : increase/decrease angular velocity (-0.1~0.1)
|
||||
r : return home
|
||||
t/y : arm/disarm
|
||||
v/n : takeoff/land
|
||||
b : offboard
|
||||
s : hover(offboard mode) and remove the mask of keyboard control
|
||||
k : hover(hover mode) 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 (-1~1)
|
||||
a/d : increase/decrease leftward velocity (-1~1)
|
||||
i/, : increase/decrease upward velocity (-1~1)
|
||||
j/l : increase/decrease angular velocity (-0.1~0.1)
|
||||
r : return home
|
||||
t/y : arm/disarm
|
||||
v/n : takeoff(disenabled now)/land
|
||||
b : offboard
|
||||
s or 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)
|
||||
|
||||
def vels(target_forward_vel, target_leftward_vel, target_upward_vel,target_angular_vel):
|
||||
return "currently:\t forward x %s\t leftward y %s\t upward z %s\t angular %s " % (target_forward_vel, target_leftward_vel, target_upward_vel, target_angular_vel)
|
||||
|
||||
def makeSimpleProfile(output, input, slop):
|
||||
if input > output:
|
||||
output = min( input, output + slop )
|
||||
elif input < output:
|
||||
output = max( input, output - slop )
|
||||
else:
|
||||
output = input
|
||||
return output
|
||||
|
||||
def constrain(input, low, high):
|
||||
if input < low:
|
||||
input = low
|
||||
elif input > high:
|
||||
input = high
|
||||
else:
|
||||
input = input
|
||||
|
||||
return input
|
||||
|
||||
def checkLinearLimitVelocity(vel):
|
||||
vel = constrain(vel, -MAX_LIN_VEL, MAX_LIN_VEL)
|
||||
return vel
|
||||
|
||||
def checkAngularLimitVelocity(vel):
|
||||
vel = constrain(vel, -MAX_ANG_VEL, MAX_ANG_VEL)
|
||||
return vel
|
||||
|
||||
if __name__=="__main__":
|
||||
|
||||
settings = termios.tcgetattr(sys.stdin)
|
||||
|
||||
rospy.init_node('keyboard_control')
|
||||
multi_cmd_vel_flu_pub = [None]*uav_num
|
||||
multi_cmd_pub = [None]*uav_num
|
||||
for i in range(uav_num):
|
||||
multi_cmd_vel_flu_pub[i] = rospy.Publisher('/xtdrone/uav'+str(i+1)+'/cmd_accel_enu', Vector3, queue_size=10)
|
||||
multi_cmd_pub[i] = rospy.Publisher('/xtdrone/uav'+str(i+1)+'/cmd',String,queue_size=10)
|
||||
leader_cmd_vel_pub = rospy.Publisher("/xtdrone/leader/cmd_accel", Vector3, queue_size=10)
|
||||
formation_switch_pub = rospy.Publisher("/gcs/formation_switch", String, queue_size=10)
|
||||
cmd= String()
|
||||
twist = Twist()
|
||||
accel = Vector3()
|
||||
|
||||
|
||||
target_forward_vel = 0.0
|
||||
target_leftward_vel = 0.0
|
||||
target_upward_vel = 0.0
|
||||
target_angular_vel = 0.0
|
||||
control_forward_vel = 0.0
|
||||
control_leftward_vel = 0.0
|
||||
control_upward_vel = 0.0
|
||||
control_angular_vel = 0.0
|
||||
|
||||
count = 0
|
||||
|
||||
try:
|
||||
print_msg()
|
||||
while(1):
|
||||
key = getKey()
|
||||
if key == 'w' :
|
||||
target_forward_vel = checkLinearLimitVelocity(target_forward_vel + LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'x' :
|
||||
target_forward_vel = checkLinearLimitVelocity(target_forward_vel - LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'a' :
|
||||
target_leftward_vel = checkLinearLimitVelocity(target_leftward_vel + LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'd' :
|
||||
target_leftward_vel = checkLinearLimitVelocity(target_leftward_vel - LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'i' :
|
||||
target_upward_vel = checkLinearLimitVelocity(target_upward_vel + LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == ',' :
|
||||
target_upward_vel = checkLinearLimitVelocity(target_upward_vel - LIN_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'j':
|
||||
target_angular_vel = checkAngularLimitVelocity(target_angular_vel + ANG_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
elif key == 'l':
|
||||
target_angular_vel = checkAngularLimitVelocity(target_angular_vel - ANG_VEL_STEP_SIZE)
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
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 == 'k':
|
||||
cmd = 'HOVER'
|
||||
cmd_vel_mask = False
|
||||
print_msg()
|
||||
print('Hover')
|
||||
elif key == 's' :
|
||||
cmd_vel_mask = False
|
||||
target_forward_vel = 0.0
|
||||
target_leftward_vel = 0.0
|
||||
target_upward_vel = 0.0
|
||||
target_angular_vel = 0.0
|
||||
control_forward_vel = 0.0
|
||||
control_leftward_vel = 0.0
|
||||
control_upward_vel = 0.0
|
||||
control_angular_vel = 0.0
|
||||
print_msg()
|
||||
print(vels(target_forward_vel,-target_leftward_vel,target_upward_vel,target_angular_vel))
|
||||
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
|
||||
|
||||
|
||||
control_forward_vel = makeSimpleProfile(control_forward_vel, target_forward_vel, (LIN_VEL_STEP_SIZE/2.0))
|
||||
control_leftward_vel = makeSimpleProfile(control_leftward_vel, target_leftward_vel, (LIN_VEL_STEP_SIZE/2.0))
|
||||
control_upward_vel = makeSimpleProfile(control_upward_vel, target_upward_vel, (LIN_VEL_STEP_SIZE/2.0))
|
||||
twist.linear.x = control_forward_vel; twist.linear.y = control_leftward_vel; twist.linear.z = control_upward_vel
|
||||
|
||||
control_angular_vel = makeSimpleProfile(control_angular_vel, target_angular_vel, (ANG_VEL_STEP_SIZE/2.0))
|
||||
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = control_angular_vel
|
||||
|
||||
accel = twist.linear
|
||||
|
||||
for i in range(uav_num):
|
||||
if ctrl_leader:
|
||||
leader_cmd_vel_pub.publish(accel)
|
||||
if not cmd == '':
|
||||
formation_switch_pub.publish(cmd)
|
||||
else:
|
||||
if not cmd_vel_mask:
|
||||
multi_cmd_vel_flu_pub[i].publish(accel)
|
||||
multi_cmd_pub[i].publish(cmd)
|
||||
|
||||
cmd = ''
|
||||
except:
|
||||
print(e)
|
||||
|
||||
finally:
|
||||
twist.linear.x = 0.0; twist.linear.y = 0.0; twist.linear.z = 0.0
|
||||
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = 0.0
|
||||
accel.x = 0.0; accel.y = 0.0; accel.z = 0.0
|
||||
cmd = ''
|
||||
for i in range(uav_num):
|
||||
multi_cmd_vel_flu_pub[i].publish(accel)
|
||||
multi_cmd_pub[i].publish(cmd)
|
||||
|
||||
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
|
||||
|
|
@ -209,10 +209,10 @@ if __name__=="__main__":
|
|||
ctrl_leader = not ctrl_leader
|
||||
print_msg()
|
||||
elif key == 'k':
|
||||
cmd = 'AUTO.LOITER'
|
||||
cmd = 'HOVER'
|
||||
cmd_vel_mask = False
|
||||
print_msg()
|
||||
print('Loiter')
|
||||
print('Hover')
|
||||
elif key == 's' :
|
||||
cmd_vel_mask = False
|
||||
target_forward_vel = 0.0
|
||||
|
|
|
|||
|
|
@ -23,7 +23,7 @@ class Leader:
|
|||
self.avoid_vel = Vector3(0,0,0)
|
||||
self.formation_config = 'waiting'
|
||||
self.target_height_recorded = False
|
||||
self.f = 500
|
||||
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)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,99 @@
|
|||
#!/usr/bin/python
|
||||
# -*- coding: UTF-8 -*-
|
||||
import rospy
|
||||
from geometry_msgs.msg import TwistStamped, Twist, Vector3, PoseStamped
|
||||
from std_msgs.msg import String
|
||||
from pyquaternion import Quaternion
|
||||
import time
|
||||
import math
|
||||
import numpy
|
||||
import sys
|
||||
|
||||
class Leader:
|
||||
|
||||
def __init__(self, leader_id, uav_num):
|
||||
self.hover = True
|
||||
self.id = leader_id
|
||||
self.local_pose = PoseStamped()
|
||||
self.local_velocity = TwistStamped()
|
||||
self.cmd_vel_enu = Twist()
|
||||
self.cmd_accel_enu = Vector3()
|
||||
self.follower_num = uav_num - 1
|
||||
self.followers_info = [None]*self.follower_num
|
||||
self.follower_arrived_num = 0
|
||||
self.follower_all_arrived = True
|
||||
self.avoid_vel = Vector3(0,0,0)
|
||||
self.formation_config = 'waiting'
|
||||
self.followers_receive = [False]*self.follower_num
|
||||
self.target_height_recorded = False
|
||||
self.f = 200
|
||||
self.K_vxy = 0.5
|
||||
self.K_vz = 0.5
|
||||
self.K_pz = 0.5
|
||||
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.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_vel", Vector3, self.avoid_vel_callback)
|
||||
self.formation_switch_sub = rospy.Subscriber("/gcs/formation_switch",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)
|
||||
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.accel_enu_pub = rospy.Publisher('/xtdrone/uav'+str(self.id)+'/cmd_accel_enu', Vector3, queue_size=10)
|
||||
|
||||
def local_pose_callback(self, msg):
|
||||
self.local_pose = msg
|
||||
|
||||
def local_velocity_callback(self, msg):
|
||||
self.local_velocity = msg
|
||||
|
||||
def cmd_vel_callback(self, msg):
|
||||
self.cmd_vel_enu = msg
|
||||
if msg.linear.z == 0:
|
||||
self.hover = True
|
||||
else:
|
||||
self.hover = False
|
||||
|
||||
def cmd_callback(self, msg):
|
||||
if not msg.data == '':
|
||||
self.formation_config = msg.data
|
||||
#print("Switch to Formation"+self.formation_config)
|
||||
|
||||
def avoid_vel_callback(self, msg):
|
||||
self.avoid_vel = msg
|
||||
#print('leader: ', self.avoid_vel)
|
||||
|
||||
def followers_info_callback(self, msg, id):
|
||||
self.followers_info[id] = msg.data
|
||||
#print("follower"+str(id)+":"+ msg.data)
|
||||
|
||||
def loop(self):
|
||||
rospy.init_node('leader')
|
||||
rate = rospy.Rate(self.f)
|
||||
while True:
|
||||
#self.cmd_vel_enu = Twist()
|
||||
for i in range(self.follower_num):
|
||||
if self.followers_info[i] == "Received":
|
||||
self.followers_receive[i] = True
|
||||
if self.follower_arrived_num > self.follower_num - 1:
|
||||
self.follower_all_arrived = True
|
||||
if self.follower_all_arrived:
|
||||
self.formation_switch_pub.publish(self.formation_config)
|
||||
if self.formation_config == 'pyramid':
|
||||
if not self.target_height_recorded:
|
||||
target_height = self.local_pose.pose.position.z + 2
|
||||
self.target_height_recorded = True
|
||||
self.cmd_vel_enu.linear.z = self.K_vz * (target_height - self.local_pose.pose.position.z)
|
||||
self.cmd_vel_enu.linear.x += self.avoid_vel.x
|
||||
self.cmd_vel_enu.linear.y += self.avoid_vel.y
|
||||
self.cmd_vel_enu.linear.z += self.avoid_vel.z
|
||||
self.cmd_accel_enu.x = self.K_vxy * (self.cmd_vel_enu.linear.x - self.local_velocity.twist.linear.x)
|
||||
self.cmd_accel_enu.y = self.K_vxy * (self.cmd_vel_enu.linear.y - self.local_velocity.twist.linear.y)
|
||||
self.cmd_accel_enu.y = self.K_vz * (self.cmd_vel_enu.linear.z - self.local_velocity.twist.linear.z)
|
||||
self.accel_enu_pub.publish(self.cmd_accel_enu)
|
||||
self.local_pose_pub.publish(self.local_pose)
|
||||
rate.sleep()
|
||||
|
||||
if __name__ == '__main__':
|
||||
leader = Leader(1,int(sys.argv[1]))
|
||||
leader.loop()
|
||||
|
|
@ -12,7 +12,7 @@ use_1_8 = 1
|
|||
#uav_num=9
|
||||
uav_num = int(sys.argv[1])
|
||||
|
||||
step_time=0.002
|
||||
step_time=0.005
|
||||
|
||||
pose_puber=[None]*uav_num
|
||||
vel_puber=[None]*uav_num
|
||||
|
|
@ -20,7 +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)
|
||||
local_vel = [TwistStamped() for i in range (uav_num)]
|
||||
|
||||
for i in range(uav_num):
|
||||
uav_id=i+use_1_8
|
||||
|
|
|
|||
|
|
@ -0,0 +1,65 @@
|
|||
import matplotlib.pyplot as plt
|
||||
from mpl_toolkits.mplot3d import Axes3D
|
||||
import matplotlib.animation as animation
|
||||
import numpy as np
|
||||
import rospy
|
||||
from geometry_msgs.msg import Twist,Pose,PoseStamped,TwistStamped,Vector3
|
||||
from gazebo_msgs.srv import GetModelState
|
||||
import sys
|
||||
from std_msgs.msg import String
|
||||
from copy import deepcopy
|
||||
|
||||
|
||||
uav_num = int(sys.argv[1])
|
||||
|
||||
step_time=0.01
|
||||
Kp=100
|
||||
|
||||
pose_puber=[None]*uav_num
|
||||
vel_puber=[None]*uav_num
|
||||
|
||||
plot_x=[0]*uav_num
|
||||
plot_y=[0]*uav_num
|
||||
plot_z=[0]*uav_num
|
||||
local_vel = [TwistStamped() for i in range (uav_num)]
|
||||
cmd_vel=[Twist() for i in range (uav_num)]
|
||||
arrive_count=0
|
||||
|
||||
for i in range(uav_num):
|
||||
uav_id = i + 1
|
||||
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)
|
||||
|
||||
|
||||
|
||||
def cmd_accel_callback(msg,id):
|
||||
cmd_vel[id].linear.x = cmd_vel[id].linear.x + Kp * msg.x * step_time
|
||||
cmd_vel[id].linear.y = cmd_vel[id].linear.y + Kp * msg.y * step_time
|
||||
cmd_vel[id].linear.z = cmd_vel[id].linear.z + Kp * msg.z * step_time
|
||||
|
||||
plot_x[id]+=step_time*cmd_vel[id].linear.x
|
||||
plot_y[id]+=step_time*cmd_vel[id].linear.y
|
||||
plot_z[id]+=step_time*cmd_vel[id].linear.z
|
||||
if id == 0:
|
||||
print(id)
|
||||
local_vel[id].twist=deepcopy(cmd_vel[id])
|
||||
|
||||
|
||||
rospy.init_node('simple_3d_simulator')
|
||||
rate = rospy.Rate(1/step_time)
|
||||
|
||||
for i in range(uav_num):
|
||||
uav_id = i + 1
|
||||
rospy.Subscriber('/xtdrone/uav'+str(uav_id)+'/cmd_accel_enu',Vector3, cmd_accel_callback,i)
|
||||
|
||||
while not rospy.is_shutdown():
|
||||
|
||||
for i in range(uav_num):
|
||||
local_pose=PoseStamped()
|
||||
local_pose.pose.position.x=plot_x[i]
|
||||
local_pose.pose.position.y=plot_y[i]
|
||||
local_pose.pose.position.z=plot_z[i]
|
||||
pose_puber[i].publish(local_pose)
|
||||
vel_puber[i].publish(local_vel[i])
|
||||
|
|
@ -9,7 +9,7 @@ import sys
|
|||
|
||||
uav_num = int(sys.argv[1])
|
||||
|
||||
step_time=0.002
|
||||
step_time=0.005
|
||||
|
||||
pose_puber=[None]*uav_num
|
||||
vel_puber=[None]*uav_num
|
||||
|
|
|
|||
Loading…
Reference in New Issue