Merge branch 'master' into pinocchio_work

Drone/FlightLib/FlightLib.py

@@ -175,11 +175,11 @@ def navto(x, y, z, yaw=float('nan'), frame_id=FRAME_ID, auto_arm=False, **kwargs
     return True
 
 
-def reach_point(x=0.0, y=0.0, z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TOLERANCE, frame_id=FRAME_ID,
+def reach_point(x=0.0, y=0.0, z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TOLERANCE, frame_id=FRAME_ID, auto_arm=False,
                 freq=FREQUENCY, timeout=TIMEOUT, interrupter=INTERRUPTER, wait=False):
-    logger.info('Reaching point: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw))
+    rospy.loginfo('Reaching point: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw))
     #print('Reaching point: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(x, y, z, yaw))
-    navigate(frame_id=frame_id, x=x, y=y, z=z, yaw=yaw, speed=speed)
+    navigate(frame_id=frame_id, x=x, y=y, z=z, yaw=yaw, speed=speed, auto_arm=auto_arm)
 
     # waiting for completion
     telemetry = get_telemetry(frame_id=frame_id)
@@ -188,17 +188,17 @@ def reach_point(x=0.0, y=0.0, z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TO
 
     while (get_distance3d(x, y, z, telemetry.x, telemetry.y, telemetry.z) > tolerance) or wait:
         if interrupter.is_set():
-            logger.warning("Reach point function interrupted!")
+            rospy.logwarn("Reach point function interrupted!")
             #print("Reach point function interrupted!")
             interrupter.clear()
             return False
 
         telemetry = get_telemetry(frame_id=frame_id)
-        logger.info('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(
+        rospy.logdebug('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(
             telemetry.x, telemetry.y, telemetry.z, telemetry.yaw))
         #print('Telemetry: | x: {:.3f} y: {:.3f} z: {:.3f} yaw: {:.3f}'.format(
         #    telemetry.x, telemetry.y, telemetry.z, telemetry.yaw))
-        logger.info('Current delta: | {:.3f}'.format(
+        rospy.logdebug('Current delta: | {:.3f}'.format(
             get_distance3d(x, y, z, telemetry.x, telemetry.y, telemetry.z)))
         #print('Current delta: | {:.3f}'.format(
         #    get_distance3d(x, y, z, telemetry.x, telemetry.y, telemetry.z)))
@@ -207,12 +207,12 @@ def reach_point(x=0.0, y=0.0, z=0.0, yaw=float('nan'), speed=SPEED, tolerance=TO
 
         if timeout is not None:
             if time_passed >= timeout:
-                logger.warning('Reaching point timed out! | time: {:3f} seconds'.format(time_passed))
-                print('Reaching point timed out! | time: {:3f} seconds'.format(time_passed))
+                rospy.logwarn('Reaching point timed out! | time: {:3f} seconds'.format(time_passed))
+                # print('Reaching point timed out! | time: {:3f} seconds'.format(time_passed))
                 return wait
         rate.sleep()
 
-    logger.info("Point reached!")
+    rospy.loginfo("Point reached!")
     #print("Point reached!")
     return True
 

Drone/copter_client.py

@@ -2,6 +2,7 @@
 import time
 import math
 import rospy
+import datetime
 import logging
 
 from FlightLib import FlightLib
@@ -66,7 +67,7 @@ def on_broadcast_bind(self):
 
     def start(self, task_manager_instance):
         client.logger.info("Init ROS node")
-        rospy.init_node('Swarm_client')
+        rospy.init_node('clever_show_client')
         if self.USE_LEDS:
             LedLib.init_led(self.LED_PIN)
         task_manager_instance.start()
@@ -417,7 +418,8 @@ def _copter_flip(*args, **kwargs):
 
 @messaging.message_callback("takeoff")
 def _command_takeoff(*args, **kwargs):
-    task_manager.add_task(time.time(), 0, animation.takeoff,
+    print("Takeoff at {}".format(datetime.datetime.now()))
+    task_manager.add_task(0, 0, animation.takeoff,
                           task_kwargs={
                               "z": client.active_client.TAKEOFF_HEIGHT,
                               "timeout": client.active_client.TAKEOFF_TIME,
@@ -426,6 +428,23 @@ def _command_takeoff(*args, **kwargs):
                           }
                           )
 
+@messaging.message_callback("takeoff_z")
+def _command_takeoff_z(*args, **kwargs):
+    z_str = kwargs.get("z", None)
+    if z_str is not None:
+        telem = FlightLib.get_telemetry(client.active_client.FRAME_ID)
+        print("Takeoff to z = {} at {}".format(z_str, datetime.datetime.now()))
+        task_manager.add_task(0, 0, FlightLib.reach_point,
+                          task_kwargs={
+                              "x": telem.x,
+                              "y": telem.y,
+                              "z": float(z_str),
+                              "frame_id": client.active_client.FRAME_ID,
+                              "timeout": client.active_client.TAKEOFF_TIME,
+                              "auto_arm": True,
+                          }
+                          )
+
 
 @messaging.message_callback("land")
 def _command_land(*args, **kwargs):

Server/server_gui.py

@@ -52,6 +52,7 @@ def setupUi(self, MainWindow):
         self.formLayout.setWidget(3, QtWidgets.QFormLayout.LabelRole, self.music_text)
         self.music_delay_spin = QtWidgets.QDoubleSpinBox(self.centralwidget)
         self.music_delay_spin.setDecimals(1)
+        self.music_delay_spin.setMaximum(1000.0)
         self.music_delay_spin.setObjectName("music_delay_spin")
         self.formLayout.setWidget(3, QtWidgets.QFormLayout.FieldRole, self.music_delay_spin)
         self.music_checkbox = QtWidgets.QCheckBox(self.centralwidget)
@@ -60,8 +61,10 @@ def setupUi(self, MainWindow):
         sizePolicy.setVerticalStretch(0)
         sizePolicy.setHeightForWidth(self.music_checkbox.sizePolicy().hasHeightForWidth())
         self.music_checkbox.setSizePolicy(sizePolicy)
+        self.music_checkbox.setFocusPolicy(QtCore.Qt.NoFocus)
+        self.music_checkbox.setContextMenuPolicy(QtCore.Qt.DefaultContextMenu)
         self.music_checkbox.setLayoutDirection(QtCore.Qt.RightToLeft)
-        self.music_checkbox.setAutoFillBackground(True)
+        self.music_checkbox.setAutoFillBackground(False)
         self.music_checkbox.setText("")
         self.music_checkbox.setChecked(False)
         self.music_checkbox.setObjectName("music_checkbox")
@@ -132,17 +135,33 @@ def setupUi(self, MainWindow):
         self.formLayout_4.setObjectName("formLayout_4")
         self.land_button = QtWidgets.QPushButton(self.centralwidget)
         self.land_button.setObjectName("land_button")
-        self.formLayout_4.setWidget(3, QtWidgets.QFormLayout.FieldRole, self.land_button)
+        self.formLayout_4.setWidget(8, QtWidgets.QFormLayout.FieldRole, self.land_button)
         self.flip_button = QtWidgets.QPushButton(self.centralwidget)
         self.flip_button.setObjectName("flip_button")
-        self.formLayout_4.setWidget(2, QtWidgets.QFormLayout.FieldRole, self.flip_button)
+        self.formLayout_4.setWidget(7, QtWidgets.QFormLayout.FieldRole, self.flip_button)
         self.takeoff_button = QtWidgets.QPushButton(self.centralwidget)
         self.takeoff_button.setEnabled(True)
         self.takeoff_button.setObjectName("takeoff_button")
         self.formLayout_4.setWidget(1, QtWidgets.QFormLayout.FieldRole, self.takeoff_button)
         self.leds_button = QtWidgets.QPushButton(self.centralwidget)
         self.leds_button.setObjectName("leds_button")
         self.formLayout_4.setWidget(0, QtWidgets.QFormLayout.FieldRole, self.leds_button)
+        self.horizontalLayout_2 = QtWidgets.QHBoxLayout()
+        self.horizontalLayout_2.setContentsMargins(-1, 0, -1, -1)
+        self.horizontalLayout_2.setObjectName("horizontalLayout_2")
+        self.z_checkbox = QtWidgets.QCheckBox(self.centralwidget)
+        self.z_checkbox.setCursor(QtGui.QCursor(QtCore.Qt.ArrowCursor))
+        self.z_checkbox.setFocusPolicy(QtCore.Qt.NoFocus)
+        self.z_checkbox.setLayoutDirection(QtCore.Qt.LeftToRight)
+        self.z_checkbox.setObjectName("z_checkbox")
+        self.horizontalLayout_2.addWidget(self.z_checkbox)
+        self.z_spin = QtWidgets.QDoubleSpinBox(self.centralwidget)
+        self.z_spin.setLayoutDirection(QtCore.Qt.LeftToRight)
+        self.z_spin.setDecimals(1)
+        self.z_spin.setProperty("value", 1.0)
+        self.z_spin.setObjectName("z_spin")
+        self.horizontalLayout_2.addWidget(self.z_spin)
+        self.formLayout_4.setLayout(4, QtWidgets.QFormLayout.FieldRole, self.horizontalLayout_2)
         self.verticalLayout.addLayout(self.formLayout_4)
         self.line_4 = QtWidgets.QFrame(self.centralwidget)
         self.line_4.setFrameShape(QtWidgets.QFrame.HLine)
@@ -167,7 +186,7 @@ def setupUi(self, MainWindow):
         self.gridLayout.addLayout(self.horizontalLayout, 0, 0, 1, 1)
         MainWindow.setCentralWidget(self.centralwidget)
         self.menubar = QtWidgets.QMenuBar(MainWindow)
-        self.menubar.setGeometry(QtCore.QRect(0, 0, 1220, 26))
+        self.menubar.setGeometry(QtCore.QRect(0, 0, 1220, 25))
         self.menubar.setObjectName("menubar")
         self.menuOptions = QtWidgets.QMenu(self.menubar)
         self.menuOptions.setObjectName("menuOptions")
@@ -278,6 +297,8 @@ def retranslateUi(self, MainWindow):
         self.flip_button.setText(_translate("MainWindow", "Flip"))
         self.takeoff_button.setText(_translate("MainWindow", "Takeoff"))
         self.leds_button.setText(_translate("MainWindow", "Test leds"))
+        self.z_checkbox.setText(_translate("MainWindow", "  Z     ="))
+        self.z_spin.setSuffix(_translate("MainWindow", " m"))
         self.reboot_fcu.setText(_translate("MainWindow", "Reboot FCU"))
         self.calibrate_gyro.setText(_translate("MainWindow", "Calibrate gyro"))
         self.calibrate_level.setText(_translate("MainWindow", "Calibrate level"))

Server/server_gui.ui

@@ -91,6 +91,9 @@
             <property name="decimals">
              <number>1</number>
             </property>
+            <property name="maximum">
+             <double>1000.000000000000000</double>
+            </property>
            </widget>
           </item>
           <item row="4" column="1">
@@ -101,11 +104,17 @@
               <verstretch>0</verstretch>
              </sizepolicy>
             </property>
+            <property name="focusPolicy">
+             <enum>Qt::NoFocus</enum>
+            </property>
+            <property name="contextMenuPolicy">
+             <enum>Qt::DefaultContextMenu</enum>
+            </property>
             <property name="layoutDirection">
              <enum>Qt::RightToLeft</enum>
             </property>
             <property name="autoFillBackground">
-             <bool>true</bool>
+             <bool>false</bool>
             </property>
             <property name="text">
              <string/>
@@ -248,14 +257,14 @@
           <property name="formAlignment">
            <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
           </property>
-          <item row="3" column="1">
+          <item row="8" column="1">
            <widget class="QPushButton" name="land_button">
             <property name="text">
              <string>Land</string>
             </property>
            </widget>
           </item>
-          <item row="2" column="1">
+          <item row="7" column="1">
            <widget class="QPushButton" name="flip_button">
             <property name="text">
              <string>Flip</string>
@@ -279,6 +288,45 @@
             </property>
            </widget>
           </item>
+          <item row="4" column="1">
+           <layout class="QHBoxLayout" name="horizontalLayout_2">
+            <property name="topMargin">
+             <number>0</number>
+            </property>
+            <item>
+             <widget class="QCheckBox" name="z_checkbox">
+              <property name="cursor">
+               <cursorShape>ArrowCursor</cursorShape>
+              </property>
+              <property name="focusPolicy">
+               <enum>Qt::NoFocus</enum>
+              </property>
+              <property name="layoutDirection">
+               <enum>Qt::LeftToRight</enum>
+              </property>
+              <property name="text">
+               <string>  Z     =</string>
+              </property>
+             </widget>
+            </item>
+            <item>
+             <widget class="QDoubleSpinBox" name="z_spin">
+              <property name="layoutDirection">
+               <enum>Qt::LeftToRight</enum>
+              </property>
+              <property name="suffix">
+               <string> m</string>
+              </property>
+              <property name="decimals">
+               <number>1</number>
+              </property>
+              <property name="value">
+               <double>1.000000000000000</double>
+              </property>
+             </widget>
+            </item>
+           </layout>
+          </item>
          </layout>
         </item>
         <item>
@@ -328,7 +376,7 @@
      <x>0</x>
      <y>0</y>
      <width>1220</width>
-     <height>26</height>
+     <height>25</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuOptions">
@@ -496,7 +544,7 @@
   <action name="action_remove_row">
    <property name="text">
     <string>Remove from table</string>
-   </property> 
+   </property>
   </action>
   <action name="action_send_calibrations">
    <property name="text">

Server/server_qt.py

@@ -305,7 +305,10 @@ def disarm_all(self):
     def takeoff_selected(self, **kwargs):
         for copter in self.model.user_selected():
             if takeoff_checks(copter):
-                copter.client.send_message("takeoff")
+                if self.ui.z_checkbox.isChecked():
+                    copter.client.send_message("takeoff_z", {"z":str(self.ui.z_spin.value())})
+                else:
+                    copter.client.send_message("takeoff")
 
     @pyqtSlot()
     @confirmation_required("This operation will flip(!!!) copters immediately. Proceed?")