modifu gcs

communication/multi_vehicle_communication.sh

@@ -1,6 +1,6 @@
 #!/bin/bash
-iris_num=18
-typhoon_h480_num=0
+iris_num=0
+typhoon_h480_num=6
 solo_num=0
 plane_num=0
 rover_num=0

control/gcs/images/resize.py

@@ -0,0 +1,5 @@
+from PIL import Image
+
+im = Image.open('indoor1.jpg')
+imBackground = im.resize((875, 587))  #700 470 840 564
+imBackground.save('indoor1_resize.jpg')

control/gcs/plotcanvas.py

@@ -0,0 +1,54 @@
+from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
+from matplotlib.figure import Figure
+import matplotlib.pyplot as plt
+from PIL import Image
+from PyQt5.QtWidgets import QApplication, QMainWindow, QSizePolicy
+
+class PlotCanvas(FigureCanvas):
+
+    def __init__(self, parent=None, Map='indoor1'):
+        self.fig, self.ax = plt.subplots()
+        self.Image_PreProcessing(Map)
+        height, width, channels = self.imBackground.shape
+        self.fig.set_size_inches(width/100.0, height/100.0)
+        plt.gca().xaxis.set_major_locator(plt.NullLocator())
+        plt.gca().yaxis.set_major_locator(plt.NullLocator())
+        plt.subplots_adjust(top=1, bottom=0, left=0, right=1, hspace=0, wspace=0)
+        plt.margins(0, 0)
+        plt.axis('off')
+        FigureCanvas.__init__(self, self.fig)
+        self.setParent(parent)
+
+        FigureCanvas.setSizePolicy(self,  QSizePolicy.Expanding, QSizePolicy.Expanding)
+        FigureCanvas.updateGeometry(self)
+
+    def canvas_update(self, Map):
+        self.Image_PreProcessing(Map)
+        # img = plt.imread('./images/open.jpg')
+        # self.ax.imshow(img, extent=[-50, 100, -50, 50])
+        self.draw()
+
+    def Image_PreProcessing(self,Map):
+        if Map == 'robocup':
+            self.imBackground = plt.imread('./images/robocup_resize.jpg')  # image size:875*587
+            self.ax.imshow(self.imBackground, extent=[-50, 150, -67.09, 67.09])  #time: 875/150+50
+        elif Map == 'indoor1':
+            self.imBackground = plt.imread('./images/indoor1_resize.jpg')
+            self.ax.imshow(self.imBackground, extent=[-16.89, 18.89, -14, 10])  #[-14, 10, -8.9, 6.9]
+        elif Map == 'indoor2':
+            self.imBackground = plt.imread('./images/indoor2_resize.jpg')
+            self.ax.imshow(self.imBackground, extent=[-9.925, 13.925, -6, 10])
+        elif Map == 'indoor3':
+            self.imBackground = plt.imread('./images/indoor3_resize.jpg')
+            self.ax.imshow(self.imBackground, extent=[-11.16, 17.16, -3, 16])
+        elif Map == 'outdoor1':
+            self.imBackground = plt.imread('./images/outdoor1_resize.jpg')
+            self.ax.imshow(self.imBackground, extent=[-50, 150, -67.09, 67.09])
+        elif Map == 'outdoor2':
+            self.imBackground = plt.imread('./images/outdoor2_resize.jpg')
+            self.ax.imshow(self.imBackground, extent=[-2235, 2235, -1500, 1500])
+        elif Map == 'outdoor3':
+            self.imBackground = plt.imread('./images/outdoor3_resize.jpg')
+            self.ax.imshow(self.imBackground, extent=[-1129.5, 465.5, -380, 690])
+        
+        

control/gcs/receive.py

@@ -4,6 +4,7 @@ import rospy
 from geometry_msgs.msg import PoseStamped, TwistStamped, Twist
 from std_msgs.msg import String
 from multiprocessing import Process,Queue
+from nav_msgs.msg import Odometry
 
 
 class Ros2Gui(QThread):
