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 a4ed522115b23c76cf0664968ff61433262763c6..a1822cb3a9ac01b0f437a40ba7ed0e7749bfcfe8 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
@@ -910,6 +910,13 @@ void MainWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to ne
ui->current_pitch_demo->setText(QString::number(local.pitch * RAD2DEG, 'f', 1));
ui->current_roll_demo->setText(QString::number(local.roll * RAD2DEG, 'f', 1));
+ ui->current_x_student->setText(QString::number(local.x, 'f', 3));
+ ui->current_y_student->setText(QString::number(local.y, 'f', 3));
+ ui->current_z_student->setText(QString::number(local.z, 'f', 3));
+ ui->current_yaw_student->setText(QString::number(local.yaw * RAD2DEG, 'f', 1));
+ ui->current_pitch_student->setText(QString::number(local.pitch * RAD2DEG, 'f', 1));
+ ui->current_roll_student->setText(QString::number(local.roll * RAD2DEG, 'f', 1));
+
// also update diff
ui->diff_x_safe->setText(QString::number(m_safe_setpoint.x - local.x, 'f', 3));
ui->diff_y_safe->setText(QString::number(m_safe_setpoint.y - local.y, 'f', 3));
@@ -920,6 +927,11 @@ void MainWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to ne
ui->diff_y_demo->setText(QString::number(m_demo_setpoint.y - local.y, 'f', 3));
ui->diff_z_demo->setText(QString::number(m_demo_setpoint.z - local.z, 'f', 3));
ui->diff_yaw_demo->setText(QString::number((m_demo_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 1));
+
+ ui->diff_x_student->setText(QString::number(m_student_setpoint.x - local.x, 'f', 3));
+ ui->diff_y_student->setText(QString::number(m_student_setpoint.y - local.y, 'f', 3));
+ ui->diff_z_student->setText(QString::number(m_student_setpoint.z - local.z, 'f', 3));
+ ui->diff_yaw_student->setText(QString::number((m_student_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 1));
}
}
}
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 a8868b9726dceaf4e6803f4871cafeaf4cd0a6ad..7890bdbb412dc9f5fc7be68e8174594943510c65 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
@@ -391,7 +391,7 @@
</font>
</property>
<property name="currentIndex">
- <number>1</number>
+ <number>3</number>
</property>
<property name="usesScrollButtons">
<bool>true</bool>
@@ -2789,32 +2789,46 @@
<property name="bottomMargin">
<number>6</number>
</property>
- <item row="15" column="1">
- <widget class="QPushButton" name="studentButton_2">
+ <item row="15" column="0">
+ <widget class="QLabel" name="label_72">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
- <property name="minimumSize">
- <size>
- <width>0</width>
- <height>40</height>
- </size>
+ <property name="text">
+ <string>roll [deg]</string>
</property>
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>50</height>
- </size>
+ </widget>
+ </item>
+ <item row="3" column="0">
+ <widget class="QLabel" name="label_67">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
</property>
<property name="text">
- <string>Commnd 2</string>
+ <string>x [m]</string>
</property>
</widget>
</item>
- <item row="11" column="0">
+ <item row="7" column="0">
+ <widget class="QLabel" name="label_68">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>y [m]</string>
+ </property>
+ </widget>
+ </item>
+ <item row="11" column="3">
<widget class="QLabel" name="label_25">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
@@ -2832,7 +2846,7 @@
</property>
</widget>
</item>
- <item row="1" column="2">
+ <item row="1" column="5">
<widget class="QLabel" name="label_27">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
@@ -2853,7 +2867,7 @@
</property>
</widget>
</item>
- <item row="14" column="2">
+ <item row="14" column="5">
<widget class="QPushButton" name="set_setpoint_button_student">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
@@ -2871,7 +2885,7 @@
</property>
</widget>
</item>
- <item row="1" column="1">
+ <item row="1" column="4">
<widget class="QLabel" name="label_26">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
@@ -2892,7 +2906,7 @@
</property>
</widget>
</item>
- <item row="1" column="0">
+ <item row="1" column="3">
<widget class="QLabel" name="label_28">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
@@ -2910,7 +2924,7 @@
</property>
</widget>
</item>
- <item row="3" column="0">
+ <item row="3" column="3">
<widget class="QLabel" name="label_29">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
@@ -2928,7 +2942,7 @@
</property>
</widget>
</item>
- <item row="9" column="0">
+ <item row="9" column="3">
<widget class="QLabel" name="label_24">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
@@ -2946,7 +2960,7 @@
</property>
</widget>
</item>
- <item row="7" column="0">
+ <item row="7" column="3">
<widget class="QLabel" name="label_23">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
@@ -2964,7 +2978,7 @@
</property>
</widget>
</item>
- <item row="3" column="2">
+ <item row="3" column="5">
<widget class="QLineEdit" name="new_setpoint_x_student">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
@@ -2979,7 +2993,7 @@
</property>
</widget>
</item>
- <item row="11" column="2">
+ <item row="11" column="5">
<widget class="QLineEdit" name="new_setpoint_yaw_student">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
@@ -2994,7 +3008,7 @@
</property>
</widget>
</item>
- <item row="3" column="1">
+ <item row="3" column="4">
<widget class="QLineEdit" name="current_setpoint_x_student">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
@@ -3012,7 +3026,7 @@
</property>
</widget>
</item>
- <item row="7" column="1">
+ <item row="7" column="4">
<widget class="QLineEdit" name="current_setpoint_y_student">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
@@ -3030,7 +3044,7 @@
</property>
</widget>
</item>
- <item row="11" column="1">
+ <item row="11" column="4">
<widget class="QLineEdit" name="current_setpoint_yaw_student">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
@@ -3048,7 +3062,7 @@
</property>
</widget>
</item>
- <item row="9" column="1">
+ <item row="9" column="4">
<widget class="QLineEdit" name="current_setpoint_z_student">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
@@ -3066,7 +3080,7 @@
</property>
</widget>
</item>
- <item row="9" column="2">
+ <item row="9" column="5">
<widget class="QLineEdit" name="new_setpoint_z_student">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
@@ -3081,7 +3095,7 @@
</property>
</widget>
</item>
- <item row="7" column="2">
+ <item row="7" column="5">
<widget class="QLineEdit" name="new_setpoint_y_student">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
@@ -3096,7 +3110,172 @@
</property>
</widget>
</item>
- <item row="15" column="0">
+ <item row="1" column="1">
+ <widget class="QLabel" name="label_73">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Current</string>
+ </property>
+ </widget>
+ </item>
+ <item row="14" column="0">
+ <widget class="QLabel" name="label_71">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>pitch [deg]</string>
+ </property>
+ </widget>
+ </item>
+ <item row="11" column="0">
+ <widget class="QLabel" name="label_70">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>yaw [deg]</string>
+ </property>
+ </widget>
+ </item>
+ <item row="9" column="0">
+ <widget class="QLabel" name="label_69">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>z [m]</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="2">
+ <widget class="QLabel" name="label_74">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Difference</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="1">
+ <widget class="QLineEdit" name="current_x_student">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="2">
+ <widget class="QLineEdit" name="diff_x_student">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ </widget>
+ </item>
+ <item row="7" column="1">
+ <widget class="QLineEdit" name="current_y_student">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ </widget>
+ </item>
+ <item row="7" column="2">
+ <widget class="QLineEdit" name="diff_y_student">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ </widget>
+ </item>
+ <item row="9" column="1">
+ <widget class="QLineEdit" name="current_z_student">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ </widget>
+ </item>
+ <item row="9" column="2">
+ <widget class="QLineEdit" name="diff_z_student">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ </widget>
+ </item>
+ <item row="11" column="1">
+ <widget class="QLineEdit" name="current_yaw_student">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ </widget>
+ </item>
+ <item row="11" column="2">
+ <widget class="QLineEdit" name="diff_yaw_student">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ </widget>
+ </item>
+ <item row="14" column="1">
+ <widget class="QLineEdit" name="current_pitch_student">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ </widget>
+ </item>
+ <item row="15" column="1">
+ <widget class="QLineEdit" name="current_roll_student">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ </widget>
+ </item>
+ <item row="16" column="2">
<widget class="QPushButton" name="studentButton_1">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
@@ -3121,7 +3300,32 @@
</property>
</widget>
</item>
- <item row="15" column="2">
+ <item row="16" column="3">
+ <widget class="QPushButton" name="studentButton_2">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>40</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Commnd 2</string>
+ </property>
+ </widget>
+ </item>
+ <item row="16" column="4">
<widget class="QPushButton" name="studentButton_3">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
@@ -3146,7 +3350,7 @@
</property>
</widget>
</item>
- <item row="15" column="3">
+ <item row="16" column="5">
<widget class="QLineEdit" name="studentField_3">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
diff --git a/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp
index 8ba28bbf9522bd642d710a54777f1641ecd84a7a..4ec1acc3bdb924d3196b80be9e3abd4db3e06d92 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp
@@ -699,7 +699,7 @@ void processFetchedParameters()
{
// Compute the feed-forward force that we need to counteract gravity (i.e., mg)
// > in units of [Newtons]
- cf_weight_in_newtons = cf_mass_in_grams * 9.81/(1000*4);
+ cf_weight_in_newtons = cf_mass_in_grams * 9.81/1000.0;
// DEBUGGING: Print out one of the computed quantities
ROS_INFO_STREAM("[STUDENT CONTROLLER] DEBUGGING: thus the weight of this agent in [Newtons] = " << cf_weight_in_newtons);