diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
index f602d16293f3b8714b06d29a6c5ea004738e14bf..be4888a000e4ac892adc5da572790d1404c9a286 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
+++ b/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 };
};
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
index 817af1e6d6a742c796ca6b997ea5004b2af9ecfc..16e466035ff962b902f5cc4057150ff29fdc793d 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
+++ b/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;
+}
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui
index 4eb82b0ea3a04a23d3c0fb8e1f8fce3e55836ece..88cf08e5ce445fe31c88bde317df0329f9d9a7f2 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui
+++ b/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>
diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
index 7335c70925d5cb36f5ff1c81db1d77f496470ac3..8f8d560e75e9fc9a9decdcce1c8bbc6beef8e177 100755
--- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp
+++ b/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);