@@ -14,21 +15,21 @@ class Ros2Gui(QThread):
         rospy.init_node('multirotor_pyqt5_receiver')
         self.multirotor_num = multi_num
         self.multirotor_type = multi_type
-        self.local_pose_sub = [None] * self.multirotor_num
+        self.map = map
+        self.multi_odom_groundtruth_sub = [None] * self.multirotor_num
         self.local_vel_sub = [None] * self.multirotor_num
         self.leader_cmd_vel_sub = [None]
-        self.local_pose = [None] * self.multirotor_num
-        self.local_vel = [None] * self.multirotor_num
         self.local_pose = [PoseStamped() for i in range(self.multirotor_num)]
         self.local_vel = [TwistStamped() for i in range(self.multirotor_num)]
-        self.plot_all = [[[0], [0]]for i in range(self.multirotor_num)]
+        self.plot_all = [[[], []]for i in range(self.multirotor_num)]
         self.n = 0
         self.count = 0
         self.vel = Twist()
+        self.time_map_x = 1
+        self.time_map_y = 875/587
         #self.q_flag = flag
         for id_1 in range(self.multirotor_num):
-            self.local_pose_sub[id_1] = rospy.Subscriber(
-                self.multirotor_type + '_' + str(id_1) + "/mavros/local_position/pose", PoseStamped, self.local_pose_callback, id_1)
+            self.multi_odom_groundtruth_sub[id_1] = rospy.Subscriber('/xtdrone/'+self.multirotor_type+'_'+str(id_1)+'/ground_truth/odom', Odometry, self.odm_groundtruth_callback, id_1)
             self.local_vel_sub[id_1] = rospy.Subscriber(
                 self.multirotor_type + '_' + str(id_1) + "/mavros/local_position/velocity_body", TwistStamped, self.local_vel_callback, id_1)
 
@@ -40,7 +41,7 @@ class Ros2Gui(QThread):
             self.n = self.n+1
             self.text_all = ''
             self.text = [' ' for i in range(self.multirotor_num)]
-            if self.n % 10 == 0:
+            if self.n % 30 == 0:
                 self.count = self.count+1
                 if self.multirotor_num < 10:
                     for id in range(self.multirotor_num):
@@ -49,9 +50,9 @@ class Ros2Gui(QThread):
                                         +"%s"%(self.local_pose[id].pose.position.y)+'    '+'z:'+"%s"%(self.local_pose[id].pose.position.z)\
                                         +'\n'+'uav vel:\n'+'uav'+str(id)+':\n'+'x:'+"%s"%(self.local_vel[id].twist.linear.x)+'    '+'y:'\
                                         +"%s"%(self.local_vel[id].twist.linear.y)+'    '+'z:'+"%s"%(self.local_vel[id].twist.linear.z)+'\n'
-                        self.text_all += self.text_all + self.text[id]
-                        self.plot_all[id][0].append(self.local_pose[id].pose.position.x)
-                        self.plot_all[id][1].append(self.local_pose[id].pose.position.y)
+                        self.text_all = self.text_all + self.text[id]
+                        self.plot_all[id][0].append(self.local_pose[id].pose.position.x*self.time_map_x)
+                        self.plot_all[id][1].append(self.local_pose[id].pose.position.y*self.time_map_y)
                     self.plot_array.emit(self.plot_all)
                     self.update_text.emit(self.text_all)
             if self.n % 50 == 0:
@@ -60,8 +61,8 @@ class Ros2Gui(QThread):
 
             rate.sleep()
 
-    def local_pose_callback(self, msg, id_1):
-        self.local_pose[id_1] = msg
+    def odm_groundtruth_callback(self, msg, i):
+        self.local_pose[i] = msg.pose
 
     def local_vel_callback(self, msg, id_1):
         self.local_vel[id_1] = msg

control/gcs/resize.py

@@ -0,0 +1,5 @@
+from PIL import Image
+
+im = Image.open('robocup.jpg')
+imBackground = im.resize((700, 470))  #700 470 840 564
+imBackground.save('outdoor1_resize.jpg')

control/gcs/send.py

