added GUI protection for safe controller setpoint outside box

pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h

@@ -208,6 +208,9 @@ private:
     void highlightSafeControllerTab();
     void highlightCustomControllerTab();
 
+    bool setpointInsideBox(Setpoint setpoint, CrazyflieContext context);
+    Setpoint correctSetpointBox(Setpoint setpoint, CrazyflieContext context);
+
     const std::vector<float> m_cutoff_voltages {3.1966,        3.2711,        3.3061,        3.3229,        3.3423,        3.3592,        3.3694,        3.385,        3.4006,        3.4044,        3.4228,        3.4228,        3.4301,        3.4445,        3.4531,        3.4677,        3.4705,        3.4712,        3.4756,        3.483,        3.4944,        3.5008,        3.5008,        3.5084,        3.511,        3.5122,        3.5243,        3.5329,        3.5412,        3.5529,        3.5609,        3.5625,        3.5638,        3.5848,        3.6016,        3.6089,        3.6223,        3.628,        3.6299,        3.6436,        3.6649,        3.6878,        3.6983,        3.7171,        3.7231,        3.7464,        3.7664,        3.7938,        3.8008,        3.816,        3.8313,        3.8482,        3.866,        3.8857,        3.8984,        3.9159,        3.9302,        3.9691,        3.997,        4.14    };
 };
 

pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp

@@ -142,6 +142,9 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
     ui->label_battery->setStyleSheet("QLabel { color : red; }");
     m_battery_state = BATTERY_STATE_NORMAL;
 
+    ui->error_label->setStyleSheet("QLabel { color : red; }");
+    ui->error_label->clear();
+
     initialize_custom_setpoint();
 }
 
@@ -481,6 +484,20 @@ void MainWindow::on_set_setpoint_button_clicked()
     if(!ui->new_setpoint_yaw->text().isEmpty())
         msg_setpoint.yaw = (ui->new_setpoint_yaw->text()).toFloat() * DEG2RAD;
 
+
+    if(!setpointInsideBox(msg_setpoint, m_context))
+    {
+        ROS_INFO("Corrected setpoint, was out of bounds");
+
+        // correct the setpoint given the box size
+        msg_setpoint = correctSetpointBox(msg_setpoint, m_context);
+        ui->error_label->setText("Setpoint is outside safety box");
+    }
+    else
+    {
+        ui->error_label->clear();
+    }
+
     this->controllerSetpointPublisher.publish(msg_setpoint);
 
     ROS_INFO_STREAM("Setpoint change clicked with:" << msg_setpoint.x << ", "<< msg_setpoint.y << ", "<< msg_setpoint.z << ", "<< msg_setpoint.yaw);
@@ -688,3 +705,43 @@ void MainWindow::on_customButton_3_clicked()
     this->PPSClientStudentCustomButtonPublisher.publish(msg_custom_button);
     ROS_INFO("Custom button 3 pressed");
 }
+
+Setpoint MainWindow::correctSetpointBox(Setpoint setpoint, CrazyflieContext context)
+{
+    Setpoint corrected_setpoint;
+    corrected_setpoint =  setpoint;
+
+    if(setpoint.x > context.localArea.xmax)
+        corrected_setpoint.x = context.localArea.xmax;
+    if(setpoint.y > context.localArea.ymax)
+        corrected_setpoint.y = context.localArea.ymax;
+    if(setpoint.z > context.localArea.zmax)
+        corrected_setpoint.z = context.localArea.zmax;
+
+    if(setpoint.x < context.localArea.xmin)
+        corrected_setpoint.x = context.localArea.xmin;
+    if(setpoint.y < context.localArea.ymin)
+        corrected_setpoint.y = context.localArea.ymin;
+    if(setpoint.z < context.localArea.zmin)
+        corrected_setpoint.z = context.localArea.zmin;
+
+}
+
+bool MainWindow::setpointInsideBox(Setpoint setpoint, CrazyflieContext context)
+{
+    //position check
+	if((setpoint.x < context.localArea.xmin) or (setpoint.x > context.localArea.xmax)) {
+		ROS_INFO_STREAM("x outside safety box");
+		return false;
+	}
+	if((setpoint.y < context.localArea.ymin) or (setpoint.y > context.localArea.ymax)) {
+		ROS_INFO_STREAM("y outside safety box");
+		return false;
+	}
+	if((setpoint.z < context.localArea.zmin) or (setpoint.z > context.localArea.zmax)) {
+		ROS_INFO_STREAM("z outside safety box");
+		return false;
+	}
+
+	return true;
+}

pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui

@@ -171,7 +171,7 @@
        <item row="3" column="0">
         <widget class="QTabWidget" name="tabWidget">
          <property name="currentIndex">
-          <number>1</number>
+          <number>0</number>
          </property>
          <widget class="QWidget" name="tab_3">
           <attribute name="title">
@@ -536,6 +536,19 @@
                 </property>
                </widget>
               </item>
+              <item row="3" column="1">
+               <widget class="QLineEdit" name="current_setpoint_z">
+                <property name="sizePolicy">
+                 <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+                  <horstretch>0</horstretch>
+                  <verstretch>0</verstretch>
+                 </sizepolicy>
+                </property>
+                <property name="readOnly">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
               <item row="3" column="0">
                <widget class="QLabel" name="label_9">
                 <property name="sizePolicy">
@@ -562,8 +575,8 @@
                 </property>
                </widget>
               </item>
-              <item row="3" column="1">
-               <widget class="QLineEdit" name="current_setpoint_z">
+              <item row="4" column="1">
+               <widget class="QLineEdit" name="current_setpoint_yaw">
                 <property name="sizePolicy">
                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
                   <horstretch>0</horstretch>
@@ -593,19 +606,6 @@
                 </property>
                </widget>
               </item>
-              <item row="4" column="1">
-               <widget class="QLineEdit" name="current_setpoint_yaw">
-                <property name="sizePolicy">
-                 <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                  <horstretch>0</horstretch>
-                  <verstretch>0</verstretch>
-                 </sizepolicy>
-                </property>
-                <property name="readOnly">
-                 <bool>true</bool>
-                </property>
-               </widget>
-              </item>
               <item row="3" column="2">
                <widget class="QLineEdit" name="new_setpoint_z">
                 <property name="sizePolicy">
@@ -650,6 +650,13 @@
                 </property>
                </widget>
               </item>
+              <item row="5" column="1">
+               <widget class="QLabel" name="error_label">
+                <property name="text">
+                 <string/>
+                </property>
+               </widget>
+              </item>
              </layout>
             </widget>
            </item>

pps_ws/src/d_fall_pps/src/PPSClient.cpp

@@ -1013,7 +1013,6 @@ int main(int argc, char* argv[])
     // namespace string for the parameter service that is running on the machine of this
     // agent
     namespace_to_own_agent_parameter_service = "ParameterService";
-    
     // Create a node handle to the parameter service running on this agent's machine
     ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
 
@@ -1068,7 +1067,6 @@ int main(int argc, char* argv[])
     controller_setpoint.y = default_setpoint[1];
     controller_setpoint.z = default_setpoint[2];
     controller_setpoint.yaw = default_setpoint[3];
-    
 
 	//ros::service::waitForService("/CentralManagerService/CentralManager");
 	centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);