aah98
/
XTDrone
forked from xtdrone/XTDrone
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

wanshanbiandui

This commit is contained in:
lan 2020-03-25 08:36:56 -07:00
parent 9974d53e08
commit 482eb88d96
8 changed files with 63 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
control/follower_accel_control.py
View File

@ -4,12 +4,13 @@ import rospy
from geometry_msgs.msg import Twist, Vector3, PoseStamped, TwistStamped
from std_msgs.msg import String
from pyquaternion import Quaternion
from formation_dict import formation_dict
from formation_dict import formation_dict_9
import time
import math
import numpy
import sys
import heapq
import copy
class Follower:
@ -26,14 +27,16 @@ class Follower:
self.following_ids = []
self.formation_config = 'waiting'
self.following_count = 0
self.Kp = 100
self.Kp = 50 #100
self.kr = (4/int((self.uav_num-1)/3))**0.5
#self.kr = 1
self.f = 50
self.velxy_max = 0.5
#self.f=1/0.01
self.velxy_max = 0.5 #0.5
self.velz_max = 1
self.following_local_pose = [None]*self.uav_num
self.following_local_pose = [PoseStamped()]*self.uav_num
self.following_local_pose_sub = [None]*self.uav_num
self.following_local_velocity = [None]*self.uav_num
self.following_local_velocity = [TwistStamped()]*self.uav_num
self.following_local_velocity_sub = [None]*self.uav_num
self.arrive_count = 0
self.local_pose_sub = rospy.Subscriber("/uav"+str(self.id)+"/mavros/local_position/pose", PoseStamped , self.local_pose_callback)
@ -44,19 +47,22 @@ class Follower:
self.vel_enu_pub = rospy.Publisher('/xtdrone/uav'+str(self.id)+'/cmd_vel_enu', Twist, queue_size=10)
self.info_pub = rospy.Publisher('/xtdrone/uav'+str(self.id)+'/info', String, queue_size=10)
def local_pose_callback(self, msg):
self.local_pose = msg
#print(self.arrive_count)
def local_velocity_callback(self, msg):
self.local_velocity = msg
#print(self.arrive_count)
def following_local_pose_callback(self, msg, id):
self.following_local_pose[id] = msg
#print('here')
def following_local_velocity_callback(self, msg, id):
self.following_local_velocity[id] = msg
def cmd_vel_callback(self, msg):
self.cmd_vel_enu = msg
@ -79,7 +85,7 @@ class Follower:
def loop(self):
rospy.init_node('follower'+str(self.id-1))
rate = rospy.Rate(self.f)
while True:
while not rospy.is_shutdown():
if self.arrive_count > 100:
self.info_pub.publish("Arrived")
#print("Arrived")
@ -87,7 +93,7 @@ class Follower:
self.following_switch = False
self.info_pub.publish("Received")
print("Follower"+str(self.id-1)+": Switch to Formation "+self.formation_config)
self.L_matrix = self.get_L_matrix(formation_dict[self.formation_config])
self.L_matrix = self.get_L_matrix(formation_dict_9[self.formation_config])
#print(self.L_matrix)
#self.L_matrix = numpy.array([[0,0,0,0,0,0,0,0,0],[1,-1,0,0,0,0,0,0,0],[1,0,-1,0,0,0,0,0,0],[1,0,0,-1,0,0,0,0,0],[1,0,0,0,-1,0,0,0,0],[1,0,0,0,0,-1,0,0,0],[1,0,0,0,0,0,-1,0,0],[1,0,0,0,0,0,0,-1,0],[1,0,0,0,0,0,0,0,-1]])
self.following_ids = numpy.argwhere(self.L_matrix[self.id-1,:] == 1)
@ -107,32 +113,42 @@ class Follower:
time.sleep(1)
self.cmd_accel_enu = Vector3(0, 0, 0)
#self.cmd_vel_enu.linear = copy.deepcopy(self.avoid_vel)
for following_id in self.following_ids:
if self.following_local_pose[following_id[0]] == None and self.following_local_velocity[following_id[0]] == None:
print(following_id)
self.cmd_accel_enu.x += self.following_local_pose[following_id[0]].pose.position.x + self.kr * self.following_local_velocity[following_id[0]].twist.linear.x - self.local_pose.pose.position.x - self.kr * self.local_velocity.twist.linear.x + formation_dict[self.formation_config][0, self.id-2]
self.cmd_accel_enu.y += self.following_local_pose[following_id[0]].pose.position.y + self.kr * self.following_local_velocity[following_id[0]].twist.linear.y - self.local_pose.pose.position.y - self.kr * self.local_velocity.twist.linear.y + formation_dict[self.formation_config][1, self.id-2]
self.cmd_accel_enu.z += self.following_local_pose[following_id[0]].pose.position.z + self.kr * self.following_local_velocity[following_id[0]].twist.linear.z - self.local_pose.pose.position.z - self.kr * self.local_velocity.twist.linear.z + formation_dict[self.formation_config][2, self.id-2]
print(following_id)
self.cmd_accel_enu.x += self.following_local_pose[following_id[0]].pose.position.x + self.kr * self.following_local_velocity[following_id[0]].twist.linear.x - self.local_pose.pose.position.x - self.kr * self.local_velocity.twist.linear.x + formation_dict_9[self.formation_config][0, self.id-2]
self.cmd_accel_enu.y += self.following_local_pose[following_id[0]].pose.position.y + self.kr * self.following_local_velocity[following_id[0]].twist.linear.y - self.local_pose.pose.position.y - self.kr * self.local_velocity.twist.linear.y + formation_dict_9[self.formation_config][1, self.id-2]
self.cmd_accel_enu.z += self.following_local_pose[following_id[0]].pose.position.z + self.kr * self.following_local_velocity[following_id[0]].twist.linear.z - self.local_pose.pose.position.z - self.kr * self.local_velocity.twist.linear.z + formation_dict_9[self.formation_config][2, self.id-2]
if not following_id[0] == 0:
self.cmd_accel_enu.x -= formation_dict[self.formation_config][0, following_id[0]-1]
self.cmd_accel_enu.y -= formation_dict[self.formation_config][1, following_id[0]-1]
self.cmd_accel_enu.z -= formation_dict[self.formation_config][2, following_id[0]-1]
self.cmd_accel_enu.x -= formation_dict_9[self.formation_config][0, following_id[0]-1]
self.cmd_accel_enu.y -= formation_dict_9[self.formation_config][1, following_id[0]-1]
self.cmd_accel_enu.z -= formation_dict_9[self.formation_config][2, following_id[0]-1]
self.cmd_vel_enu.linear.x = self.local_velocity.twist.linear.x + self.avoid_vel.x + self.Kp * self.cmd_accel_enu.x / self.f
self.cmd_vel_enu.linear.y = self.local_velocity.twist.linear.y +self.avoid_vel.y + self.Kp * self.cmd_accel_enu.y / self.f
self.cmd_vel_enu.linear.z = self.local_velocity.twist.linear.z +self.avoid_vel.z + self.Kp * self.cmd_accel_enu.z / self.f
if self.cmd_vel_enu.linear.x > self.velxy_max:
self.cmd_vel_enu.linear.x = self.velxy_max
elif self.cmd_vel_enu.linear.x < - self.velxy_max:
self.cmd_vel_enu.linear.x = - self.velxy_max
if self.cmd_vel_enu.linear.y > self.velxy_max:
self.cmd_vel_enu.linear.y = self.velxy_max
elif self.cmd_vel_enu.linear.y < - self.velxy_max:
self.cmd_vel_enu.linear.y = - self.velxy_max
if self.cmd_vel_enu.linear.z > self.velz_max:
self.cmd_vel_enu.linear.z = self.velz_max
elif self.cmd_vel_enu.linear.z < - self.velz_max:
self.cmd_vel_enu.linear.z = - self.velz_max
if (self.cmd_vel_enu.linear.x)**2+(self.cmd_vel_enu.linear.y)**2+(self.cmd_vel_enu.linear.z)**2<0.2:
self.arrive_count += 1
else:
self.arrive_count = 0
self.vel_enu_pub.publish(self.cmd_vel_enu)
if not self.formation_config == 'waiting':
self.vel_enu_pub.publish(self.cmd_vel_enu)
rate.sleep()
#函数输入为相对leader的位置矩阵和无人机数量，输出为L矩阵
@ -323,5 +339,7 @@ class Follower:
'''
if __name__ == '__main__':
follower = Follower(int(sys.argv[1]),9)
follower = Follower(int(sys.argv[1]),int(sys.argv[2]))
#follower = Follower(int(sys.argv[1]),9)
#follower = Follower(9,9)
follower.loop()

8
control/follower_vel_control.py
View File

@ -77,9 +77,9 @@ class Follower:
self.following_switch = False
self.info_pub.publish("Received")
print("Follower"+str(self.id-1)+": Switch to Formation "+self.formation_config)
#self.L_matrix = self.get_L_matrix(formation_dict_9[self.formation_config])
self.L_matrix = self.get_L_matrix(formation_dict_9[self.formation_config])
#print(self.L_matrix)
self.L_matrix = numpy.array([[0,0,0,0,0,0,0,0,0],[1,-1,0,0,0,0,0,0,0],[1,0,-1,0,0,0,0,0,0],[1,0,0,-1,0,0,0,0,0],[1,0,0,0,-1,0,0,0,0],[1,0,0,0,0,-1,0,0,0],[1,0,0,0,0,0,-1,0,0],[1,0,0,0,0,0,0,-1,0],[1,0,0,0,0,0,0,0,-1]])
#self.L_matrix = numpy.array([[0,0,0,0,0,0,0,0,0],[1,-1,0,0,0,0,0,0,0],[1,0,-1,0,0,0,0,0,0],[1,0,0,-1,0,0,0,0,0],[1,0,0,0,-1,0,0,0,0],[1,0,0,0,0,-1,0,0,0],[1,0,0,0,0,0,-1,0,0],[1,0,0,0,0,0,0,-1,0],[1,0,0,0,0,0,0,0,-1]])
self.following_ids = numpy.argwhere(self.L_matrix[self.id-1,:] == 1)
#if self.id == 2:
#print(self.following_ids)
@ -92,13 +92,13 @@ class Follower:
self.following_local_pose_sub[following_id[0]] = rospy.Subscriber("/uav"+str(following_id[0]+1)+"/mavros/local_position/pose", PoseStamped , self.following_local_pose_callback,following_id[0])
self.following_count += 1
time.sleep(1)
print(self.avoid_vel)
#print(self.avoid_vel)
self.cmd_vel_enu.linear = copy.deepcopy(self.avoid_vel)
if self.cmd_vel_enu.linear.x == 0 and self.cmd_vel_enu.linear.y == 0 and self.cmd_vel_enu.linear.z == 0:
for following_id in self.following_ids:
self.cmd_vel_enu.linear.x += self.following_local_pose[following_id[0]].pose.position.x - self.local_pose.pose.position.x + formation_dict_9[self.formation_config][0, self.id-2]
self.cmd_vel_enu.linear.y += self.following_local_pose[following_id[0]].pose.position.y - self.local_pose.pose.position.y + formation_dict_9[self.formation_config][1, self.id-2]
self.cmd_vel_enu.linear.z += self.following_local_pose[following_id[0]].pose.position.z - self.local_pose.pose.position.z+ formation_dict_9[self.formation_config][2, self.id-2]
self.cmd_vel_enu.linear.z += self.following_local_pose[following_id[0]].pose.position.z - self.local_pose.pose.position.z + formation_dict_9[self.formation_config][2, self.id-2]
if not following_id[0] == 0:
self.cmd_vel_enu.linear.x -= formation_dict_9[self.formation_config][0, following_id[0]-1]
self.cmd_vel_enu.linear.y -= formation_dict_9[self.formation_config][1, following_id[0]-1]

BIN
control/formation_dict.pyc
View File

Binary file not shown.

4
control/keyboard_multi_control_new.py
View File

@ -12,8 +12,8 @@ ANG_VEL_STEP_SIZE = 0.01
cmd_vel_mask = False
ctrl_leader = False
uav_num = int(sys.argv[1])
uav_num=9
#uav_num = int(sys.argv[1])
if uav_num == 9:
formation_configs = ['waiting', 'cube', 'pyramid', 'triangle']
if uav_num == 6:

2
control/leader.py
View File

@ -61,7 +61,7 @@ class Leader:
rospy.init_node('leader')
rate = rospy.Rate(50)
while True:
self.cmd_vel_enu = Twist()
#self.cmd_vel_enu = Twist()
for follower_info in self.followers_info:
if follower_info == "Arrived":
self.follower_arrived_num += 1

4
control/run.sh
View File

@ -3,7 +3,7 @@ python leader.py $1 &
uav_num=2
while(( $uav_num<= $1 ))
do
python follower_vel_control.py $uav_num $1&
python follower_accel_control.py $uav_num $1&
#echo $uav_num
let "uav_num++"
done
done

29
control/simple_3d_simulator_accel.py
View File

@ -9,10 +9,10 @@ import sys
use_1_8 = 1
uav_num=9
#uav_num = int(sys.argv[1])
uav_num = int(sys.argv[1])
step_time=0.001
step_time=0.01
pose_puber=[None]*uav_num
vel_puber=[None]*uav_num
@ -20,6 +20,7 @@ vel_puber=[None]*uav_num
plot_x=[0]*(uav_num)
plot_y=[0]*(uav_num)
plot_z=[0]*(uav_num)
local_vel = [TwistStamped()]*(uav_num)
for i in range(uav_num):
uav_id=i+use_1_8
@ -51,20 +52,24 @@ def init():
ax.set_zlim3d(-label_lim, label_lim)
#def cmd_accel_callback(msg,id):
accel=msg
plot_x[id]+=step_time*accel.linear.x
plot_y[id]+=step_time*accel.linear.y
plot_z[id]+=step_time*accel.linear.z
def cmd_vel_callback(msg,id):
# accel=msg
# plot_x[id]+=step_time*accel.linear.x
# plot_y[id]+=step_time*accel.linear.y
# plot_z[id]+=step_time*accel.linear.z
local_vel[id].twist=msg
plot_x[id]+=step_time*local_vel[id].twist.linear.x
plot_y[id]+=step_time*local_vel[id].twist.linear.y
plot_z[id]+=step_time*local_vel[id].twist.linear.z
rospy.init_node('simple_3d_simulator')
rate = rospy.Rate(1000)
rate = rospy.Rate(1/step_time)
for i in range(uav_num):
uav_id=i+use_1_8
rospy.Subscriber('/xtdrone/uav'+str(uav_id)+'/cmd_accel_flu', Twist, cmd_accel_callback,i)
rospy.Subscriber('/xtdrone/uav'+str(uav_id)+'/cmd_accel_enu', Twist, cmd_accel_callback,i)
rospy.Subscriber('/xtdrone/uav'+str(uav_id)+'/cmd_vel_flu', Twist, cmd_vel_callback,i)
rospy.Subscriber('/xtdrone/uav'+str(uav_id)+'/cmd_vel_enu', Twist, cmd_vel_callback,i)
try:
while not rospy.is_shutdown():
@ -74,7 +79,7 @@ try:
local_pose.pose.position.y=plot_y[i]
local_pose.pose.position.z=plot_z[i]
pose_puber[i].publish(local_pose)
pose_puber[i].publish(local_pose)
vel_puber[i].publish(local_vel[i])
init()
ax.scatter(plot_x,plot_y,plot_z,marker="x")
plt.pause(step_time)

4
control/stop.sh
View File

@ -1,2 +1,2 @@
kill -9 $(ps -ef|grep follower_vel_control.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ')
kill -9 $(ps -ef|grep leader.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ')
kill -9 $(ps -ef|grep follower_accel_control.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ')
kill -9 $(ps -ef|grep leader.py|gawk '$0 !~/grep/ {print $2}' |tr -s '\n' ' ')