├── README.md ├── bebop_ctrl.py └── trajectory_tracking.py /README.md: -------------------------------------------------------------------------------- 1 | # Python-Trajectory-Tracking-Control-for-UAV 2 | # 单无人机对螺旋轨迹跟踪的实物实验 3 | 4 | 无人机用的是bebop，软件框架是ROS，语言为Python，硬件不同跑不起来，不过可以参考代码中控制逻辑，希望能有些帮助。 5 | 6 | 控制方法是对串级PID控制，外环是位置环，内环是速度环 7 | 8 | # 运行 9 | 10 | 终端运行bebop_ctrl.py，在弹出的qt界面中进行操作，控制bebop无人机 11 | 12 | 无人机的控制代码在trajectory_tracking.py中。 13 | -------------------------------------------------------------------------------- /bebop_ctrl.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # @Time : 2022/01/04 上午07:57 4 | # @Author : Chenan_Wang 5 | # @File : bebop_ctrl.py 6 | # @Software: PyCharm 7 | 8 | from PyQt5.QtWidgets import * 9 | from trajectory_tracking import DataWindow 10 | import rospy 11 | import sys 12 | 13 | 14 | if __name__ == '__main__': 15 | 16 | rospy.init_node('turtle_ctrl_node') 17 | 18 | app = QApplication(sys.argv) 19 | 20 | window = DataWindow() 21 | window.show() 22 | 23 | sys.exit(app.exec_()) 24 | 25 | -------------------------------------------------------------------------------- /trajectory_tracking.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # @Time : 2022/01/06 下午23:10 4 | # @Author : Chenan_Wang 5 | # @File : trajectory_tracking.py 6 | # @Software: PyCharm 7 | 8 | from PyQt5.QtWidgets import * 9 | from PyQt5.QtCore import * 10 | from PyQt5.QtGui import * 11 | from geometry_msgs.msg import Twist, PoseStamped 12 | from std_msgs.msg import Empty 13 | import message_filters, rospy, threading 14 | import math, os, sys, time 15 | import numpy as np 16 | import matplotlib.pyplot as plt 17 | 18 | # class MainWindow(QWidget): 19 | # def __init__(self): 20 | # super(MainWindow, self).__init__() 21 | # self.setWindowTitle('Bebop Ctrl') 22 | # self.resize(400, 400) 23 | # 24 | # layout = QFormLayout() 25 | # self.setLayout(layout) 26 | # 27 | # self.linear_x = QLineEdit() 28 | # self.linear_y = QLineEdit() 29 | # self.linear_z = QLineEdit() 30 | # 31 | # btn = QPushButton('TakeOff !') 32 | # layout.addRow('Vel_X', self.linear_x) 33 | # layout.addRow('Vel_Y', self.linear_y) 34 | # layout.addRow('Vel_Z', self.linear_z) 35 | # layout.addRow(btn) 36 | # 37 | # btn.clicked.connect(self.click_send) 38 | # 39 | # topic_name = '/bebop1/cmd_vel' 40 | # self.publisher = rospy.Publisher(topic_name, Twist, queue_size=100) 41 | # 42 | # def click_send(self): 43 | # print 'send' 44 | # 45 | # linear_x = float(self.linear_x.text()) 46 | # linear_y = float(self.linear_y.text()) 47 | # linear_z = float(self.linear_z.text()) 48 | # 49 | # twist = Twist() 50 | # twist.linear.x = linear_x 51 | # twist.linear.y = linear_y 52 | # twist.linear.z = linear_z 53 | # self.publisher.publish(twist) 54 | 55 | 56 | class DataWindow(QWidget): 57 | def __init__(self): 58 | super(DataWindow, self).__init__() 59 | self.uav2_x_last = 0 60 | self.uav2_y_last = 0 61 | self.uav2_z_last = 0 62 | self.uav2_x = 0 63 | self.uav2_y = 0 64 | self.uav2_z = 0 65 | self.uav2_vx = 0 66 | self.uav2_vy = 0 67 | self.uav2_vz = 0 68 | self.uav2_x_err = 0 69 | self.uav2_y_err = 0 70 | self.uav2_z_err = 0 71 | self.uav2_vx_err = 0 72 | self.uav2_vy_err = 0 73 | self.control_vx_2 = 0 74 | self.control_vy_2 = 0 75 | self.msec_time_2 = 0 76 | self.vvx_tar_2 = 0 77 | self.vvy_tar_2 = 0 78 | 79 | self.uav4_x_last = 0 80 | self.uav4_y_last = 0 81 | self.uav4_z_last = 0 82 | self.uav4_x = 0 83 | self.uav4_y = 0 84 | self.uav4_z = 0 85 | self.uav4_vx = 0 86 | self.uav4_vy = 0 87 | self.uav4_vz = 0 88 | self.uav4_x_err = 0 89 | self.uav4_y_err = 0 90 | self.uav4_z_err = 0 91 | self.uav4_vx_err = 0 92 | self.uav4_vy_err = 0 93 | self.control_vx_4 = 0 94 | self.control_vy_4 = 0 95 | self.msec_time_4 = 0 96 | self.vvx_tar_4 = 0 97 | self.vvy_tar_4 = 0 98 | self.theta_4 = 0 99 | self.x_target_4 = 0 100 | self.y_target_4 = 0 101 | self.z_target_4 = 0 102 | 103 | self.tb1_x = 0 104 | self.tb1_y = 0 105 | self.tb2_x = 0 106 | self.tb2_y = 0 107 | self.tb3_x = 0 108 | self.tb3_y = 0 109 | self.tb4_x = 0 110 | self.tb4_y = 0 111 | 112 | self.flag_send = 0 113 | self.flag_draw = 1 114 | self.flag1 = 0 115 | self.flag2 = 0 116 | self.flag3 = 0 117 | self.flag4 = 0 118 | 119 | update_timer = QTimer(self) 120 | update_timer.setInterval(16) # 16ms update -- 60fps 121 | update_timer.start() 122 | 123 | update_timer.timeout.connect(self.on_update) 124 | 125 | self.setWindowTitle('Bebop Ctrl') 126 | self.resize(400, 400) 127 | 128 | wlayout = QVBoxLayout() 129 | layout1 = QFormLayout() 130 | layout2 = QHBoxLayout() 131 | layout3 = QHBoxLayout() 132 | layout4 = QHBoxLayout() 133 | layout5 = QHBoxLayout() 134 | layout6 = QHBoxLayout() 135 | layout7 = QHBoxLayout() 136 | layout8 = QHBoxLayout() 137 | layout9 = QHBoxLayout() 138 | splitter1 = QSplitter(Qt.Horizontal) 139 | 140 | self.linear_x = QLineEdit() 141 | self.linear_y = QLineEdit() 142 | self.linear_z = QLineEdit() 143 | 144 | self.lb_x = QLabel() 145 | self.lb_y = QLabel() 146 | self.lb_z = QLabel() 147 | self.lb_linear_x = QLabel() 148 | self.lb_linear_y = QLabel() 149 | self.lb_linear_z = QLabel() 150 | 151 | btn_send = QPushButton('Send !') 152 | btn_stop = QPushButton('Stop !') 153 | btn_takeoff = QPushButton('TakeOff !') 154 | btn_land = QPushButton('Land !') 155 | btn_draw = QPushButton('Draw') 156 | btn_save = QPushButton('Save') 157 | label1 = QLabel() 158 | label1.setText('x') 159 | label2 = QLabel() 160 | label2.setText(' y ') 161 | label3 = QLabel() 162 | label3.setText(' z ') 163 | label4 = QLabel() 164 | label4.setText(' ') 165 | label_text = QLabel() 166 | label_text.setText('选择无人机序号：') 167 | label_text2 = QLabel() 168 | label_text2.setText('手动输入目标点：') 169 | label_text3 = QLabel() 170 | label_text3.setText('手动输入速度指令：') 171 | ck1 = QCheckBox('UAV_01') 172 | ck2 = QCheckBox('UAV_02') 173 | ck3 = QCheckBox('UAV_03') 174 | ck4 = QCheckBox('UAV_04') 175 | 176 | topleft = QFrame(self) 177 | topleft.setFrameShape(QFrame.StyledPanel) 178 | 179 | splitter1.addWidget(topleft) 180 | layout5.addWidget(label_text) 181 | layout4.addWidget(ck1) 182 | layout4.addWidget(ck2) 183 | layout4.addWidget(ck3) 184 | layout4.addWidget(ck4) 185 | 186 | # self.setLayout(layout1) 187 | layout6.addWidget(label_text2) 188 | layout3.addWidget(label1) 189 | layout3.addWidget(self.linear_x) 190 | layout3.addWidget(label2) 191 | layout3.addWidget(self.linear_y) 192 | layout3.addWidget(label3) 193 | layout3.addWidget(self.linear_z) 194 | layout3.addWidget(label4) 195 | layout3.addWidget(btn_send) 196 | layout3.addWidget(btn_stop) 197 | layout7.addWidget(label4) 198 | layout8.addWidget(label4) 199 | layout9.addWidget(label4) 200 | 201 | layout1.addRow('当前位置 X：', self.lb_x) 202 | layout1.addRow('当前位置 Y：', self.lb_y) 203 | layout1.addRow('当前位置 Z：', self.lb_z) 204 | layout1.addRow('当前速度 Vx：', self.lb_linear_x) 205 | layout1.addRow('当前速度 Vy：', self.lb_linear_y) 206 | layout1.addRow('当前速度 Vz：', self.lb_linear_z) 207 | 208 | layout2.addWidget(btn_takeoff) 209 | layout2.addWidget(btn_land) 210 | layout2.addWidget(btn_draw) 211 | layout2.addWidget(btn_save) 212 | 213 | # wlayout.addLayout(splitter1) 214 | wlayout.addLayout(layout5) 215 | wlayout.addLayout(layout4) 216 | wlayout.addLayout(layout7) 217 | wlayout.addLayout(layout6) 218 | wlayout.addLayout(layout3) 219 | wlayout.addLayout(layout8) 220 | wlayout.addLayout(layout1) 221 | wlayout.addLayout(layout9) 222 | wlayout.addLayout(layout2) 223 | self.setLayout(wlayout) 224 | 225 | ck1.stateChanged.connect(self.ck1_func) 226 | ck2.stateChanged.connect(self.ck2_func) 227 | ck3.stateChanged.connect(self.ck3_func) 228 | ck4.stateChanged.connect(self.ck4_func) 229 | 230 | btn_send.clicked.connect(self.a) 231 | btn_stop.clicked.connect(self.data_stop) 232 | btn_takeoff.clicked.connect(self.takeoff_send) 233 | btn_land.clicked.connect(self.land_send) 234 | btn_draw.clicked.connect(self.a_draw) 235 | btn_save.clicked.connect(self.draw_save) 236 | 237 | # pose_topic_name = '/vrpn_client_node/uav1219_2/pose' 238 | # rospy.Subscriber(pose_topic_name, PoseStamped, self.pose_callback, queue_size=1) 239 | 240 | UAV_01 = message_filters.Subscriber('/vrpn_client_node/UAV_01/pose', PoseStamped, queue_size=1) 241 | UAV_02 = message_filters.Subscriber('/vrpn_client_node/uav1213_1/pose', PoseStamped, queue_size=1) 242 | UAV_03 = message_filters.Subscriber('/vrpn_client_node/UAV_03/pose', PoseStamped, queue_size=1) 243 | UAV_04 = message_filters.Subscriber('/vrpn_client_node/uav1219_2/pose', PoseStamped, queue_size=1) 244 | AGV_01 = message_filters.Subscriber('/vrpn_client_node/tb3/pose', PoseStamped, queue_size=1) 245 | # AGV_02 = message_filters.Subscriber('/vrpn_client_node/tb2/pose', PoseStamped, queue_size=1) 246 | # AGV_03 = message_filters.Subscriber('/vrpn_client_node/tb3/pose', PoseStamped, queue_size=1) 247 | # AGV_04 = message_filters.Subscriber('/vrpn_client_node/tb4/pose', PoseStamped, queue_size=1) 248 | sync = message_filters.ApproximateTimeSynchronizer([UAV_02, UAV_04, AGV_01], 10, 0.1, allow_headerless=True) 249 | sync.registerCallback(self.multi_callback) 250 | 251 | def GetNowTime(self): 252 | return time.strftime("%m%d%H%M%S", time.localtime(time.time())) 253 | 254 | def multi_callback(self, UAV_02, UAV_04, AGV_01): 255 | if self.flag2 == 1: 256 | if not isinstance(UAV_02, PoseStamped): 257 | return 258 | self.tb1_x = - AGV_01.pose.position.x 259 | self.tb1_y = AGV_01.pose.position.y 260 | # self.tb2_x = - AGV_02.pose.position.x 261 | # self.tb2_y = AGV_02.pose.position.y 262 | # self.tb3_x = - AGV_03.pose.position.x 263 | # self.tb3_y = AGV_03.pose.position.y 264 | # self.tb4_x = - AGV_04.pose.position.x 265 | # self.tb4_y = AGV_04.pose.position.y 266 | 267 | uav2_x = - UAV_02.pose.position.x 268 | uav2_y = UAV_02.pose.position.y 269 | uav2_z = UAV_02.pose.position.z - 58 270 | last_time_2 = self.msec_time_2 271 | nsec_time_2 = UAV_02.header.stamp.nsecs 272 | self.msec_time_2 = nsec_time_2 / 1000000 273 | if (self.msec_time_2 - last_time_2) > 0: 274 | time_gap_2 = round((self.msec_time_2 - last_time_2), 4) 275 | else: 276 | time_gap_2 = round((self.msec_time_2 - last_time_2 + 1000), 4) 277 | self.uav2_x = round(uav2_x, 1) 278 | self.uav2_y = round(uav2_y, 1) 279 | self.uav2_z = round(uav2_z, 1) 280 | uav2_vx = (self.uav2_x - self.uav2_x_last) / time_gap_2 281 | uav2_vy = (self.uav2_y - self.uav2_y_last) / time_gap_2 282 | uav2_vz = (self.uav2_z - self.uav2_z_last) / time_gap_2 283 | self.uav2_vx = round(uav2_vx, 8) 284 | self.uav2_vy = round(uav2_vy, 8) 285 | self.uav2_vz = round(uav2_vz, 8) 286 | self.lb_x.setText(str(self.uav2_x)) 287 | self.lb_y.setText(str(self.uav2_y)) 288 | self.lb_z.setText(str(self.uav2_z)) 289 | self.lb_linear_x.setText(str(self.uav2_vx)) 290 | self.lb_linear_y.setText(str(self.uav2_vy)) 291 | self.lb_linear_z.setText(str(self.uav2_vz)) 292 | self.uav2_x_last = self.uav2_x 293 | self.uav2_y_last = self.uav2_y 294 | self.uav2_z_last = self.uav2_z 295 | 296 | if self.flag4 == 1: 297 | if not isinstance(UAV_04, PoseStamped): 298 | return 299 | self.tb1_x = - AGV_01.pose.position.x 300 | self.tb1_y = AGV_01.pose.position.y 301 | 302 | uav4_x = - UAV_04.pose.position.x 303 | uav4_y = UAV_04.pose.position.y 304 | uav4_z = UAV_04.pose.position.z - 58 305 | 306 | last_time_4 = self.msec_time_4 307 | nsec_time_4 = UAV_04.header.stamp.nsecs 308 | self.msec_time_4 = nsec_time_4 / 1000000 309 | if (self.msec_time_4 - last_time_4) > 0: 310 | time_gap_4 = round((self.msec_time_4 - last_time_4), 4) 311 | else: 312 | time_gap_4 = round((self.msec_time_4 - last_time_4 + 1000), 4) 313 | self.uav4_x = round(uav4_x, 1) 314 | self.uav4_y = round(uav4_y, 1) 315 | self.uav4_z = round(uav4_z, 1) 316 | uav4_vx = (self.uav4_x - self.uav4_x_last) / time_gap_4 317 | uav4_vy = (self.uav4_y - self.uav4_y_last) / time_gap_4 318 | uav4_vz = (self.uav4_z - self.uav4_z_last) / time_gap_4 319 | self.uav4_vx = round(uav4_vx, 8) 320 | self.uav4_vy = round(uav4_vy, 8) 321 | self.uav4_vz = round(uav4_vz, 8) 322 | self.lb_x.setText(str(self.uav4_x)) 323 | self.lb_y.setText(str(self.uav4_y)) 324 | self.lb_z.setText(str(self.uav4_z)) 325 | self.lb_linear_x.setText(str(self.uav4_vx)) 326 | self.lb_linear_y.setText(str(self.uav4_vy)) 327 | self.lb_linear_z.setText(str(self.uav4_vz)) 328 | self.uav4_x_last = self.uav4_x 329 | self.uav4_y_last = self.uav4_y 330 | self.uav4_z_last = self.uav4_z 331 | 332 | def ck1_func(self, state): 333 | if state == 2: 334 | print 'Select UAV_01 !!!' 335 | self.flag1 = 1 336 | elif state == 0: 337 | print "Deselect UAV_01 !!!" 338 | self.flag1 = 0 339 | print 'Flag1 =', self.flag1 340 | 341 | def ck2_func(self, state): 342 | if state == 2: 343 | print 'Select UAV_02 !!!' 344 | self.flag2 = 1 345 | elif state == 0: 346 | print "Deselect UAV_02 !!!" 347 | self.flag2 = 0 348 | print 'Flag2 =', self.flag2 349 | 350 | def ck3_func(self, state): 351 | if state == 2: 352 | print 'Select UAV_03 !!!' 353 | self.flag3 = 1 354 | elif state == 0: 355 | print "Deselect UAV_03 !!!" 356 | self.flag3 = 0 357 | print 'Flag3 =', self.flag3 358 | 359 | def ck4_func(self, state): 360 | if state == 2: 361 | print 'Select UAV_04 !!!' 362 | self.flag4 = 1 363 | elif state == 0: 364 | print "Deselect UAV_04 !!!" 365 | self.flag4 = 0 366 | print 'Flag4 =', self.flag4 367 | 368 | def takeoff_send(self): 369 | print 'UAV Takeoff !!!' 370 | if self.flag1 == 1: 371 | takeoff_pub = rospy.Publisher('/bebop1/takeoff', Empty, queue_size=10) 372 | takeoff_pub.publish(Empty()) 373 | if self.flag2 == 1: 374 | takeoff_pub = rospy.Publisher('/bebop2/takeoff', Empty, queue_size=10) 375 | takeoff_pub.publish(Empty()) 376 | if self.flag3 == 1: 377 | takeoff_pub = rospy.Publisher('/bebop3/takeoff', Empty, queue_size=10) 378 | takeoff_pub.publish(Empty()) 379 | if self.flag4 == 1: 380 | takeoff_pub = rospy.Publisher('/bebop4/takeoff', Empty, queue_size=10) 381 | takeoff_pub.publish(Empty()) 382 | 383 | def land_send(self): 384 | print 'UAV Land !!!' 385 | if self.flag1 == 1: 386 | land_pub = rospy.Publisher('/bebop1/land', Empty, queue_size=10) 387 | land_pub.publish(Empty()) 388 | if self.flag2 == 1: 389 | land_pub = rospy.Publisher('/bebop2/land', Empty, queue_size=10) 390 | land_pub.publish(Empty()) 391 | if self.flag3 == 1: 392 | land_pub = rospy.Publisher('/bebop3/land', Empty, queue_size=10) 393 | land_pub.publish(Empty()) 394 | if self.flag4 == 1: 395 | land_pub = rospy.Publisher('/bebop4/land', Empty, queue_size=10) 396 | land_pub.publish(Empty()) 397 | 398 | def a_draw(self): 399 | t2 = threading.Thread(target=self.data_draw, name='t2') 400 | t2.start() 401 | 402 | def data_draw(self): 403 | self.flag_draw = 1 404 | fig, ax1 = plt.subplots(figsize=(4, 7)) 405 | # fig2 = plt.figure() 406 | # ax2 = fig2.add_subplot(311) 407 | # ax3 = fig2.add_subplot(312) 408 | # ax8 = fig2.add_subplot(313) 409 | fig3 = plt.figure() 410 | ax5 = fig3.add_subplot(311) 411 | ax6 = fig3.add_subplot(312) 412 | ax4 = fig3.add_subplot(313) 413 | now_x2 = [] 414 | now_y2 = [] 415 | now_z2 = [] 416 | now_x4 = [] 417 | now_y4 = [] 418 | now_z4 = [] 419 | controlv2 = [] 420 | controlv4 = [] 421 | x_tar2 = [] 422 | y_tar2 = [] 423 | z_tar2 = [] 424 | x_tar4 = [] 425 | y_tar4 = [] 426 | z_tar4 = [] 427 | vvvx2 = [] 428 | vvvy2 = [] 429 | vvvx4 = [] 430 | vvvy4 = [] 431 | vx_tar2 = [] 432 | vy_tar2 = [] 433 | vx_tar4 = [] 434 | vy_tar4 = [] 435 | 436 | t = [] 437 | i = 0 438 | k = 0 439 | while True: 440 | if self.flag_draw == 1: 441 | if self.flag2 == 1 and self.flag4 == 1: 442 | t.append(i) 443 | 444 | x_target_2 = float(self.linear_x.text()) 445 | y_target_2 = float(self.linear_y.text()) 446 | z_target_2 = float(self.linear_z.text()) 447 | # x_target_2 = self.tb1_x 448 | # y_target_2 = self.tb1_y 449 | x2 = self.uav2_x 450 | y2 = self.uav2_y 451 | z2 = self.uav2_z 452 | now_x2.append(x2) 453 | now_y2.append(y2) 454 | now_z2.append(z2) 455 | x_tar2.append(x_target_2) 456 | y_tar2.append(y_target_2) 457 | z_tar2.append(z_target_2) 458 | vx2 = self.uav2_vx 459 | vy2 = self.uav2_vy 460 | vvvx2.append(vx2) 461 | vvvy2.append(vy2) 462 | vx_tar2.append(self.vvx_tar_2) 463 | vy_tar2.append(self.vvy_tar_2) 464 | control_vx_2 = self.control_vx_2 465 | controlv2.append(control_vx_2) 466 | 467 | x_target_4 = float(self.linear_x.text()) 468 | y_target_4 = float(self.linear_y.text()) 469 | z_target_4 = float(self.linear_z.text()) 470 | x4 = self.uav4_x 471 | y4 = self.uav4_y 472 | z4 = self.uav4_z 473 | now_x4.append(x4) 474 | now_y4.append(y4) 475 | now_z4.append(z4) 476 | x_tar4.append(x_target_4) 477 | y_tar4.append(y_target_4) 478 | z_tar4.append(z_target_4) 479 | vx4 = self.uav4_vx 480 | vy4 = self.uav4_vy 481 | vvvx4.append(vx4) 482 | vvvy4.append(vy4) 483 | vx_tar4.append(self.vvx_tar_4) 484 | vy_tar4.append(self.vvy_tar_4) 485 | control_vx_4 = self.control_vx_4 486 | controlv4.append(control_vx_4) 487 | 488 | # ax1.cla() 489 | # # ax2.cla() 490 | # # ax3.cla() 491 | # # ax4.cla() 492 | # ax5.cla() 493 | # ax6.cla() 494 | # 495 | # ax1.set_title("UAVs") 496 | # ax1.set_xlabel("y / mm") 497 | # ax1.set_ylabel("x / mm") 498 | # ax1.set_xlim(-1000, 1000) 499 | # ax1.set_ylim(-1800, 1800) 500 | # ax1.grid() 501 | # ax1.scatter(y2, x2, s=10, c='g') 502 | # ax1.scatter(y4, x4, s=10, c='g') 503 | # ax1.scatter(y_target_2, x_target_2, s=10, c='r') 504 | # 505 | # ax5.set_ylim(-1000, 1000) 506 | # ax5.plot(t, x_tar2) 507 | # ax5.plot(t, now_x2) 508 | # ax6.set_xlabel("t") 509 | # ax6.set_ylabel("y / mm") 510 | # ax6.set_ylim(-1000, 1000) 511 | # ax6.plot(t, y_tar2) 512 | # ax6.plot(t, now_y2) 513 | # ax4.plot(t, controlv2, label='v') 514 | i += 1 515 | # plt.pause(0.01) 516 | 517 | if self.flag2 == 1 and self.flag4 != 1: 518 | t.append(i) 519 | 520 | x_target_2 = float(self.linear_x.text()) 521 | y_target_2 = float(self.linear_y.text()) 522 | 523 | # x_target_2 = self.tb1_x 524 | # y_target_2 = self.tb1_y 525 | 526 | x2 = self.uav2_x 527 | y2 = self.uav2_y 528 | now_x2.append(x2) 529 | now_y2.append(y2) 530 | x_tar2.append(x_target_2) 531 | y_tar2.append(y_target_2) 532 | vx2 = self.uav2_vx 533 | vy2 = self.uav2_vy 534 | vvvx2.append(vx2) 535 | vvvy2.append(vy2) 536 | vx_tar2.append(self.vvx_tar_2) 537 | vy_tar2.append(self.vvy_tar_2) 538 | control_vx_2 = self.control_vx_2 539 | controlv2.append(control_vx_2) 540 | 541 | ax1.cla() 542 | # ax2.cla() 543 | # ax3.cla() 544 | # ax4.cla() 545 | ax5.cla() 546 | ax6.cla() 547 | 548 | ax1.set_title("UAVs") 549 | ax1.set_xlabel("y / mm") 550 | ax1.set_ylabel("x / mm") 551 | ax1.set_xlim(-1000, 1000) 552 | ax1.set_ylim(-1800, 1800) 553 | ax1.grid() 554 | ax1.scatter(y2, x2, marker='x', s=50, c='g') 555 | ax1.scatter(y_target_2, x_target_2, marker='*', s=80, c='r') 556 | 557 | ax5.set_ylim(-1000, 1000) 558 | ax5.plot(t, x_tar2) 559 | ax5.plot(t, now_x2) 560 | ax6.set_xlabel("t") 561 | ax6.set_ylabel("y / mm") 562 | ax6.set_ylim(-1000, 1000) 563 | ax6.plot(t, y_tar2) 564 | ax6.plot(t, now_y2) 565 | ax4.plot(t, controlv2, label='v') 566 | i += 1 567 | plt.pause(0.01) 568 | 569 | if self.flag4 == 1 and self.flag2 != 1: 570 | t.append(i) 571 | theta = k*0.01 572 | # print(theta) 573 | 574 | x_target_4 = 500 * np.cos(theta) 575 | y_target_4 = 500 * np.sin(theta) 576 | z_target_4 = 500 + k 577 | k += 2 578 | print(z_target_4) 579 | # x_target_4 = float(self.linear_x.text()) 580 | # y_target_4 = float(self.linear_y.text()) 581 | # z_target_4 = float(self.linear_z.text()) 582 | x4 = self.uav4_x 583 | y4 = self.uav4_y 584 | z4 = self.uav4_z 585 | now_x4.append(x4) 586 | now_y4.append(y4) 587 | now_z4.append(z4) 588 | x_tar4.append(self.x_target_4) 589 | y_tar4.append(self.y_target_4) 590 | z_tar4.append(self.z_target_4) 591 | vx4 = self.uav4_vx 592 | vy4 = self.uav4_vy 593 | vvvx4.append(vx4) 594 | vvvy4.append(vy4) 595 | vx_tar4.append(self.vvx_tar_4) 596 | vy_tar4.append(self.vvy_tar_4) 597 | control_vx_4 = self.control_vx_4 598 | controlv4.append(control_vx_4) 599 | ax1.cla() 600 | # ax2.cla() 601 | # ax3.cla() 602 | # ax4.cla() 603 | ax5.cla() 604 | ax6.cla() 605 | ax1.set_title("UAVs") 606 | ax1.set_xlabel("y / mm") 607 | ax1.set_ylabel("x / mm") 608 | ax1.set_xlim(-1000, 1000) 609 | ax1.set_ylim(-1800, 1800) 610 | ax1.grid() 611 | ax1.scatter(y4, x4, s=10, c='g') 612 | ax1.scatter(self.y_target_4, self.x_target_4, s=10, c='r') 613 | 614 | # ax2.set_xlabel("t") 615 | # ax2.set_ylabel("vx (mm/ms)") 616 | # ax2.set_ylim(-1, 1) 617 | # ax2.plot(t, vx_tar) 618 | # ax2.plot(t, vvvx) 619 | # ax3.set_xlabel("t") 620 | # ax3.set_ylim(-1, 1) 621 | # ax3.set_ylabel("vy (mm/ms)") 622 | # ax3.plot(t, vy_tar) 623 | # ax3.plot(t, vvvy) 624 | 625 | ax5.set_xlabel("t") 626 | ax5.set_ylabel("x / mm") 627 | ax5.set_ylim(-1000, 1000) 628 | ax5.plot(t, x_tar4) 629 | ax5.plot(t, now_x4) 630 | 631 | ax6.set_xlabel("t") 632 | ax6.set_ylabel("y / mm") 633 | ax6.set_ylim(-1000, 1000) 634 | ax6.plot(t, y_tar4) 635 | ax6.plot(t, now_y4) 636 | 637 | ax4.set_xlabel("t") 638 | ax4.set_ylabel("z / mm") 639 | ax4.set_ylim(0, 1500) 640 | ax4.plot(t, z_tar4) 641 | ax4.plot(t, now_z4) 642 | 643 | i += 1 644 | plt.pause(0.01) 645 | 646 | else: 647 | time = self.GetNowTime() 648 | if self.flag2 == 1: 649 | x2_target = np.array([x_tar2]).T 650 | y2_target = np.array([y_tar2]).T 651 | z2_target = np.array([z_tar2]).T 652 | now_xx2 = np.array([now_x2]).T 653 | now_yy2 = np.array([now_y2]).T 654 | now_zz2 = np.array([now_z2]).T 655 | vx2_target = np.array([vx_tar2]).T 656 | vy2_target = np.array([vy_tar2]).T 657 | now_vvvx = np.array([vvvx2]).T 658 | now_vvvy = np.array([vvvy2]).T 659 | t_list = np.array([t]).T 660 | control_uav1 = np.array([controlv2]).T 661 | # print x2_target 662 | uav2_data_save = np.concatenate((t_list, x2_target, y2_target, z2_target, now_xx2, now_yy2, now_zz2, 663 | vx2_target, vy2_target, now_vvvx, now_vvvy, control_uav1), axis=1) 664 | np.savetxt("/home/hlzy/chenan_wang/mulit-bebop_ws/uav2_data_"+time+".txt", uav2_data_save, fmt='%f', delimiter=', ') 665 | plt.savefig("/home/hlzy/chenan_wang/mulit-bebop_ws/uav2_"+time+".png") 666 | if self.flag4 == 1: 667 | x4_target = np.array([x_tar4]).T 668 | y4_target = np.array([y_tar4]).T 669 | z4_target = np.array([z_tar4]).T 670 | now_xx4 = np.array([now_x4]).T 671 | now_yy4 = np.array([now_y4]).T 672 | now_zz4 = np.array([now_z4]).T 673 | vx4_target = np.array([vx_tar4]).T 674 | vy4_target = np.array([vy_tar4]).T 675 | now_vvvx = np.array([vvvx4]).T 676 | now_vvvy = np.array([vvvy4]).T 677 | t_list = np.array([t]).T 678 | control_uav1 = np.array([controlv4]).T 679 | # print x4_target 680 | uav4_data_save = np.concatenate((t_list, x4_target, y4_target, z4_target, now_xx4, now_yy4, now_zz4, 681 | vx4_target, vy4_target, now_vvvx, now_vvvy, control_uav1), axis=1) 682 | np.savetxt("/home/hlzy/chenan_wang/mulit-bebop_ws/uav4_data_"+time+".txt", uav4_data_save, fmt='%f', delimiter=', ') 683 | plt.savefig("/home/hlzy/chenan_wang/mulit-bebop_ws/uav4_"+time+".png") 684 | plt.close(fig) 685 | # plt.close(fig2) 686 | plt.close(fig3) 687 | break 688 | 689 | def a(self): 690 | t = threading.Thread(target=self.data_send, name='t') 691 | t.start() 692 | 693 | def data_stop(self): 694 | self.flag_send = 0 695 | 696 | def draw_save(self): 697 | self.flag_draw = 0 698 | 699 | def data_send(self): 700 | print 'Send Data !!!' 701 | self.flag_send = 1 702 | j = 0 703 | while True: 704 | if self.flag_send == 1: 705 | rospy.sleep(0.2) 706 | if self.flag1 == 1: 707 | topic_name = '/bebop1/cmd_vel' 708 | data_pub = rospy.Publisher(topic_name, Twist, queue_size=100) 709 | linear_x = float(self.linear_x.text()) 710 | linear_y = float(self.linear_y.text()) 711 | linear_z = float(self.linear_z.text()) 712 | twist = Twist() 713 | twist.linear.x = linear_x 714 | twist.linear.y = linear_y 715 | twist.linear.z = linear_z 716 | data_pub.publish(twist) 717 | 718 | if self.flag2 == 1: 719 | topic_name = '/bebop2/cmd_vel' 720 | data_pub = rospy.Publisher(topic_name, Twist, queue_size=100) 721 | x_target_2 = float(self.linear_x.text()) 722 | y_target_2 = float(self.linear_y.text()) 723 | z_target_2 = float(self.linear_z.text()) 724 | 725 | # x_target_2 = self.tb1_x - 300 726 | # y_target_2 = self.tb1_y + 500 727 | 728 | # x_target_2 = self.tb1_x # + 400 729 | # y_target_2 = self.tb1_y + 600 730 | 731 | x_last_err = self.uav2_x_err 732 | y_last_err = self.uav2_y_err 733 | # z_last_err = self.uav2_z_err 734 | self.uav2_x_err = x_target_2 - self.uav2_x 735 | self.uav2_y_err = y_target_2 - self.uav2_y 736 | self.uav2_z_err = z_target_2 - self.uav2_z 737 | sum_err = math.sqrt(math.pow(self.uav2_x_err, 2) + math.pow(self.uav2_y_err, 2)) 738 | 739 | linear_x = 0.0014 * self.uav2_x_err + 0.0012 * (self.uav2_x_err - x_last_err) 740 | linear_y = 0.0014 * self.uav2_y_err + 0.0012 * (self.uav2_y_err - y_last_err) 741 | linear_z = 0.0013 * self.uav4_z_err 742 | # linear_x = 0.0011 * self.uav2_x_err + 0.0011 * (self.uav2_x_err - x_last_err) 743 | # linear_y = 0.0011 * self.uav2_y_err + 0.0011 * (self.uav2_y_err - y_last_err) 744 | # print 0.005*(self.uav2_x_err - x_last_err), 0.005*(self.uav2_y_err - y_last_err) 745 | 746 | # if linear_x > 0.3: 747 | # linear_x = 0.3 748 | # if linear_x < -0.3: 749 | # linear_x = -0.3 750 | # if linear_y > 0.3: 751 | # linear_y = 0.3 752 | # if linear_y < -0.3: 753 | # linear_y = -0.3 754 | 755 | self.vvx_tar_2 = linear_x 756 | self.vvy_tar_2 = linear_y 757 | # linear_x = float(self.linear_x.text()) 758 | # linear_y = float(self.linear_y.text()) 759 | 760 | vx_last_err = self.uav2_vx_err 761 | vy_last_err = self.uav2_vy_err 762 | 763 | self.uav2_vx_err = linear_x - self.uav2_vx 764 | self.uav2_vy_err = linear_y - self.uav2_vy 765 | 766 | if sum_err <= 20: 767 | angle_x = 0 768 | angle_y = 0 769 | else: 770 | angle_x = -0.2 * self.uav2_vx_err + 0.012 * (self.uav2_vx_err - vx_last_err) 771 | angle_y = -0.2 * self.uav2_vy_err + 0.012 * (self.uav2_vx_err - vy_last_err) 772 | 773 | twist = Twist() 774 | twist.linear.x = angle_x 775 | twist.linear.y = angle_y 776 | twist.linear.z = linear_z 777 | 778 | if twist.linear.x > 0.3: 779 | twist.linear.x = 0.3 780 | if twist.linear.x < -0.3: 781 | twist.linear.x = -0.3 782 | if twist.linear.y > 0.3: 783 | twist.linear.y = 0.3 784 | if twist.linear.y < -0.3: 785 | twist.linear.y = -0.3 786 | if twist.linear.z > 0.1: 787 | twist.linear.z = 0.1 788 | if twist.linear.z < -0.1: 789 | twist.linear.z = -0.1 790 | 791 | if abs(self.uav2_x) >= 9999: 792 | twist.linear.x = 0 793 | twist.linear.y = 0 794 | 795 | self.control_vx_2 = twist.linear.x 796 | self.control_vy_2 = twist.linear.y 797 | # print 'Control pub: ', twist.linear.x, twist.linear.y 798 | data_pub.publish(twist) 799 | 800 | if self.flag3 == 1: 801 | topic_name = '/bebop3/cmd_vel' 802 | data_pub = rospy.Publisher(topic_name, Twist, queue_size=100) 803 | linear_x = float(self.linear_x.text()) 804 | linear_y = float(self.linear_y.text()) 805 | linear_z = float(self.linear_z.text()) 806 | twist = Twist() 807 | twist.linear.x = linear_x 808 | twist.linear.y = linear_y 809 | twist.linear.z = linear_z 810 | data_pub.publish(twist) 811 | 812 | if self.flag4 == 1: 813 | topic_name = '/bebop4/cmd_vel' 814 | data_pub = rospy.Publisher(topic_name, Twist, queue_size=100) 815 | 816 | self.theta_4 = j * 0.04 817 | self.x_target_4 = 600 * np.cos(self.theta_4) 818 | self.y_target_4 = 600 * np.sin(self.theta_4) 819 | self.z_target_4 = 500 + j 820 | j += 2.5 821 | 822 | # x_target_4 = float(self.linear_x.text()) 823 | # y_target_4 = float(self.linear_y.text()) 824 | # x_target_4 = float(self.linear_x.text()) - 600 825 | # y_target_4 = float(self.linear_y.text()) 826 | # z_target_4 = float(self.linear_z.text()) 827 | 828 | # x_target_4 = self.tb1_x # 400 829 | # y_target_4 = self.tb1_y - 600 830 | # print(x_target_4, y_target_4) 831 | 832 | x_last_err = self.uav4_x_err 833 | y_last_err = self.uav4_y_err 834 | z_last_err = self.uav4_z_err 835 | self.uav4_x_err = self.x_target_4 - self.uav4_x 836 | self.uav4_y_err = self.y_target_4 - self.uav4_y 837 | self.uav4_z_err = self.z_target_4 - self.uav4_z 838 | sum_err = math.sqrt(math.pow(self.uav4_x_err, 4) + math.pow(self.uav4_y_err, 4)) 839 | 840 | linear_x = 0.0013 * self.uav4_x_err + 0.0011 * (self.uav4_x_err - x_last_err) 841 | linear_y = 0.0013 * self.uav4_y_err + 0.0011 * (self.uav4_y_err - y_last_err) 842 | linear_z = 0.001 * self.uav4_z_err 843 | print(linear_z) 844 | 845 | # if linear_x > 0.3: 846 | # linear_x = 0.3 847 | # if linear_x < -0.3: 848 | # linear_x = -0.3 849 | # if linear_y > 0.3: 850 | # linear_y = 0.3 851 | # if linear_y < -0.3: 852 | # linear_y = -0.3 853 | 854 | self.vvx_tar_4 = linear_x 855 | self.vvy_tar_4 = linear_y 856 | 857 | vx_last_err = self.uav4_vx_err 858 | vy_last_err = self.uav4_vy_err 859 | 860 | self.uav4_vx_err = linear_x - self.uav4_vx 861 | self.uav4_vy_err = linear_y - self.uav4_vy 862 | 863 | if sum_err <= 30: 864 | angle_x = 0 865 | angle_y = 0 866 | else: 867 | angle_x = -0.22 * self.uav4_vx_err + 0.01 * (self.uav4_vx_err - vx_last_err) 868 | angle_y = -0.22 * self.uav4_vy_err + 0.01 * (self.uav4_vx_err - vy_last_err) 869 | 870 | twist = Twist() 871 | twist.linear.x = angle_x 872 | twist.linear.y = angle_y 873 | twist.linear.z = linear_z 874 | 875 | if twist.linear.x > 0.3: 876 | twist.linear.x = 0.3 877 | if twist.linear.x < -0.3: 878 | twist.linear.x = -0.3 879 | if twist.linear.y > 0.3: 880 | twist.linear.y = 0.3 881 | if twist.linear.y < -0.3: 882 | twist.linear.y = -0.3 883 | if twist.linear.z > 0.2: 884 | twist.linear.z = 0.2 885 | if twist.linear.z < -0.2: 886 | twist.linear.z = -0.2 887 | 888 | # if twist.linear.x > 0.08: 889 | # twist.linear.x = 0.08 890 | # if twist.linear.x < -0.08: 891 | # twist.linear.x = -0.08 892 | # if twist.linear.y > 0.08: 893 | # twist.linear.y = 0.08 894 | # if twist.linear.y < -0.08: 895 | # twist.linear.y = -0.08 896 | 897 | if self.uav4_x >= 9999: 898 | twist.linear.x = 0 899 | twist.linear.y = 0 900 | twist.linear.y = 0 901 | if self.uav4_x <= -9999: 902 | twist.linear.x = 0 903 | twist.linear.y = 0 904 | twist.linear.y = 0 905 | 906 | self.control_vx_4 = twist.linear.x 907 | self.control_vy_4 = twist.linear.y 908 | data_pub.publish(twist) 909 | 910 | else: 911 | break 912 | print("Stop !!!") 913 | 914 | def on_update(self): 915 | # render UI 916 | self.update() 917 | if rospy.is_shutdown(): 918 | self.close() 919 | --------------------------------------------------------------------------------