@@ -7,6 +7,7 @@ from multiprocessing import Process,Queue
 from PyQt5.QtCore import *
 from receive import Ros2Gui
 from PIL import Image
+import random
 
 from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
 from matplotlib.figure import Figure
@@ -28,22 +29,26 @@ class Gui2Ros(QMainWindow,xt_ui.Ui_MainWindow):
         self.local_vel = Twist()
         self.m = PlotCanvas(self, self.map)
         self.m.move(180, 0)
-        self.x = [[]for i in range(self.multirotor_num)]
-        self.y = [[] for i in range(self.multirotor_num)]
 
     def initplot(self):
         self.map = self.comboBox_maps.currentText()
         self.m.canvas_update(self.map)
 
-
     def startrun(self):
         print 'start run!'
+        self.color_plot = ['' for i in range(self.multirotor_num)]
+        for i in range(self.multirotor_num):
+            color_R = hex(random.randint(16,255))
+            color_G = hex(random.randint(16,255))
+            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','')
+            print self.color_plot[i]
         self.pSend2ros = Process(target=self.run_process)
         self.pSend2ros.start()
         self.text_thread = Ros2Gui(self.multirotor_num, self.multirotor_type)
         self.text_thread.update_text.connect(self.display)
-        if self.map == 'robocup':
-            self.text_thread.plot_array.connect(self.plot)
+        self.text_thread.plot_array.connect(self.plot)
         self.text_thread.start()
 
     #publish messages to ros nodes like a keyboard
@@ -152,10 +157,9 @@ class Gui2Ros(QMainWindow,xt_ui.Ui_MainWindow):
         self.text_show_info.setPlainText(data)
 
     def plot(self, data):
-        for i in range(self.multirotor_num):
-            self.x[i] = data[i][0]
-            self.y[i] = data[i][1]
-            self.m.ax.plot(self.x[i], self.y[i], 'r')
+         for i in range(self.multirotor_num):
+            self.m.ax.plot(data[i][0], data[i][1], color = self.color_plot[i])
+            # self.m.canvas_update(self.map)
             self.m.draw()
 
 

control/gcs/xt_ui.py

@@ -59,7 +59,7 @@ class Ui_MainWindow(object):
         self.centralwidget = QtWidgets.QWidget(MainWindow)
         self.centralwidget.setObjectName("centralwidget")
         self.frame = QtWidgets.QFrame(self.centralwidget)
-        self.frame.setGeometry(QtCore.QRect(170, 500, 711, 271))
+        self.frame.setGeometry(QtCore.QRect(170, 500, 710, 271))
         self.frame.setStyleSheet("border-color: rgb(46, 52, 54);\n"
 "background-color: rgb(238, 238, 236);")
         self.frame.setFrameShape(QtWidgets.QFrame.StyledPanel)
@@ -268,7 +268,7 @@ class Ui_MainWindow(object):
         self.verticalLayout_7.addWidget(self.button_stop)
         self.gridLayout_4.addLayout(self.verticalLayout_7, 1, 0, 1, 1)
         self.frame_3 = QtWidgets.QFrame(self.centralwidget)
-        self.frame_3.setGeometry(QtCore.QRect(170, 0, 711, 470))
+        self.frame_3.setGeometry(QtCore.QRect(170, 0, 710, 470))
         self.frame_3.setStyleSheet("background-color: rgb(238, 238, 236);")
         self.frame_3.setFrameShape(QtWidgets.QFrame.StyledPanel)
         self.frame_3.setFrameShadow(QtWidgets.QFrame.Raised)
@@ -406,6 +406,9 @@ class Ui_MainWindow(object):
         self.comboBox_maps.addItem("")
         self.comboBox_maps.addItem("")
         self.comboBox_maps.addItem("")
