From b6147fd15e0510a62d6b5c54e613ef11e4ace6f7 Mon Sep 17 00:00:00 2001 From: Your Name Date: Sat, 19 Dec 2020 10:13:54 +0800 Subject: [PATCH] add ugvs --- communication/multi_vehicle_communication.sh | 4 +- control/gcs/receive.py | 2 +- control/gcs/send.py | 200 +++++++++++-------- control/gcs/xtd_ui.py | 172 ++++++++++++++-- 4 files changed, 278 insertions(+), 100 deletions(-) diff --git a/communication/multi_vehicle_communication.sh b/communication/multi_vehicle_communication.sh index 9638416..2e33c4c 100644 --- a/communication/multi_vehicle_communication.sh +++ b/communication/multi_vehicle_communication.sh @@ -1,6 +1,6 @@ #!/bin/bash -iris_num=0 -typhoon_h480_num=6 +iris_num=4 +typhoon_h480_num=2 solo_num=0 plane_num=0 rover_num=0 diff --git a/control/gcs/receive.py b/control/gcs/receive.py index b87a1a7..fd246a5 100644 --- a/control/gcs/receive.py +++ b/control/gcs/receive.py @@ -12,7 +12,7 @@ class Ros2Gui(QThread): plot_array = pyqtSignal(list) def __init__(self, multi_select, multi_num, multi_type): super(Ros2Gui, self).__init__() - # rospy.init_node('multirotor_pyqt5_receiver') + rospy.init_node('multirotor_pyqt5_receiver') self.multirotor_num = 0 self.multirotor_type = multi_type self.map = map diff --git a/control/gcs/send.py b/control/gcs/send.py index 3eed65f..7b11665 100644 --- a/control/gcs/send.py +++ b/control/gcs/send.py @@ -21,15 +21,12 @@ class Gui2Ros(QMainWindow,xtd_ui.Ui_MainWindow): self.map = 'indoor1' self.comboBox_maps.currentIndexChanged.connect(self.initplot) self.button_run.clicked.connect(self.startrun) - self.button_control.clicked.connect(self.startcontrol) - self.cmd = '' - self.ctrl_leader = True - self.cmd_vel_mask = False self.close_flag = False self.local_pose = PoseStamped() self.local_vel = Twist() self.m = PlotCanvas(self, self.map) self.m.move(180, 0) + self.flag = 0 # rospy.init_node('multirotor_pyqt5_control') def initplot(self): @@ -39,22 +36,17 @@ class Gui2Ros(QMainWindow,xtd_ui.Ui_MainWindow): def startrun(self): print 'start run!' self.init_controller() - + self.pSend2ros = Process(target=self.run_process) + self.pSend2ros.start() self.text_thread = Ros2Gui(self.multirotor_select, self.multirotor_num, self.multi_type) self.text_thread.update_text.connect(self.display) self.text_thread.plot_array.connect(self.plot) self.text_thread.start() # self.pSend2ros = Process(target=self.run_process) # self.pSend2ros.start() - - - def startcontrol(self): - print 'start control!' - self.pSend2ros = Process(target=self.run_process) - self.pSend2ros.start() def init_controller(self): - rospy.init_node('multirotor_pyqt5_control') + self.text_show_info.setPlainText('data') self.multi_num = 0 self.multi_type = [] @@ -72,18 +64,19 @@ class Gui2Ros(QMainWindow,xtd_ui.Ui_MainWindow): color_B = hex(random.randint(16,255)) self.color_plot[i] = '#'+str(color_R)+str(color_G)+str(color_B) self.color_plot[i] = self.color_plot[i].replace('0x','') + + + #publish messages to ros nodes like a keyboard + def run_process(self): + rospy.init_node('multirotor_pyqt5_control') counnnt = 0 if self.control_type == 'vel': self.multi_cmd_vel_flu_pub = [None] * self.multi_num self.multi_cmd_pub = [None] * self.multi_num for i in self.multirotor_select: for k in range(self.multirotor_num[i]): - self.multi_cmd_vel_flu_pub[counnnt] = rospy.Publisher( - '/xtdrone/' + self.multi_type[counnnt] + '_' + str(k) + '/cmd_vel_flu', - Twist, queue_size=10) - self.multi_cmd_pub[i] = rospy.Publisher('/xtdrone/' + self.multi_type[counnnt] + '_' + str(k) + '/cmd', - String, - queue_size=10) + self.multi_cmd_vel_flu_pub[counnnt] = rospy.Publisher('/xtdrone/' + self.multi_type[counnnt] + '_' + str(k) + '/cmd_vel_flu', Twist, queue_size=10) + self.multi_cmd_pub[counnnt] = rospy.Publisher('/xtdrone/' + self.multi_type[counnnt] + '_' + str(k) + '/cmd', String,queue_size=10) counnnt += 1 self.leader_cmd_vel_flu_pub = rospy.Publisher("/xtdrone/leader/cmd_vel_flu", Twist, queue_size=10) self.leader_cmd_pub = rospy.Publisher("/xtdrone/leader/cmd", String, queue_size=10) @@ -101,79 +94,124 @@ class Gui2Ros(QMainWindow,xtd_ui.Ui_MainWindow): counnnt = 0 self.leader_cmd_accel_flu_pub = rospy.Publisher("/xtdrone/leader/cmd_accel_flu", Twist, queue_size=10) self.leader_cmd_pub = rospy.Publisher("/xtdrone/leader/cmd", String, queue_size=10) - - #publish messages to ros nodes like a keyboard - def run_process(self): - # rospy.init_node('multirotor_pyqt5_control') - last_forward = 0.0 - last_upward = 0.0 - last_leftward = 0.0 - last_orientation = 0.0 + self.twist = [Twist() for i in range (self.multi_num)] + self.cmd = ['' for i in range (self.multi_num)] + self.ctrl_leader = True + self.cmd_vel_mask = False + for j in range(self.multi_num): + self.twist[j].angular.x = 0.0 + self.twist[j].angular.y = 0.0 + last_forward = [0.0 for i in range(self.multi_num)] + last_upward = [0.0 for i in range(self.multi_num)] + last_leftward = [0.0 for i in range(self.multi_num)] + last_orientation = [0.0 for i in range(self.multi_num)] last_ctrl_leader = False last_cmd_vel_mask = False + last_multirotor_get_control = [0 for i in range(self.multi_num)] + last_forward_all = 0.0 + last_upward_all = 0.0 + last_leftward_all = 0.0 + last_orientation_all = 0.0 + num = 0 rate = rospy.Rate(30) check_stop_flag = False print('StartRun!') + start_flag = False + flag = False + time = 0 while True: - num += 1 - #print(check_stop_flag) - if self.q_forward.empty(): - self.twist.linear.x = last_forward - else: - self.twist.linear.x = self.q_forward.get() - last_forward = self.twist.linear.x - if self.q_upward.empty(): - self.twist.linear.z = last_upward - else: - self.twist.linear.z = self.q_upward.get() - last_upward = self.twist.linear.z - if self.q_leftward.empty(): - self.twist.linear.y = last_leftward - else: - self.twist.linear.y = self.q_leftward.get() - last_leftward = self.twist.linear.y - if self.q_orientation.empty(): - self.twist.angular.z = last_orientation - else: - self.twist.angular.z = self.q_orientation.get() - last_orientation = self.twist.angular.z - if self.q_cmd.empty(): - self.cmd = '' - else: - self.cmd = self.q_cmd.get() - if self.q_ctrl_leader.empty(): - self.ctrl_leader = last_ctrl_leader - else: - self.ctrl_leader = self.q_ctrl_leader.get() - last_ctrl_leader = self.ctrl_leader - if self.q_cmd_vel_mask.empty(): - self.cmd_vel_mask = last_cmd_vel_mask - else: - self.cmd_vel_mask = self.q_cmd_vel_mask.get() - last_cmd_vel_mask = self.cmd_vel_mask - if self.q_stop_flag.empty(): - pass - else: - check_stop_flag = self.q_stop_flag.get() - if check_stop_flag: - self.cmd = 'AUTO.RTL' - for i in range(self.multi_num): + if not start_flag: + flag = self.q_start_control_flag.get() + if flag: + time += 1 + start_flag = True + num += 1 + if self.q_multirotor_get_control.empty(): + multirotor_get_control = last_multirotor_get_control + else: + multirotor_get_control = self.q_multirotor_get_control.get() + last_multirotor_get_control = multirotor_get_control + if self.q_forward.empty(): + for i in range(self.multi_num): + if multirotor_get_control[i]: + self.twist[i].linear.x = last_forward[i] + else: + for i in range(self.multi_num): + if multirotor_get_control[i]: + self.twist[i].linear.x = self.q_forward.get() + last_forward[i] = self.twist.linear.x[i] + if self.q_upward.empty(): + for i in range(self.multi_num): + if multirotor_get_control[i]: + self.twist[i].linear.z = last_upward[i] + else: + for i in range(self.multi_num): + if multirotor_get_control[i]: + self.twist[i].linear.z = self.q_upward.get() + last_upward[i] = self.twist[i].linear.z + if self.q_leftward.empty(): + for i in range(self.multi_num): + if multirotor_get_control[i]: + self.twist[i].linear.y = last_leftward[i] + else: + for i in range(self.multi_num): + if multirotor_get_control[i]: + self.twist[i].linear.y = self.q_leftward.get() + last_leftward[i] = self.twist[i].linear.y + if self.q_orientation.empty(): + for i in range(self.multi_num): + if multirotor_get_control[i]: + self.twist[i].angular.z = last_orientation[i] + else: + for i in range(self.multi_num): + if multirotor_get_control[i]: + self.twist[i].angular.z = self.q_orientation.get() + last_orientation[i] = self.twist[i].angular.z + if self.q_cmd.empty(): + for i in range(self.multi_num): + if multirotor_get_control[i]: + self.cmd[i] = '' + else: + for i in range(self.multi_num): + if multirotor_get_control[i]: + self.cmd[i] = self.q_cmd.get() + if self.q_ctrl_leader.empty(): + self.ctrl_leader = last_ctrl_leader + else: + self.ctrl_leader = self.q_ctrl_leader.get() + last_ctrl_leader = self.ctrl_leader + + if self.q_cmd_vel_mask.empty(): + self.cmd_vel_mask = last_cmd_vel_mask + else: + self.cmd_vel_mask = self.q_cmd_vel_mask.get() + last_cmd_vel_mask = self.cmd_vel_mask + if self.q_stop_flag.empty(): + pass + else: + check_stop_flag = self.q_stop_flag.get() + if check_stop_flag: + for i in range(self.multi_num): + self.cmd[i] = 'AUTO.RTL' if self.ctrl_leader: - if self.control_type == 'vel': - self.leader_cmd_vel_flu_pub.publish(self.twist) - else: - self.leader_cmd_accel_flu_pub.publish(self.twist) - self.leader_cmd_pub.publish(self.cmd) + for i in range(self.multi_num): + if multirotor_get_control[i]: + if self.control_type == 'vel': + self.leader_cmd_vel_flu_pub.publish(self.twist[i]) + else: + self.leader_cmd_accel_flu_pub.publish(self.twist[i]) + self.leader_cmd_pub.publish(self.cmd[i]) + break else: - if not self.cmd_vel_mask: - if self.control_type == 'vel': - self.multi_cmd_vel_flu_pub[i].publish(self.twist) - print self.multi_cmd_pub[i] - else: - self.multi_cmd_accel_flu_pub[i].publish(self.twist) - self.multi_cmd_pub[i].publish(self.cmd) + for i in range(self.multi_num): + if not self.cmd_vel_mask: + if self.control_type == 'vel': + self.multi_cmd_vel_flu_pub[i].publish(self.twist[i]) + else: + self.multi_cmd_accel_flu_pub[i].publish(self.twist[i]) + self.multi_cmd_pub[i].publish(self.cmd[i]) rate.sleep() if check_stop_flag: self.q_stop_flag.put(True) diff --git a/control/gcs/xtd_ui.py b/control/gcs/xtd_ui.py index 138127b..c6b2631 100644 --- a/control/gcs/xtd_ui.py +++ b/control/gcs/xtd_ui.py @@ -18,8 +18,10 @@ from plotcanvas import PlotCanvas class Ui_MainWindow(object): def __init__(self): self.multirotor_type = ['iris','typhoon_h480','solo','tailsitter','quadplane','plane','tiltrotor','rotor'] - self.multirotor_num = [1, 0, 0, 0, 0, 0, 0] + self.multirotor_num = [1, 0, 0, 0, 0, 0, 0, 0] + self.multi_type_num = 8 self.multirotor_select=[] + self.multirotor_get_control = [] self.control_type = 'vel' self.task = 'Flying' self.map = 'indoor1' @@ -27,10 +29,7 @@ class Ui_MainWindow(object): self.terminal_point = numpy.array([0.0,0.0]) self.formation2 = 'formation1' self.control_type = 'vel' - self.twist = Twist() - self.twist.angular.x = 0.0 - self.twist.angular.y = 0.0 - self.cmd2 = 'control the leader' + self.cmd2 = 'control all' self.q_cmd = Queue() self.q_ctrl_leader = Queue() self.q_forward = Queue() @@ -40,6 +39,8 @@ class Ui_MainWindow(object): self.q_stop_flag = Queue() self.q_cmd_vel_mask = Queue() self.q_close_flag = Queue() + self.q_start_control_flag = Queue() + self.q_multirotor_get_control = Queue() self.once = 0 self.twice = 0 @@ -77,7 +78,7 @@ class Ui_MainWindow(object): self.label_control_board_2.setAlignment(QtCore.Qt.AlignCenter) self.label_control_board_2.setObjectName("label_control_board_2") self.layoutWidget = QtWidgets.QWidget(self.frame) - self.layoutWidget.setGeometry(QtCore.QRect(40, 60, 331, 198)) + self.layoutWidget.setGeometry(QtCore.QRect(180, 60, 331, 198)) self.layoutWidget.setObjectName("layoutWidget") self.gridLayout = QtWidgets.QGridLayout(self.layoutWidget) self.gridLayout.setContentsMargins(0, 0, 0, 0) @@ -199,7 +200,7 @@ class Ui_MainWindow(object): self.text_control_tips.setObjectName("text_control_tips") self.gridLayout.addWidget(self.text_control_tips, 2, 0, 1, 2) self.layoutWidget1 = QtWidgets.QWidget(self.frame) - self.layoutWidget1.setGeometry(QtCore.QRect(430, 60, 234, 191)) + self.layoutWidget1.setGeometry(QtCore.QRect(530, 60, 171, 191)) self.layoutWidget1.setObjectName("layoutWidget1") self.gridLayout_4 = QtWidgets.QGridLayout(self.layoutWidget1) self.gridLayout_4.setContentsMargins(0, 0, 0, 0) @@ -285,7 +286,53 @@ class Ui_MainWindow(object): self.button_stop.setObjectName("button_stop") self.horizontalLayout_3.addWidget(self.button_stop) self.verticalLayout_7.addLayout(self.horizontalLayout_3) - self.gridLayout_4.addLayout(self.verticalLayout_7, 1, 0, 1, 1) + self.gridLayout_4.addLayout(self.verticalLayout_7, 0, 0, 1, 1) + self.widget = QtWidgets.QWidget(self.frame) + self.widget.setGeometry(QtCore.QRect(20, 60, 151, 204)) + self.widget.setObjectName("widget") + self.verticalLayout_8 = QtWidgets.QVBoxLayout(self.widget) + self.verticalLayout_8.setContentsMargins(0, 0, 0, 0) + self.verticalLayout_8.setObjectName("verticalLayout_8") + self.checkBox_iris = QtWidgets.QCheckBox(self.widget) + self.checkBox_iris.setStyleSheet("font: 57 12pt \"Ubuntu\";\n" +"color: rgb(32, 74, 135);") + self.checkBox_iris.setObjectName("checkBox_iris") + self.verticalLayout_8.addWidget(self.checkBox_iris) + self.checkBox_typhoon = QtWidgets.QCheckBox(self.widget) + self.checkBox_typhoon.setStyleSheet("font: 57 12pt \"Ubuntu\";\n" +"color: rgb(32, 74, 135);") + self.checkBox_typhoon.setObjectName("checkBox_typhoon") + self.verticalLayout_8.addWidget(self.checkBox_typhoon) + self.checkBox_solo = QtWidgets.QCheckBox(self.widget) + self.checkBox_solo.setStyleSheet("font: 57 12pt \"Ubuntu\";\n" +"color: rgb(32, 74, 135);") + self.checkBox_solo.setObjectName("checkBox_solo") + self.verticalLayout_8.addWidget(self.checkBox_solo) + self.checkBox_tailsitter = QtWidgets.QCheckBox(self.widget) + self.checkBox_tailsitter.setStyleSheet("font: 57 12pt \"Ubuntu\";\n" +"color: rgb(32, 74, 135);") + self.checkBox_tailsitter.setObjectName("checkBox_tailsitter") + self.verticalLayout_8.addWidget(self.checkBox_tailsitter) + self.checkBox_quadplane = QtWidgets.QCheckBox(self.widget) + self.checkBox_quadplane.setStyleSheet("font: 57 12pt \"Ubuntu\";\n" +"color: rgb(32, 74, 135);") + self.checkBox_quadplane.setObjectName("checkBox_quadplane") + self.verticalLayout_8.addWidget(self.checkBox_quadplane) + self.checkBox_plane = QtWidgets.QCheckBox(self.widget) + self.checkBox_plane.setStyleSheet("font: 57 12pt \"Ubuntu\";\n" +"color: rgb(32, 74, 135);") + self.checkBox_plane.setObjectName("checkBox_plane") + self.verticalLayout_8.addWidget(self.checkBox_plane) + self.checkBox_tiltrotor = QtWidgets.QCheckBox(self.widget) + self.checkBox_tiltrotor.setStyleSheet("font: 57 12pt \"Ubuntu\";\n" +"color: rgb(32, 74, 135);") + self.checkBox_tiltrotor.setObjectName("checkBox_tiltrotor") + self.verticalLayout_8.addWidget(self.checkBox_tiltrotor) + self.checkBox_rotor = QtWidgets.QCheckBox(self.widget) + self.checkBox_rotor.setStyleSheet("font: 57 12pt \"Ubuntu\";\n" +"color: rgb(32, 74, 135);") + self.checkBox_rotor.setObjectName("checkBox_rotor") + self.verticalLayout_8.addWidget(self.checkBox_rotor) self.frame_3 = QtWidgets.QFrame(self.centralwidget) self.frame_3.setGeometry(QtCore.QRect(170, 0, 711, 470)) self.frame_3.setStyleSheet("background-color: rgb(238, 238, 236);") @@ -340,7 +387,7 @@ class Ui_MainWindow(object): self.frame_5.setFrameShadow(QtWidgets.QFrame.Raised) self.frame_5.setObjectName("frame_5") self.comboBox_controltype = QtWidgets.QComboBox(self.frame_5) - self.comboBox_controltype.setGeometry(QtCore.QRect(30, 700, 101, 25)) + self.comboBox_controltype.setGeometry(QtCore.QRect(30, 690, 101, 25)) self.comboBox_controltype.setStyleSheet("color: rgb(32, 74, 135);\n" "font: 57 12pt \"Ubuntu\";") self.comboBox_controltype.setObjectName("comboBox_controltype") @@ -348,7 +395,7 @@ class Ui_MainWindow(object): self.comboBox_controltype.addItem("") self.label_uavnumber_2 = QtWidgets.QLabel(self.frame_5) self.label_uavnumber_2.setEnabled(True) - self.label_uavnumber_2.setGeometry(QtCore.QRect(10, 670, 101, 19)) + self.label_uavnumber_2.setGeometry(QtCore.QRect(10, 660, 101, 19)) sizePolicy = QtWidgets.QSizePolicy(QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Fixed) sizePolicy.setHorizontalStretch(1) sizePolicy.setVerticalStretch(0) @@ -730,7 +777,7 @@ class Ui_MainWindow(object): self.box_command.setItemText(5, _translate("MainWindow", "take off")) self.box_command.setItemText(6, _translate("MainWindow", "land")) self.box_command.setItemText(7, _translate("MainWindow", "offboard")) - self.box_command.setItemText(8, _translate("MainWindow", "hover and remove the mask")) + self.box_command.setItemText(8, _translate("MainWindow", "hover")) self.label_choose_formation.setText(_translate("MainWindow", "choose formation")) self.box_formation.setItemText(0, _translate("MainWindow", "waiting")) self.box_formation.setItemText(1, _translate("MainWindow", "formation1")) @@ -738,6 +785,14 @@ class Ui_MainWindow(object): self.box_formation.setItemText(3, _translate("MainWindow", "formation3")) self.button_control.setText(_translate("MainWindow", "CONTROL")) self.button_stop.setText(_translate("MainWindow", "STOP!")) + self.checkBox_iris.setText(_translate("MainWindow", "iris")) + self.checkBox_typhoon.setText(_translate("MainWindow", "typhoon_h480")) + self.checkBox_solo.setText(_translate("MainWindow", "solo")) + self.checkBox_tailsitter.setText(_translate("MainWindow", "tailsitter")) + self.checkBox_quadplane.setText(_translate("MainWindow", "quadplane")) + self.checkBox_plane.setText(_translate("MainWindow", "plane")) + self.checkBox_tiltrotor.setText(_translate("MainWindow", "tiltrotor")) + self.checkBox_rotor.setText(_translate("MainWindow", "rotor")) self.label_control_board_4.setText(_translate("MainWindow", "vehicle states")) self.more_topics.setText(_translate("MainWindow", "more topics")) self.label_website.setToolTip(_translate("MainWindow", "XTDrone wiki")) @@ -797,6 +852,22 @@ class Ui_MainWindow(object): self.box_command.currentIndexChanged.connect(self.get_command) self.box_formation.currentIndexChanged.connect(self.get_formation) self.comboBox_maps.currentIndexChanged.connect(self.initplot) + self.checkBox_iris.setCheckable(False) + self.checkBox_iris.stateChanged.connect(self.get_uav_control) + self.checkBox_typhoon.setCheckable(False) + self.checkBox_typhoon.stateChanged.connect(self.get_uav_control) + self.checkBox_solo.setCheckable(False) + self.checkBox_solo.stateChanged.connect(self.get_uav_control) + self.checkBox_tailsitter.setCheckable(False) + self.checkBox_tailsitter.stateChanged.connect(self.get_uav_control) + self.checkBox_plane.setCheckable(False) + self.checkBox_plane.stateChanged.connect(self.get_uav_control) + self.checkBox_tiltrotor.setCheckable(False) + self.checkBox_tiltrotor.stateChanged.connect(self.get_uav_control) + self.checkBox_rotor.setCheckable(False) + self.checkBox_rotor.stateChanged.connect(self.get_uav_control) + self.checkBox_quadplane.setCheckable(False) + self.checkBox_quadplane.stateChanged.connect(self.get_uav_control) def initplot(self): self.map = self.comboBox_maps.currentText() @@ -815,33 +886,58 @@ class Ui_MainWindow(object): self.once += 1 self.map = str(self.comboBox_maps.currentText()) if self.once == 1: - for i in range (7): + for i in range (self.multi_type_num): if i == 0: self.multirotor_num[i] = int(self.box_iris_num.value()) + if self.multirotor_num[i] > 0.5: + self.multirotor_select.append(i) + self.checkBox_iris.setCheckable(True) elif i == 1: self.multirotor_num[i] = int(self.box_typhoon_num.value()) + if self.multirotor_num[i] > 0.5: + self.multirotor_select.append(i) + self.checkBox_typhoon.setCheckable(True) elif i == 2: self.multirotor_num[i] = int(self.box_solo_num.value()) + if self.multirotor_num[i] > 0.5: + self.multirotor_select.append(i) + self.checkBox_solo.setCheckable(True) elif i == 3: self.multirotor_num[i] = int(self.box_tailsitter_num.value()) + if self.multirotor_num[i] > 0.5: + self.multirotor_select.append(i) + self.checkBox_tailsitter.setCheckable(True) elif i == 4: self.multirotor_num[i] = int(self.box_quadplane_num.value()) + if self.multirotor_num[i] > 0.5: + self.multirotor_select.append(i) + self.checkBox_quadplane.setCheckable(True) elif i == 5: self.multirotor_num[i] = int(self.box_plane_num.value()) + if self.multirotor_num[i] > 0.5: + self.multirotor_select.append(i) + self.checkBox_plane.setCheckable(True) elif i == 6: self.multirotor_num[i] = int(self.box_tiltrotor_num.value()) + if self.multirotor_num[i] > 0.5: + self.multirotor_select.append(i) + self.checkBox_tiltrotor.setCheckable(True) else: self.multirotor_num[i] = int(self.box_rotor_num.value()) + if self.multirotor_num[i] > 0.5: + self.multirotor_select.append(i) + self.checkBox_rotor.setCheckable(True) if self.multirotor_num[i] != 6 and self.multirotor_num[i] != 9 and self.multirotor_num[i] != 18: self.box_formation.setVisible(False) - if self.multirotor_num[i] > 0.5: - self.multirotor_select.append(i) + self.control_type = str(self.comboBox_controltype.currentText()) self.button_control.setEnabled(True) self.button_run.setEnabled(False) def on_button_control_clicked(self): self.twice += 1 + print 'Start Control!' + self.q_start_control_flag.put(True) if self.twice == 1: self.box_go_and_back_2.setReadOnly(False) self.box_up_and_down_2.setReadOnly(False) @@ -890,8 +986,6 @@ class Ui_MainWindow(object): self.q_orientation.put(orientation_exp ) #print('orientation_exp:',self.twist.angular.z) - - def get_command(self): self.cmd2 = str(self.box_command.currentText()) print('cmd2:' + self.cmd2) @@ -979,6 +1073,52 @@ class Ui_MainWindow(object): self.q_cmd.put(formation) #print(self.formation) + def get_uav_control(self): + self.multirotor_get_control = [] + for i in range(self.multirotor_num[0]): + if self.checkBox_iris.isChecked(): + self.multirotor_get_control.append(1) + else: + self.multirotor_get_control.append(0) + for i in range(self.multirotor_num[1]): + if self.checkBox_typhoon.isChecked(): + self.multirotor_get_control.append(1) + else: + self.multirotor_get_control.append(0) + for i in range(self.multirotor_num[2]): + if self.checkBox_solo.isChecked(): + self.multirotor_get_control.append(1) + else: + self.multirotor_get_control.append(0) + for i in range(self.multirotor_num[3]): + if self.checkBox_tailsitter.isChecked(): + self.multirotor_get_control.append(1) + else: + self.multirotor_get_control.append(0) + for i in range(self.multirotor_num[4]): + if self.checkBox_quadplane.isChecked(): + self.multirotor_get_control.append(1) + else: + self.multirotor_get_control.append(0) + for i in range(self.multirotor_num[5]): + if self.checkBox_plane.isChecked(): + self.multirotor_get_control.append(1) + else: + self.multirotor_get_control.append(0) + for i in range(self.multirotor_num[6]): + if self.checkBox_tiltrotor.isChecked(): + self.multirotor_get_control.append(1) + else: + self.multirotor_get_control.append(0) + for i in range(self.multirotor_num[7]): + if self.checkBox_rotor.isChecked(): + self.multirotor_get_control.append(1) + else: + self.multirotor_get_control.append(0) + print self.multirotor_get_control + self.q_multirotor_get_control.put(self.multirotor_get_control) + + if __name__== '__main__': app = QApplication(sys.argv)