In case of SelfLoc failure the anchors can be read from the YAML file

pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/mainguiwindow.h

@@ -137,6 +137,7 @@ private slots:
     void on_calibrateAnchors_button_pressed();
 
     void on_reloadAnchors_button_pressed();
+    void on_reloadAnchorsFromFile_button_pressed();
 
     void on_set_offset_button_pressed();
 

pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/mainguiwindow.cpp

@@ -1034,6 +1034,8 @@ bool MainGUIWindow::updateUWBSettings(bool enableChecked)
     msg.data = 1;
     UWBSettings_publisher.publish(msg);
 
+    ui->checkBox_enable_UWB->setEnabled(true);
+
     return true;
 }
 
@@ -1099,7 +1101,6 @@ void MainGUIWindow::updateAnchors(const Anchors* const a)
 
 void MainGUIWindow::on_checkBox_enable_UWB_toggled(bool checked)
 {
-
     // Send Command to enable UWB to UWBManagerService
     std_msgs::Int32 server_msg;
 
@@ -1121,6 +1122,34 @@ void MainGUIWindow::on_reloadAnchors_button_pressed()
     UWBServiceClientUpdate_publisher.publish(server_msg);
 
     updateUWBSettings(!ui->checkBox_enable_UWB->isChecked());
+}
+
+void MainGUIWindow::on_reloadAnchorsFromFile_button_pressed()
+{
+    // Load Data from UWBManagerService
+    std_msgs::Int32 server_msg;
+    server_msg.data = UWB_UPDATE_ANCHORS;
+    UWBServiceClientUpdate_publisher.publish(server_msg);
+
+    ROS_WARN("[Teacher GUI] Loading Anchors from file!");
+
+    // Receive the updated information
+    Anchors a;
+
+    if(UWBServiceClient.call(a))
+    {
+        updateAnchors(&a);
+    }
+    else
+    {
+        ROS_ERROR("[Teacher GUI] Could not update UWB Settings!");
+        return;
+    }
+
+    // Publish message that clients have to reload data from UWBManagerService
+    std_msgs::Int32 msg;
+    msg.data = 1;
+    UWBSettings_publisher.publish(msg);
 
     ui->checkBox_enable_UWB->setEnabled(true);
 }
@@ -1134,12 +1163,14 @@ void MainGUIWindow::on_calibrateAnchors_button_pressed()
     if(!updateUWBSettings(!ui->checkBox_enable_UWB->isChecked()))
     {
         ROS_ERROR("[Teacher GUI] UWB Anchor calibration failed!");
+        ui->checkBox_localisation_done->setChecked(false);
         return;
     }
 
     ui->checkBox_localisation_done->setChecked(true);
     ui->checkBox_enable_UWB->setEnabled(true);
-    //ui->reloadAnchors_button->setEnabled(true);
+    ui->reloadAnchors_button->setEnabled(true);
+    ui->reloadAnchorsFromFile_button->setEnabled(false);
     ui->set_offset_button->setEnabled(true);
     ui->uwbxoffset->setEnabled(true);
     ui->uwbyoffset->setEnabled(true);
@@ -1178,6 +1209,9 @@ void MainGUIWindow::on_set_invert_button_pressed()
         invertCheckBoxes[i]->setChecked(false);
     }
 
+    if(invert == 0)
+        return;
+
     // Receive the updated information
     Anchors a;
 

pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/mainguiwindow.ui

@@ -510,14 +510,28 @@
               </widget>
             </item>
             <item row="2" column="7">
-              <widget class="QPushButton" name="reloadAnchors_button">
-                <property name="text">
-                  <string>Reload Anchors</string>
-                </property>
-                <!-- <property name="enabled">
-                  <bool>false</bool>
-                </property> -->
-              </widget>
+              <layout class="QGridLayout">
+                <item row="1" column="1">
+                  <widget class="QPushButton" name="reloadAnchorsFromFile_button">
+                    <property name="text">
+                      <string>Load Anchor File</string>
+                    </property>
+                  </widget>
+                </item>
+                <item row="2" column="1">
+                  <widget class="QPushButton" name="reloadAnchors_button">
+                    <property name="text">
+                      <string>Reload Anchors</string>
+                    </property>
+                    <property name="enabled">
+                      <bool>false</bool>
+                    </property>
+                  </widget>
+                </item>
+                
+              </layout>
+
+              
             </item>
             
             <item row="3" column="1">

pps_ws/src/d_fall_pps/include/DataListener.h

@@ -32,6 +32,7 @@ public:
 
 	void setOffset(double x, double y, double z);
 	bool startAnchorLocalistaion();
+	void invertAnchor(unsigned int id);
 
 private:
 	Serial serial;

pps_ws/src/d_fall_pps/src/DataListener.cpp

@@ -210,6 +210,13 @@ bool DataListener::startAnchorLocalistaion()
 	return false;
 }
 
+void DataListener::invertAnchor(unsigned int id)
+{
+	anchor_position[id].z *= -1;
+
+	_writeToYAML();
+}
+
 void DataListener::_writeToYAML()
 {
 	ros::NodeHandle nodeHandle("~");

pps_ws/src/d_fall_pps/src/UWBManagerService.cpp

@@ -25,10 +25,10 @@
 
 #include "DataListener.h"
 
-#define UWB_UPDATE_DISABLE 0
-#define UWB_UPDATE_ENABLE 1
-#define UWB_UPDATE_ANCHORS 5
-#define UWB_CALIBRATE_ANCHORS 7
+#define UWB_UPDATE_DISABLE      0
+#define UWB_UPDATE_ENABLE       1
+#define UWB_UPDATE_ANCHORS      5
+#define UWB_CALIBRATE_ANCHORS   7
 
 using namespace d_fall_pps;
 
@@ -88,7 +88,6 @@ void d_fall_pps::readYaml()
         tempAnchor.z = anchorArr[2];
 
         anchors.data.push_back(tempAnchor);
-
     }
 
     return;
@@ -103,10 +102,7 @@ bool d_fall_pps::getAnchorPositions(Anchors::Request& request, Anchors::Response
         for(int i = 0; i < 6; ++i)
         {
             if(request.invert & (1 << i))
-            {
-                ROS_WARN("inver Anchor %i", i);
-                anchors.data[i].z *= -1;
-            }
+                selfLoc.invertAnchor(i);
         }
     }