+        self.comboBox_maps.addItem("")
+        self.comboBox_maps.addItem("")
+        self.comboBox_maps.addItem("")
         self.comboBox_vehicle_types = QtWidgets.QComboBox(self.frame_5)
         self.comboBox_vehicle_types.setGeometry(QtCore.QRect(30, 480, 101, 25))
         self.comboBox_vehicle_types.setStyleSheet("color: rgb(32, 74, 135);\n"
@@ -494,7 +497,7 @@ class Ui_MainWindow(object):
 
     def retranslateUi(self, MainWindow):
         _translate = QtCore.QCoreApplication.translate
-        MainWindow.setWindowTitle(_translate("MainWindow", "XTDrone Window"))
+        MainWindow.setWindowTitle(_translate("MainWindow", "XTDGroundControl"))
         self.label_control_board_2.setText(_translate("MainWindow", "control board"))
         self.label_go_back_2.setText(_translate("MainWindow", "go and back"))
         self.box_go_and_back_2.setToolTip(_translate("MainWindow", "maximum:+-10.00"))
@@ -538,9 +541,12 @@ class Ui_MainWindow(object):
         self.label_control_board_5.setText(_translate("MainWindow", "Settings"))
         self.label_uavnumber_4.setText(_translate("MainWindow", "maps:"))
         self.comboBox_maps.setItemText(0, _translate("MainWindow", "indoor1"))
-        self.comboBox_maps.setItemText(1, _translate("MainWindow", "outdoor1"))
-        self.comboBox_maps.setItemText(2, _translate("MainWindow", "outdoor3"))
-        self.comboBox_maps.setItemText(3, _translate("MainWindow", "robocup"))
+        self.comboBox_maps.setItemText(1, _translate("MainWindow", "indoor2"))
+        self.comboBox_maps.setItemText(2, _translate("MainWindow", "indoor3"))
+        self.comboBox_maps.setItemText(3, _translate("MainWindow", "outdoor1"))
+        self.comboBox_maps.setItemText(4, _translate("MainWindow", "outdoor2"))
+        self.comboBox_maps.setItemText(5, _translate("MainWindow", "outdoor3"))
+        self.comboBox_maps.setItemText(6, _translate("MainWindow", "robocup"))
         self.comboBox_vehicle_types.setItemText(0, _translate("MainWindow", "iris"))
         self.comboBox_vehicle_types.setItemText(1, _translate("MainWindow", "solo"))
         self.comboBox_vehicle_types.setItemText(2, _translate("MainWindow", "typhoon_h480"))

control/gui/plotcanvas.py

@@ -1,40 +0,0 @@
-from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
-from matplotlib.figure import Figure
-import matplotlib.pyplot as plt
-from PIL import Image
-from PyQt5.QtWidgets import QApplication, QMainWindow, QSizePolicy
-
-class PlotCanvas(FigureCanvas):
-
-    def __init__(self, parent=None, Map='indoor1'):
-        self.Image_PreProcessing(Map)
-        img = plt.imread('./images/open.jpg')
-        self.fig, self.ax = plt.subplots()
-        self.ax.imshow(img, extent=[-50, 100, -50, 50])
-        height, width, channels = img.shape
-        self.fig.set_size_inches(width/100.0, height/100.0)
-        plt.gca().xaxis.set_major_locator(plt.NullLocator())
-        plt.gca().yaxis.set_major_locator(plt.NullLocator())
-        plt.subplots_adjust(top=1, bottom=0, left=0, right=1, hspace=0, wspace=0)
-        plt.margins(0, 0)
-        plt.axis('off')
-        FigureCanvas.__init__(self, self.fig)
-        self.setParent(parent)
-
-        FigureCanvas.setSizePolicy(self,  QSizePolicy.Expanding, QSizePolicy.Expanding)
-        FigureCanvas.updateGeometry(self)
-
-    def canvas_update(self, Map):
-        self.Image_PreProcessing(Map)
-        img = plt.imread('./images/open.jpg')
-        self.ax.imshow(img, extent=[-50, 100, -50, 50])
-        self.draw()
-
-    def Image_PreProcessing(self,Map):
-        if Map == 'robocup':
-            im = Image.open('./images/robocup_world.jpg')
-            print('robocup')
-        else:
-            im = Image.open('./images/xt.jpg')
-        imBackground = im.resize((876, 587))  #701 470 840 564
-        imBackground.save('./images/open.jpg')