├── README.md └── control ├── scripts ├── time_test.py ├── land.py ├── takeoff.py ├── speed_test.py ├── ctrl_node_19.py ├── ctrl_node_new.py ├── ctrl2_node.py ├── ctrl_node.py └── ctrl_node_back.py ├── package.xml └── CMakeLists.txt /README.md: -------------------------------------------------------------------------------- 1 | # Multi-Bebop 2 | 基于ROS的多无人机协同控制 3 | 4 | # 使用说明 5 | 6 | 1. 在bebop的工作空间下安装bebop_ws包并编译 7 | 2. 启动launch文件：roslaunch bebop_tools bebop_nodelet_iv.launch 8 | 3. 下载control在src下 9 | 4. 启动py文件即可 10 | -------------------------------------------------------------------------------- /control/scripts/time_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # @Time : 2020/12/19 下午1:29 4 | # @Author : Chenan_Wang 5 | # @File : time_test.py 6 | # @Software: CLion 7 | 8 | import rospy 9 | from std_msgs.msg import String 10 | 11 | if __name__ == '__main__': 12 | # 创建节点 13 | rospy.init_node('publisher_node') 14 | 15 | # topic name 主题名称，唯一标示作用（类似于广播频段） 16 | # 命名规则 ‘/a/b/c/d' 17 | topic_name = '/hello/publisher' 18 | # date_class：数据类型 目前暂时使用 字符串 19 | publisher = rospy.Publisher(topic_name, String, queue_size=100) 20 | 21 | count = 0 22 | 23 | rate = rospy.Rate(4) 24 | 25 | while not rospy.is_shutdown(): 26 | # 往外发送数据 27 | msg = String() 28 | msg.data = 'hello topic {}'.format(count) 29 | publisher.publish(msg) 30 | 31 | rate.sleep() 32 | count += 1 33 | if count == 24: 34 | msg = String() 35 | msg.data = 'hello topic {}'.format(count) 36 | publisher.publish(msg) 37 | break 38 | -------------------------------------------------------------------------------- /control/scripts/land.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # @Time : 2020/12/16 下午10:14 4 | # @Author :Chenan_Wang 5 | # @File : ctrl_node.py.py 6 | # @Software: PyCharm 7 | 8 | import rospy 9 | import time 10 | from std_msgs.msg import Empty 11 | from geometry_msgs.msg import Twist 12 | from geometry_msgs.msg import Pose 13 | from geometry_msgs.msg import PoseStamped 14 | from std_msgs.msg import String 15 | 16 | # linear_vel = 0.05 17 | # angular_vel = 0.0 18 | # 19 | # twist = Twist() 20 | # 21 | # twist.linear.x = linear_vel 22 | # twist.linear.y = 0.0 23 | # twist.linear.z = 0.0 24 | # 25 | # twist.angular.x = 0.0 26 | # twist.angular.y = 0.0 27 | # twist.angular.z = angular_vel 28 | 29 | # 30 | # def __init__(self): 31 | # self.now_err_x = 0 32 | # self.last_err_x = 0 33 | # self.sum_err_x = 0 34 | 35 | 36 | def land(): 37 | land1_pub = rospy.Publisher('/bebop/land', Empty, queue_size=10) 38 | land2_pub = rospy.Publisher('/bebop2/land', Empty, queue_size=10) 39 | i = 0 40 | while 1: 41 | time.sleep(1) 42 | land1_pub.publish(Empty()) 43 | print "bebop_1 land!" 44 | land2_pub.publish(Empty()) 45 | print "bebop_2 land!" 46 | i += 1 47 | if i == 1: 48 | break 49 | 50 | 51 | if __name__ == '__main__': 52 | 53 | try: 54 | rospy.init_node('land_node') 55 | land() 56 | time.sleep(5) 57 | 58 | except rospy.ROSInterruptException: 59 | pass 60 | 61 | -------------------------------------------------------------------------------- /control/scripts/takeoff.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # @Time : 2020/12/16 下午10:14 4 | # @Author :Chenan_Wang 5 | # @File : ctrl_node.py.py 6 | # @Software: PyCharm 7 | 8 | import rospy 9 | import time 10 | from std_msgs.msg import Empty 11 | from geometry_msgs.msg import Twist 12 | from geometry_msgs.msg import Pose 13 | from geometry_msgs.msg import PoseStamped 14 | from std_msgs.msg import String 15 | 16 | # linear_vel = 0.05 17 | # angular_vel = 0.0 18 | # 19 | # twist = Twist() 20 | # 21 | # twist.linear.x = linear_vel 22 | # twist.linear.y = 0.0 23 | # twist.linear.z = 0.0 24 | # 25 | # twist.angular.x = 0.0 26 | # twist.angular.y = 0.0 27 | # twist.angular.z = angular_vel 28 | 29 | # 30 | # def __init__(self): 31 | # self.now_err_x = 0 32 | # self.last_err_x = 0 33 | # self.sum_err_x = 0 34 | 35 | 36 | def takeoff(): 37 | takeoff1_pub = rospy.Publisher('/bebop/takeoff', Empty, queue_size=10) 38 | takeoff2_pub = rospy.Publisher('/bebop2/takeoff', Empty, queue_size=10) 39 | rate = rospy.Rate(1) # 10hz 40 | i = 0 41 | while 1: 42 | print "wait 3s ..." 43 | time.sleep(2) 44 | takeoff1_pub.publish(Empty()) 45 | print "bebop_1 takeoff!" 46 | takeoff2_pub.publish(Empty()) 47 | print "bebop_2 takeoff!" 48 | i += 1 49 | if i == 1: 50 | break 51 | 52 | 53 | if __name__ == '__main__': 54 | 55 | try: 56 | rospy.init_node('takeoff_node') 57 | # now_err_x = 0 58 | takeoff() 59 | time.sleep(5) 60 | 61 | except rospy.ROSInterruptException: 62 | pass 63 | 64 | -------------------------------------------------------------------------------- /control/scripts/speed_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # @Time : 2020/12/19 下午1:13 4 | # @Author : Chenan_Wang 5 | # @File : speed_test.py 6 | # @Software: CLion 7 | 8 | import rospy 9 | import time 10 | from std_msgs.msg import Empty 11 | from geometry_msgs.msg import Twist 12 | from geometry_msgs.msg import Pose 13 | from geometry_msgs.msg import PoseStamped 14 | 15 | now_err_x = 0 16 | 17 | 18 | def takeoff(): 19 | takeoff_pub = rospy.Publisher('/bebop/takeoff', Empty, queue_size=10) 20 | rate = rospy.Rate(1) # 10hz 21 | i = 0 22 | while 1: 23 | print "wait 3s ..." 24 | time.sleep(2) 25 | takeoff_pub.publish(Empty()) 26 | print "bebop takeoff!" 27 | i += 1 28 | if i == 1: 29 | break 30 | 31 | 32 | def land(): 33 | land_pub = rospy.Publisher('/bebop/land', Empty, queue_size=10) 34 | rate = rospy.Rate(20) # 10hz 35 | i = 0 36 | while 1: 37 | time.sleep(3.5) 38 | land_pub.publish(Empty()) 39 | print "bebop land!" 40 | i += 1 41 | if i == 1: 42 | break 43 | 44 | 45 | def callback(msg): 46 | twist = Twist() 47 | 48 | global now_err_x 49 | 50 | last_err_x = now_err_x 51 | now_x = msg.pose.position.x 52 | now_err_x = now_x - 0 53 | 54 | twist.linear.x = 0.02 55 | twist.linear.y = 0 56 | twist.linear.z = 0 57 | 58 | speed = 1000 * twist.linear.x 59 | 60 | speed_x = (now_err_x - last_err_x) 61 | sppp = 10 * (-speed_x) 62 | 63 | if now_x > -900: 64 | 65 | # print 'now_x:', now_x 66 | print 'x:', msg.pose.position.x, 'y:', msg.pose.position.y 67 | print 'Set_speed:', speed 68 | print 'Real_speed:', sppp 69 | print "bebop send cmd!!!!" 70 | print '————————————————————————' 71 | else: 72 | twist.linear.x = 0 73 | twist.linear.y = 0 74 | twist.linear.z = 0 75 | 76 | cmd_pub = rospy.Publisher('/bebop/cmd_vel', Twist, queue_size=10) 77 | cmd_pub.publish(twist) 78 | 79 | # rate.sleep() 80 | 81 | 82 | if __name__ == '__main__': 83 | 84 | try: 85 | rospy.init_node('ctrl_node') 86 | # now_err_x = 0 87 | takeoff() 88 | time.sleep(5) 89 | print "Go!!!" 90 | rospy.Subscriber('/vrpn_client_node/bebop/pose', PoseStamped, callback) 91 | 92 | print "wait 3s ..." 93 | time.sleep(50) 94 | # cmd() 95 | land() 96 | 97 | except rospy.ROSInterruptException: 98 | pass 99 | 100 | -------------------------------------------------------------------------------- /control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | control 4 | 0.0.0 5 | The control package 6 | 7 | 8 | 9 | 10 | hlzy 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /control/scripts/ctrl_node_19.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # @Time : 2020/12/16 下午10:14 4 | # @Author :Chenan_Wang 5 | # @File : ctrl_node.py.py 6 | # @Software: PyCharm 7 | 8 | import rospy 9 | import time 10 | import math 11 | from std_msgs.msg import Empty 12 | from geometry_msgs.msg import Twist 13 | from geometry_msgs.msg import Pose 14 | from geometry_msgs.msg import PoseStamped 15 | from std_msgs.msg import String 16 | 17 | aaa = 0 18 | 19 | 20 | def takeoff(): 21 | takeoff_pub = rospy.Publisher('/bebop/takeoff', Empty, queue_size=10) 22 | rate = rospy.Rate(1) # 10hz 23 | i = 0 24 | while 1: 25 | print "wait 3s ..." 26 | time.sleep(2) 27 | takeoff_pub.publish(Empty()) 28 | print "bebop takeoff!" 29 | i += 1 30 | if i == 1: 31 | break 32 | 33 | 34 | def land(): 35 | land_pub = rospy.Publisher('/bebop/land', Empty, queue_size=10) 36 | rate = rospy.Rate(20) # 10hz 37 | i = 0 38 | while 1: 39 | time.sleep(3.5) 40 | land_pub.publish(Empty()) 41 | print "bebop land!" 42 | i += 1 43 | if i == 1: 44 | break 45 | 46 | 47 | def callback(msg): 48 | twist = Twist() 49 | 50 | x = msg.pose.position.x 51 | y = msg.pose.position.y 52 | 53 | x_sim = [1600, 800, 0, -600, -600, -500, -500, 0, 400, 700, 0] 54 | y_sim = [700, 700, 700, 700, 0, -600, -600, -500, -500, -500, 0] 55 | 56 | global aaa 57 | 58 | kp = 0.0002 59 | 60 | x_t = x_sim[aaa] 61 | y_t = y_sim[aaa] 62 | 63 | err_x = x - x_t 64 | err_y = y - y_t 65 | 66 | now_err = math.sqrt(math.pow((x_t-x), 2) + math.pow((y_t-y), 2)) 67 | 68 | if -270 < err_x < 270: 69 | twist.linear.x = 0 70 | else: 71 | twist.linear.x = kp * err_x 72 | 73 | if -270 < err_y < 270: 74 | twist.linear.y = 0 75 | else: 76 | twist.linear.y = -kp * err_y 77 | 78 | print 'Point:', aaa 79 | 80 | if now_err < 400: 81 | aaa = aaa + 1 82 | twist.linear.x = 0 83 | twist.linear.y = 0 84 | # time.sleep(1) 85 | if aaa >= 16: 86 | aaa = 16 87 | 88 | if twist.linear.x > 0.08: 89 | twist.linear.x = 0.08 90 | 91 | if twist.linear.x < -0.08: 92 | twist.linear.x = -0.08 93 | 94 | if twist.linear.y > 0.08: 95 | twist.linear.y = 0.08 96 | 97 | if twist.linear.y < -0.08: 98 | twist.linear.y = -0.08 99 | 100 | twist.angular.x = 0.0 101 | twist.angular.y = 0.0 102 | twist.angular.z = 0.0 103 | 104 | cmd_pub = rospy.Publisher('/bebop/cmd_vel', Twist, queue_size=1) 105 | cmd_pub.publish(twist) 106 | 107 | print 'x_t =', x_t, '; y_t =', y_t 108 | print 'x =', x, '; y =', y 109 | print 'speed_x =', twist.linear.x, '; speed_y =', twist.linear.y 110 | print 'err_x =', err_x, '; err_y =', err_y 111 | print 'now_err:', now_err 112 | print "bebop send cmd!" 113 | print '----------------------------' 114 | 115 | 116 | if __name__ == '__main__': 117 | 118 | try: 119 | rospy.init_node('ctrl_node') 120 | takeoff() 121 | time.sleep(5) 122 | print "Go!!!" 123 | rospy.Subscriber('/vrpn_client_node/feiji/pose', PoseStamped, callback) 124 | 125 | print "wait 3s ..." 126 | time.sleep(50) 127 | land() 128 | 129 | except rospy.ROSInterruptException: 130 | pass 131 | 132 | -------------------------------------------------------------------------------- /control/scripts/ctrl_node_new.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # @Time : 2020/12/16 下午10:14 4 | # @Author :Chenan_Wang 5 | # @File : ctrl_node.py.py 6 | # @Software: PyCharm 7 | 8 | import rospy 9 | import time 10 | from std_msgs.msg import Empty 11 | from geometry_msgs.msg import Twist 12 | from geometry_msgs.msg import Pose 13 | from geometry_msgs.msg import PoseStamped 14 | 15 | # linear_vel = 0.05 16 | # angular_vel = 0.0 17 | # 18 | # twist = Twist() 19 | # 20 | # twist.linear.x = linear_vel 21 | # twist.linear.y = 0.0 22 | # twist.linear.z = 0.0 23 | # 24 | # twist.angular.x = 0.0 25 | # twist.angular.y = 0.0 26 | # twist.angular.z = angular_vel 27 | 28 | # 29 | # def __init__(self): 30 | # self.now_err_x = 0 31 | # self.last_err_x = 0 32 | # self.sum_err_x = 0 33 | 34 | now_err_x = 0 35 | now_err_y = 0 36 | 37 | 38 | def takeoff(): 39 | takeoff_pub = rospy.Publisher('/bebop/takeoff', Empty, queue_size=10) 40 | rate = rospy.Rate(1) # 10hz 41 | i = 0 42 | while 1: 43 | print "wait 3s ..." 44 | time.sleep(2) 45 | takeoff_pub.publish(Empty()) 46 | print "bebop takeoff!" 47 | i += 1 48 | if i == 1: 49 | break 50 | 51 | 52 | def land(): 53 | land_pub = rospy.Publisher('/bebop/land', Empty, queue_size=10) 54 | rate = rospy.Rate(20) # 10hz 55 | i = 0 56 | while 1: 57 | time.sleep(3.5) 58 | land_pub.publish(Empty()) 59 | print "bebop land!" 60 | i += 1 61 | if i == 1: 62 | break 63 | 64 | 65 | def cmd(): 66 | cmd_pub = rospy.Publisher('/bebop/cmd_vel', Twist, queue_size=10) 67 | rate = rospy.Rate(20) # 10hz 68 | while not rospy.is_shutdown(): 69 | # cmd_pub.publish(twist) 70 | print "bebop send cmd!" 71 | rate.sleep() 72 | 73 | # def callback(self, data): 74 | 75 | 76 | def callback(msg): 77 | twist = Twist() 78 | 79 | global now_err_x 80 | global now_err_y 81 | kdx = 0.01 82 | kdy = 0.01 83 | 84 | x = msg.pose.position.x 85 | y = msg.pose.position.y 86 | z = msg.pose.position.z 87 | 88 | # kp_x = 0.001 89 | # ki_x = 0.001 90 | # kd_x = 0.001 91 | # 92 | last_err_x = now_err_x 93 | last_err_y = now_err_y 94 | now_err_x = x - 0 95 | now_err_y = y - 0 96 | # sum_err_x += now_err_x 97 | 98 | err_x = x - 1800 99 | err_y = y - 1000 100 | px = 0.0015 * err_x 101 | py = 0.0015 * err_y 102 | 103 | linear_vel = 0.1 104 | # 105 | # if twist.linear.x > 0.06: 106 | # twist.linear.x = 0.06 107 | # 108 | # if twist.linear.x < -0.06: 109 | # twist.linear.x = -0.06 110 | # 111 | # if twist.linear.y > 0.06: 112 | # twist.linear.y = 0.06 113 | # 114 | # if twist.linear.y < -0.06: 115 | # twist.linear.y = -0.06 116 | # 117 | # twist.angular.x = 0.0 118 | # twist.angular.y = 0.0 119 | # twist.angular.z = 0.0 120 | 121 | cmd_pub = rospy.Publisher('/bebop/cmd_vel', Twist, queue_size=10) 122 | rate = rospy.Rate(1) 123 | 124 | while not rospy.is_shutdown(): 125 | if -270 < err_x < 270: 126 | twist.linear.x = 0 127 | else: 128 | twist.linear.x = px * linear_vel + kdx * (now_err_x - last_err_x) * linear_vel 129 | 130 | if -270 < err_y < 270: 131 | twist.linear.y = 0 132 | else: 133 | twist.linear.y = -py * linear_vel - kdx * (now_err_y - last_err_y) * linear_vel 134 | 135 | twist.linear.z = 0.0 136 | cmd_pub.publish(twist) 137 | print x, y 138 | print twist.linear.x, twist.linear.y 139 | print "bebop send cmd!" 140 | rate.sleep() 141 | 142 | 143 | if __name__ == '__main__': 144 | 145 | rospy.init_node('ctrl_node') 146 | # takeoff() 147 | # time.sleep(5) 148 | print "Go!!!" 149 | rospy.Subscriber('/vrpn_client_node/feiji/pose', PoseStamped, callback) 150 | 151 | print "wait 3s ..." 152 | time.sleep(50) 153 | # cmd() 154 | land() 155 | 156 | -------------------------------------------------------------------------------- /control/scripts/ctrl2_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # @Time : 2020/12/16 下午10:14 4 | # @Author :Chenan_Wang 5 | # @File : ctrl_node.py.py 6 | # @Software: PyCharm 7 | 8 | import rospy 9 | import time 10 | from std_msgs.msg import Empty 11 | from geometry_msgs.msg import Twist 12 | from geometry_msgs.msg import Pose 13 | from geometry_msgs.msg import PoseStamped 14 | from std_msgs.msg import String 15 | 16 | # linear_vel = 0.05 17 | # angular_vel = 0.0 18 | # 19 | # twist = Twist() 20 | # 21 | # twist.linear.x = linear_vel 22 | # twist.linear.y = 0.0 23 | # twist.linear.z = 0.0 24 | # 25 | # twist.angular.x = 0.0 26 | # twist.angular.y = 0.0 27 | # twist.angular.z = angular_vel 28 | 29 | # 30 | # def __init__(self): 31 | # self.now_err_x = 0 32 | # self.last_err_x = 0 33 | # self.sum_err_x = 0 34 | 35 | now_err_x = 0 36 | now_err_y = 0 37 | sum_err_x = 0 38 | sum_err_y = 0 39 | 40 | 41 | def takeoff(): 42 | takeoff_pub = rospy.Publisher('/bebop2/takeoff', Empty, queue_size=10) 43 | rate = rospy.Rate(1) # 10hz 44 | i = 0 45 | while 1: 46 | print "wait 3s ..." 47 | time.sleep(2) 48 | takeoff_pub.publish(Empty()) 49 | print "bebop takeoff!" 50 | i += 1 51 | if i == 1: 52 | break 53 | 54 | 55 | def land(): 56 | land_pub = rospy.Publisher('/bebop2/land', Empty, queue_size=10) 57 | rate = rospy.Rate(20) # 10hz 58 | i = 0 59 | while 1: 60 | time.sleep(3.5) 61 | land_pub.publish(Empty()) 62 | print "bebop land!" 63 | i += 1 64 | if i == 1: 65 | break 66 | 67 | 68 | def callback(msg): 69 | twist = Twist() 70 | global now_err_x 71 | global now_err_y 72 | global sum_err_x 73 | global sum_err_y 74 | 75 | x = msg.pose.position.x 76 | y = msg.pose.position.y 77 | z = msg.pose.position.z 78 | 79 | last_err_x = now_err_x 80 | last_err_y = now_err_y 81 | now_err_x = x + 800 82 | now_err_y = y - 600 83 | sum_err_x += now_err_x 84 | sum_err_x += now_err_y 85 | 86 | px = 0.0002 87 | py = 0.0002 88 | 89 | if twist.linear.x > 0: 90 | kdx = -0.005 91 | else: 92 | kdx = 0.005 93 | 94 | if twist.linear.y > 0: 95 | kdy = 0.005 96 | else: 97 | kdy = -0.005 98 | 99 | linear_vel = 0.1 100 | 101 | if -270 < now_err_x < 270: 102 | twist.linear.x = 0 103 | # time.sleep(1) 104 | else: 105 | # twist.linear.x = px * linear_vel + kdx * (now_err_x - last_err_x) * linear_vel 106 | # twist.linear.x = px * linear_vel + kdx * (now_err_x - last_err_x) 107 | twist.linear.x = px * now_err_x 108 | 109 | if -270 < now_err_y < 270: 110 | twist.linear.y = 0 111 | # time.sleep(1) 112 | else: 113 | # twist.linear.y = -py * linear_vel - kdy * (now_err_y - last_err_y) * linear_vel 114 | # twist.linear.y = -py * linear_vel - kdy * (now_err_y - last_err_y) 115 | twist.linear.y = -py * now_err_y - 0.01 116 | 117 | twist.linear.z = 0.0 118 | 119 | if twist.linear.x > 0.08: 120 | twist.linear.x = 0.08 121 | 122 | if twist.linear.x < -0.08: 123 | twist.linear.x = -0.08 124 | 125 | if twist.linear.y > 0.08: 126 | twist.linear.y = 0.08 127 | 128 | if twist.linear.y < -0.08: 129 | twist.linear.y = -0.08 130 | 131 | twist.angular.x = 0.0 132 | twist.angular.y = 0.0 133 | twist.angular.z = 0.0 134 | 135 | print x, y 136 | print twist.linear.x, twist.linear.y 137 | print "ok" 138 | cmd_pub = rospy.Publisher('/bebop2/cmd_vel', Twist, queue_size=1) 139 | # rate = rospy.Rate(20) # 10hz 140 | 141 | cmd_pub.publish(twist) 142 | print "bebop send cmd!" 143 | 144 | 145 | if __name__ == '__main__': 146 | 147 | try: 148 | rospy.init_node('ctrl_node_2') 149 | # now_err_x = 0 150 | # takeoff() 151 | time.sleep(2) 152 | print "Go!!!" 153 | rospy.Subscriber('/vrpn_client_node/bebop2/pose', PoseStamped, callback) 154 | 155 | print "wait 3s ..." 156 | time.sleep(50) 157 | # cmd() 158 | land() 159 | 160 | except rospy.ROSInterruptException: 161 | pass 162 | 163 | -------------------------------------------------------------------------------- /control/scripts/ctrl_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # @Time : 2020/12/16 下午10:14 4 | # @Author :Chenan_Wang 5 | # @File : ctrl_node.py.py 6 | # @Software: PyCharm 7 | 8 | import rospy 9 | import time 10 | from std_msgs.msg import Empty 11 | from geometry_msgs.msg import Twist 12 | from geometry_msgs.msg import Pose 13 | from geometry_msgs.msg import PoseStamped 14 | from std_msgs.msg import String 15 | 16 | # linear_vel = 0.05 17 | # angular_vel = 0.0 18 | # 19 | # twist = Twist() 20 | # 21 | # twist.linear.x = linear_vel 22 | # twist.linear.y = 0.0 23 | # twist.linear.z = 0.0 24 | # 25 | # twist.angular.x = 0.0 26 | # twist.angular.y = 0.0 27 | # twist.angular.z = angular_vel 28 | 29 | # 30 | # def __init__(self): 31 | # self.now_err_x = 0 32 | # self.last_err_x = 0 33 | # self.sum_err_x = 0 34 | 35 | now_err_x = 0 36 | now_err_y = 0 37 | sum_err_x = 0 38 | sum_err_y = 0 39 | 40 | 41 | def takeoff(): 42 | takeoff_pub = rospy.Publisher('/bebop/takeoff', Empty, queue_size=10) 43 | rate = rospy.Rate(1) # 10hz 44 | i = 0 45 | while 1: 46 | print "wait 3s ..." 47 | time.sleep(2) 48 | takeoff_pub.publish(Empty()) 49 | print "bebop takeoff!" 50 | i += 1 51 | if i == 1: 52 | break 53 | 54 | 55 | def land(): 56 | land_pub = rospy.Publisher('/bebop/land', Empty, queue_size=10) 57 | rate = rospy.Rate(20) # 10hz 58 | i = 0 59 | while 1: 60 | time.sleep(3.5) 61 | land_pub.publish(Empty()) 62 | print "bebop land!" 63 | i += 1 64 | if i == 1: 65 | break 66 | 67 | 68 | def cmd(): 69 | cmd_pub = rospy.Publisher('/bebop/cmd_vel', Twist, queue_size=10) 70 | rate = rospy.Rate(20) # 10hz 71 | while not rospy.is_shutdown(): 72 | # cmd_pub.publish(twist) 73 | print "bebop send cmd!" 74 | rate.sleep() 75 | 76 | # def callback(self, data): 77 | 78 | 79 | def callback(msg): 80 | twist = Twist() 81 | global now_err_x 82 | global now_err_y 83 | global sum_err_x 84 | global sum_err_y 85 | 86 | x = msg.pose.position.x 87 | y = msg.pose.position.y 88 | z = msg.pose.position.z 89 | 90 | last_err_x = now_err_x 91 | last_err_y = now_err_y 92 | now_err_x = x + 800 93 | now_err_y = y + 700 94 | sum_err_x += now_err_x 95 | sum_err_x += now_err_y 96 | 97 | px = 0.0002 98 | py = 0.0002 99 | 100 | if twist.linear.x > 0: 101 | kdx = -0.005 102 | else: 103 | kdx = 0.005 104 | 105 | if twist.linear.y > 0: 106 | kdy = 0.005 107 | else: 108 | kdy = -0.005 109 | 110 | linear_vel = 0.1 111 | 112 | if -270 < now_err_x < 270: 113 | twist.linear.x = 0 114 | # time.sleep(1) 115 | else: 116 | # twist.linear.x = px * linear_vel + kdx * (now_err_x - last_err_x) * linear_vel 117 | # twist.linear.x = px * linear_vel + kdx * (now_err_x - last_err_x) 118 | twist.linear.x = px * now_err_x 119 | 120 | if -270 < now_err_y < 270: 121 | twist.linear.y = 0 122 | # time.sleep(1) 123 | else: 124 | # twist.linear.y = -py * linear_vel - kdy * (now_err_y - last_err_y) * linear_vel 125 | # twist.linear.y = -py * linear_vel - kdy * (now_err_y - last_err_y) 126 | twist.linear.y = -py * now_err_y - 0.01 127 | 128 | twist.linear.z = 0.0 129 | 130 | if twist.linear.x > 0.08: 131 | twist.linear.x = 0.08 132 | 133 | if twist.linear.x < -0.08: 134 | twist.linear.x = -0.08 135 | 136 | if twist.linear.y > 0.08: 137 | twist.linear.y = 0.08 138 | 139 | if twist.linear.y < -0.08: 140 | twist.linear.y = -0.08 141 | 142 | twist.angular.x = 0.0 143 | twist.angular.y = 0.0 144 | twist.angular.z = 0.0 145 | 146 | print x, y 147 | print twist.linear.x, twist.linear.y 148 | print "ok" 149 | cmd_pub = rospy.Publisher('/bebop/cmd_vel', Twist, queue_size=1) 150 | # rate = rospy.Rate(20) # 10hz 151 | 152 | cmd_pub.publish(twist) 153 | print "bebop send cmd!" 154 | 155 | 156 | if __name__ == '__main__': 157 | 158 | try: 159 | rospy.init_node('ctrl_node') 160 | # now_err_x = 0 161 | # takeoff() 162 | time.sleep(1) 163 | print "Go!!!" 164 | rospy.Subscriber('/vrpn_client_node/bebop/pose', PoseStamped, callback) 165 | 166 | print "wait 3s ..." 167 | time.sleep(50) 168 | # cmd() 169 | land() 170 | 171 | except rospy.ROSInterruptException: 172 | pass 173 | 174 | -------------------------------------------------------------------------------- /control/scripts/ctrl_node_back.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # @Time : 2020/12/16 下午10:14 4 | # @Author :Chenan_Wang 5 | # @File : ctrl_node.py.py 6 | # @Software: PyCharm 7 | 8 | import rospy 9 | import time 10 | from std_msgs.msg import Empty 11 | from geometry_msgs.msg import Twist 12 | from geometry_msgs.msg import Pose 13 | from geometry_msgs.msg import PoseStamped 14 | from std_msgs.msg import String 15 | 16 | # linear_vel = 0.05 17 | # angular_vel = 0.0 18 | # 19 | # twist = Twist() 20 | # 21 | # twist.linear.x = linear_vel 22 | # twist.linear.y = 0.0 23 | # twist.linear.z = 0.0 24 | # 25 | # twist.angular.x = 0.0 26 | # twist.angular.y = 0.0 27 | # twist.angular.z = angular_vel 28 | 29 | # 30 | # def __init__(self): 31 | # self.now_err_x = 0 32 | # self.last_err_x = 0 33 | # self.sum_err_x = 0 34 | 35 | now_err_x = 0 36 | now_err_y = 0 37 | sum_err_x = 0 38 | sum_err_y = 0 39 | 40 | 41 | def takeoff(): 42 | takeoff_pub = rospy.Publisher('/bebop/takeoff', Empty, queue_size=10) 43 | rate = rospy.Rate(1) # 10hz 44 | i = 0 45 | while 1: 46 | print "wait 3s ..." 47 | time.sleep(2) 48 | takeoff_pub.publish(Empty()) 49 | print "bebop takeoff!" 50 | i += 1 51 | if i == 1: 52 | break 53 | 54 | 55 | def land(): 56 | land_pub = rospy.Publisher('/bebop/land', Empty, queue_size=10) 57 | rate = rospy.Rate(20) # 10hz 58 | i = 0 59 | while 1: 60 | time.sleep(3.5) 61 | land_pub.publish(Empty()) 62 | print "bebop land!" 63 | i += 1 64 | if i == 1: 65 | break 66 | 67 | 68 | def cmd(): 69 | cmd_pub = rospy.Publisher('/bebop/cmd_vel', Twist, queue_size=10) 70 | rate = rospy.Rate(20) # 10hz 71 | while not rospy.is_shutdown(): 72 | # cmd_pub.publish(twist) 73 | print "bebop send cmd!" 74 | rate.sleep() 75 | 76 | # def callback(self, data): 77 | 78 | 79 | def callback(msg): 80 | twist = Twist() 81 | global now_err_x 82 | global now_err_y 83 | global sum_err_x 84 | global sum_err_y 85 | 86 | x = msg.pose.position.x 87 | y = msg.pose.position.y 88 | z = msg.pose.position.z 89 | 90 | last_err_x = now_err_x 91 | last_err_y = now_err_y 92 | now_err_x = x - 1300 93 | now_err_y = y - 500 94 | sum_err_x += now_err_x 95 | sum_err_x += now_err_y 96 | 97 | px = 0.0002 98 | py = 0.0002 99 | 100 | if twist.linear.x > 0: 101 | kdx = -0.005 102 | else: 103 | kdx = 0.005 104 | 105 | if twist.linear.y > 0: 106 | kdy = 0.005 107 | else: 108 | kdy = -0.005 109 | 110 | linear_vel = 0.1 111 | 112 | if -270 < now_err_x < 270: 113 | twist.linear.x = 0 114 | # time.sleep(1) 115 | else: 116 | # twist.linear.x = px * linear_vel + kdx * (now_err_x - last_err_x) * linear_vel 117 | # twist.linear.x = px * linear_vel + kdx * (now_err_x - last_err_x) 118 | twist.linear.x = px * now_err_x 119 | 120 | if -270 < now_err_y < 270: 121 | twist.linear.y = 0 122 | # time.sleep(1) 123 | else: 124 | # twist.linear.y = -py * linear_vel - kdy * (now_err_y - last_err_y) * linear_vel 125 | # twist.linear.y = -py * linear_vel - kdy * (now_err_y - last_err_y) 126 | twist.linear.y = -py * now_err_y - 0.01 127 | 128 | twist.linear.z = 0.0 129 | 130 | if twist.linear.x > 0.08: 131 | twist.linear.x = 0.08 132 | 133 | if twist.linear.x < -0.08: 134 | twist.linear.x = -0.08 135 | 136 | if twist.linear.y > 0.08: 137 | twist.linear.y = 0.08 138 | 139 | if twist.linear.y < -0.08: 140 | twist.linear.y = -0.08 141 | 142 | twist.angular.x = 0.0 143 | twist.angular.y = 0.0 144 | twist.angular.z = 0.0 145 | 146 | print x, y 147 | print twist.linear.x, twist.linear.y 148 | print "ok" 149 | cmd_pub = rospy.Publisher('/bebop/cmd_vel', Twist, queue_size=1) 150 | # rate = rospy.Rate(20) # 10hz 151 | 152 | cmd_pub.publish(twist) 153 | print "bebop send cmd!" 154 | 155 | 156 | if __name__ == '__main__': 157 | 158 | try: 159 | rospy.init_node('ctrl_node') 160 | # now_err_x = 0 161 | # takeoff() 162 | # time.sleep(5) 163 | print "Go!!!" 164 | rospy.Subscriber('/vrpn_client_node/bebop/pose', PoseStamped, callback) 165 | 166 | print "wait 3s ..." 167 | time.sleep(50) 168 | # cmd() 169 | land() 170 | 171 | except rospy.ROSInterruptException: 172 | pass 173 | 174 | -------------------------------------------------------------------------------- /control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(control) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a exec_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if your package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES control 104 | # CATKIN_DEPENDS other_catkin_pkg 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | include_directories( 115 | # include 116 | # ${catkin_INCLUDE_DIRS} 117 | ) 118 | 119 | ## Declare a C++ library 120 | # add_library(${PROJECT_NAME} 121 | # src/${PROJECT_NAME}/control.cpp 122 | # ) 123 | 124 | ## Add cmake target dependencies of the library 125 | ## as an example, code may need to be generated before libraries 126 | ## either from message generation or dynamic reconfigure 127 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 128 | 129 | ## Declare a C++ executable 130 | ## With catkin_make all packages are built within a single CMake context 131 | ## The recommended prefix ensures that target names across packages don't collide 132 | # add_executable(${PROJECT_NAME}_node src/control_node.cpp) 133 | 134 | ## Rename C++ executable without prefix 135 | ## The above recommended prefix causes long target names, the following renames the 136 | ## target back to the shorter version for ease of user use 137 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 138 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 139 | 140 | ## Add cmake target dependencies of the executable 141 | ## same as for the library above 142 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 143 | 144 | ## Specify libraries to link a library or executable target against 145 | # target_link_libraries(${PROJECT_NAME}_node 146 | # ${catkin_LIBRARIES} 147 | # ) 148 | 149 | ############# 150 | ## Install ## 151 | ############# 152 | 153 | # all install targets should use catkin DESTINATION variables 154 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 155 | 156 | ## Mark executable scripts (Python etc.) for installation 157 | ## in contrast to setup.py, you can choose the destination 158 | # catkin_install_python(PROGRAMS 159 | # scripts/my_python_script 160 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark executables for installation 164 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 165 | # install(TARGETS ${PROJECT_NAME}_node 166 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 167 | # ) 168 | 169 | ## Mark libraries for installation 170 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 171 | # install(TARGETS ${PROJECT_NAME} 172 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 175 | # ) 176 | 177 | ## Mark cpp header files for installation 178 | # install(DIRECTORY include/${PROJECT_NAME}/ 179 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 180 | # FILES_MATCHING PATTERN "*.h" 181 | # PATTERN ".svn" EXCLUDE 182 | # ) 183 | 184 | ## Mark other files for installation (e.g. launch and bag files, etc.) 185 | # install(FILES 186 | # # myfile1 187 | # # myfile2 188 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 189 | # ) 190 | 191 | ############# 192 | ## Testing ## 193 | ############# 194 | 195 | ## Add gtest based cpp test target and link libraries 196 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_control.cpp) 197 | # if(TARGET ${PROJECT_NAME}-test) 198 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 199 | # endif() 200 | 201 | ## Add folders to be run by python nosetests 202 | # catkin_add_nosetests(test) 203 | --------------------------------------------------------------------------------