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 df0bec3737a765fa6d2f3c8c62ca892d46c45f07..62e1fdc05b7969c206cf37ae3b243879201f6234 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
@@ -15,6 +15,7 @@
// commands for CrazyRadio
#define CMD_RECONNECT 0
+#define CMD_DISCONNECT 1
// CrazyRadio states:
@@ -59,6 +60,8 @@ private slots:
void on_set_setpoint_button_clicked();
+ void on_pushButton_3_clicked();
+
private:
Ui::MainWindow *ui;
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 5447e708541251779203d0eca449638b8ba97421..bad2244a2b8eceef852e888b3f7efaf6979b140f 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
@@ -77,12 +77,12 @@ MainWindow::~MainWindow()
void MainWindow::disableGUI()
{
- ui->groupBox->setEnabled(false);
+ ui->groupBox_general->setEnabled(false);
}
void MainWindow::enableGUI()
{
- ui->groupBox->setEnabled(true);
+ ui->groupBox_general->setEnabled(true);
}
void MainWindow::DBChangedCallback(const std_msgs::Int32& msg)
@@ -305,3 +305,11 @@ void MainWindow::on_set_setpoint_button_clicked()
this->setpointPublisher.publish(msg_setpoint);
}
+
+void MainWindow::on_pushButton_3_clicked()
+{
+ std_msgs::Int32 msg;
+ msg.data = CMD_DISCONNECT;
+ this->crazyRadioCommandPublisher.publish(msg);
+ ROS_INFO("command disconnect published");
+}
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 af87d524cfcf3a6ebd9aec2c1021f7134a1bf6f2..325c02d180d925faf1840332817c86b2d4a33596 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
@@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
- <width>809</width>
- <height>573</height>
+ <width>854</width>
+ <height>565</height>
</rect>
</property>
<property name="windowTitle">
@@ -15,651 +15,667 @@
</property>
<widget class="QWidget" name="centralWidget">
<layout class="QGridLayout" name="gridLayout">
- <item row="3" column="1">
- <widget class="QTabWidget" name="tabWidget">
- <property name="currentIndex">
- <number>1</number>
+ <item row="0" column="2">
+ <widget class="QLabel" name="connected_disconnected_label">
+ <property name="text">
+ <string>Connected/Disconnected RF status</string>
</property>
- <widget class="QWidget" name="tab_3">
- <attribute name="title">
- <string>Safe Controller</string>
- </attribute>
- <layout class="QGridLayout" name="gridLayout_5">
- <item row="0" column="0">
- <widget class="QGroupBox" name="groupBox_2">
- <property name="title">
- <string/>
- </property>
- <layout class="QGridLayout" name="gridLayout_3">
- <item row="6" column="1">
- <widget class="QLineEdit" name="current_roll">
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="2" column="1">
- <widget class="QLineEdit" name="current_y">
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="4" column="1">
- <widget class="QLineEdit" name="current_yaw">
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="3" column="1">
- <widget class="QLineEdit" name="current_z">
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="1" column="0">
- <widget class="QLabel" name="current_x_label">
- <property name="text">
- <string>x =</string>
- </property>
- </widget>
- </item>
- <item row="3" column="0">
- <widget class="QLabel" name="current_z_label">
- <property name="text">
- <string>z =</string>
- </property>
- </widget>
- </item>
- <item row="2" column="0">
- <widget class="QLabel" name="current_y_label">
- <property name="text">
- <string>y =</string>
- </property>
- </widget>
- </item>
- <item row="4" column="0">
- <widget class="QLabel" name="current_yaw_label">
- <property name="text">
- <string>yaw = </string>
- </property>
- </widget>
- </item>
- <item row="1" column="1">
- <widget class="QLineEdit" name="current_x">
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="5" column="0">
- <widget class="QLabel" name="current_pitch_label">
- <property name="text">
- <string>pitch =</string>
- </property>
- </widget>
- </item>
- <item row="6" column="0">
- <widget class="QLabel" name="current_roll_label">
- <property name="text">
- <string>roll =</string>
- </property>
- </widget>
- </item>
- <item row="5" column="1">
- <widget class="QLineEdit" name="current_pitch">
- <property name="readOnly">
- <bool>true</bool>
- </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QPushButton" name="RF_Connect_button">
+ <property name="text">
+ <string>Connect RF</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="2">
+ <widget class="QPushButton" name="pushButton_3">
+ <property name="text">
+ <string>Disconnect</string>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="1" colspan="2">
+ <widget class="QGroupBox" name="groupBox_general">
+ <property name="title">
+ <string/>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_10">
+ <item row="1" column="0">
+ <widget class="QTabWidget" name="tabWidget">
+ <property name="currentIndex">
+ <number>1</number>
+ </property>
+ <widget class="QWidget" name="tab_3">
+ <attribute name="title">
+ <string>Safe Controller</string>
+ </attribute>
+ <layout class="QGridLayout" name="gridLayout_5">
+ <item row="0" column="0">
+ <widget class="QGroupBox" name="groupBox_2">
+ <property name="title">
+ <string/>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_3">
+ <item row="6" column="1">
+ <widget class="QLineEdit" name="current_roll">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QLineEdit" name="current_y">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="1">
+ <widget class="QLineEdit" name="current_yaw">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="1">
+ <widget class="QLineEdit" name="current_z">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QLabel" name="current_x_label">
+ <property name="text">
+ <string>x =</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="0">
+ <widget class="QLabel" name="current_z_label">
+ <property name="text">
+ <string>z =</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="QLabel" name="current_y_label">
+ <property name="text">
+ <string>y =</string>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="0">
+ <widget class="QLabel" name="current_yaw_label">
+ <property name="text">
+ <string>yaw = </string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QLineEdit" name="current_x">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="0">
+ <widget class="QLabel" name="current_pitch_label">
+ <property name="text">
+ <string>pitch =</string>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="0">
+ <widget class="QLabel" name="current_roll_label">
+ <property name="text">
+ <string>roll =</string>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="1">
+ <widget class="QLineEdit" name="current_pitch">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QLabel" name="label_4">
+ <property name="text">
+ <string>Current</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="2">
+ <widget class="QLineEdit" name="diff_x">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="2">
+ <widget class="QLineEdit" name="diff_y">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="2">
+ <widget class="QLineEdit" name="diff_z">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="2">
+ <widget class="QLineEdit" name="diff_yaw">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QLabel" name="label_5">
+ <property name="text">
+ <string>Difference</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
</widget>
</item>
<item row="0" column="1">
- <widget class="QLabel" name="label_4">
- <property name="text">
- <string>Current</string>
- </property>
- </widget>
- </item>
- <item row="1" column="2">
- <widget class="QLineEdit" name="diff_x">
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="2" column="2">
- <widget class="QLineEdit" name="diff_y">
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="3" column="2">
- <widget class="QLineEdit" name="diff_z">
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="4" column="2">
- <widget class="QLineEdit" name="diff_yaw">
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="0" column="2">
- <widget class="QLabel" name="label_5">
- <property name="text">
- <string>Difference</string>
- </property>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QGroupBox" name="groupBox_3">
- <property name="title">
- <string/>
- </property>
- <layout class="QGridLayout" name="gridLayout_4">
- <item row="2" column="2">
- <widget class="QLineEdit" name="new_setpoint_y"/>
- </item>
- <item row="4" column="2">
- <widget class="QLineEdit" name="new_setpoint_yaw"/>
- </item>
- <item row="1" column="2">
- <widget class="QLineEdit" name="new_setpoint_x"/>
- </item>
- <item row="2" column="1">
- <widget class="QLineEdit" name="current_setpoint_y">
- <property name="readOnly">
- <bool>true</bool>
- </property>
+ <widget class="QGroupBox" name="groupBox_3">
+ <property name="title">
+ <string/>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_4">
+ <item row="2" column="2">
+ <widget class="QLineEdit" name="new_setpoint_y"/>
+ </item>
+ <item row="4" column="2">
+ <widget class="QLineEdit" name="new_setpoint_yaw"/>
+ </item>
+ <item row="1" column="2">
+ <widget class="QLineEdit" name="new_setpoint_x"/>
+ </item>
+ <item row="2" column="1">
+ <widget class="QLineEdit" name="current_setpoint_y">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QLineEdit" name="current_setpoint_x">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="0">
+ <widget class="QLabel" name="label_11">
+ <property name="text">
+ <string>yaw =</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QLabel" name="label_12">
+ <property name="font">
+ <font>
+ <pointsize>7</pointsize>
+ </font>
+ </property>
+ <property name="text">
+ <string>Current</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QLabel" name="label_7">
+ <property name="text">
+ <string>x =</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="0">
+ <widget class="QLabel" name="label_9">
+ <property name="text">
+ <string>z =</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="QLabel" name="label_8">
+ <property name="text">
+ <string>y =</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="1">
+ <widget class="QLineEdit" name="current_setpoint_z">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="2">
+ <widget class="QPushButton" name="set_setpoint_button">
+ <property name="font">
+ <font>
+ <pointsize>7</pointsize>
+ </font>
+ </property>
+ <property name="text">
+ <string>Set setpoint</string>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="1">
+ <widget class="QLineEdit" name="current_setpoint_yaw">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="2">
+ <widget class="QLineEdit" name="new_setpoint_z"/>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_3">
+ <property name="text">
+ <string>Setpoint:</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QLabel" name="label_13">
+ <property name="font">
+ <font>
+ <pointsize>7</pointsize>
+ </font>
+ </property>
+ <property name="text">
+ <string>New</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
</widget>
</item>
<item row="1" column="1">
- <widget class="QLineEdit" name="current_setpoint_x">
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="4" column="0">
- <widget class="QLabel" name="label_11">
- <property name="text">
- <string>yaw =</string>
- </property>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QLabel" name="label_12">
- <property name="font">
- <font>
- <pointsize>7</pointsize>
- </font>
- </property>
- <property name="text">
- <string>Current</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="1" column="0">
- <widget class="QLabel" name="label_7">
- <property name="text">
- <string>x =</string>
- </property>
- </widget>
- </item>
- <item row="3" column="0">
- <widget class="QLabel" name="label_9">
- <property name="text">
- <string>z =</string>
- </property>
- </widget>
- </item>
- <item row="2" column="0">
- <widget class="QLabel" name="label_8">
- <property name="text">
- <string>y =</string>
- </property>
- </widget>
- </item>
- <item row="3" column="1">
- <widget class="QLineEdit" name="current_setpoint_z">
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="5" column="2">
- <widget class="QPushButton" name="set_setpoint_button">
- <property name="font">
- <font>
- <pointsize>7</pointsize>
- </font>
- </property>
- <property name="text">
- <string>Set setpoint</string>
- </property>
- </widget>
- </item>
- <item row="4" column="1">
- <widget class="QLineEdit" name="current_setpoint_yaw">
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="3" column="2">
- <widget class="QLineEdit" name="new_setpoint_z"/>
- </item>
- <item row="0" column="0">
- <widget class="QLabel" name="label_3">
+ <widget class="QPushButton" name="pushButton">
<property name="text">
- <string>Setpoint:</string>
- </property>
- </widget>
- </item>
- <item row="0" column="2">
- <widget class="QLabel" name="label_13">
- <property name="font">
- <font>
- <pointsize>7</pointsize>
- </font>
- </property>
- <property name="text">
- <string>New</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
+ <string>Enable Controller</string>
</property>
</widget>
</item>
</layout>
</widget>
- </item>
- <item row="1" column="1">
- <widget class="QPushButton" name="pushButton">
- <property name="text">
- <string>Enable Controller</string>
- </property>
- </widget>
- </item>
- </layout>
- </widget>
- <widget class="QWidget" name="tab_4">
- <attribute name="title">
- <string>Custom Controller</string>
- </attribute>
- <layout class="QGridLayout" name="gridLayout_9">
- <item row="0" column="0">
- <widget class="QGroupBox" name="groupBox_5">
- <property name="title">
- <string/>
- </property>
- <layout class="QGridLayout" name="gridLayout_7">
- <item row="6" column="1">
- <widget class="QLineEdit" name="current_roll_2">
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="2" column="1">
- <widget class="QLineEdit" name="current_y_2">
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="4" column="1">
- <widget class="QLineEdit" name="current_yaw_2">
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="3" column="1">
- <widget class="QLineEdit" name="current_z_2">
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="1" column="0">
- <widget class="QLabel" name="current_x_label_2">
- <property name="text">
- <string>x =</string>
- </property>
- </widget>
- </item>
- <item row="3" column="0">
- <widget class="QLabel" name="current_z_label_2">
- <property name="text">
- <string>z =</string>
- </property>
- </widget>
- </item>
- <item row="2" column="0">
- <widget class="QLabel" name="current_y_label_2">
- <property name="text">
- <string>y =</string>
- </property>
- </widget>
- </item>
- <item row="4" column="0">
- <widget class="QLabel" name="current_yaw_label_2">
- <property name="text">
- <string>yaw = </string>
- </property>
- </widget>
- </item>
- <item row="1" column="1">
- <widget class="QLineEdit" name="current_x_2">
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="5" column="0">
- <widget class="QLabel" name="current_pitch_label_2">
- <property name="text">
- <string>pitch =</string>
- </property>
- </widget>
- </item>
- <item row="6" column="0">
- <widget class="QLabel" name="current_roll_label_2">
- <property name="text">
- <string>roll =</string>
- </property>
- </widget>
- </item>
- <item row="5" column="1">
- <widget class="QLineEdit" name="current_pitch_2">
- <property name="readOnly">
- <bool>true</bool>
- </property>
+ <widget class="QWidget" name="tab_4">
+ <attribute name="title">
+ <string>Custom Controller</string>
+ </attribute>
+ <layout class="QGridLayout" name="gridLayout_9">
+ <item row="0" column="0">
+ <widget class="QGroupBox" name="groupBox_5">
+ <property name="title">
+ <string/>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_7">
+ <item row="6" column="1">
+ <widget class="QLineEdit" name="current_roll_2">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QLineEdit" name="current_y_2">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="1">
+ <widget class="QLineEdit" name="current_yaw_2">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="1">
+ <widget class="QLineEdit" name="current_z_2">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QLabel" name="current_x_label_2">
+ <property name="text">
+ <string>x =</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="0">
+ <widget class="QLabel" name="current_z_label_2">
+ <property name="text">
+ <string>z =</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="QLabel" name="current_y_label_2">
+ <property name="text">
+ <string>y =</string>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="0">
+ <widget class="QLabel" name="current_yaw_label_2">
+ <property name="text">
+ <string>yaw = </string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QLineEdit" name="current_x_2">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="0">
+ <widget class="QLabel" name="current_pitch_label_2">
+ <property name="text">
+ <string>pitch =</string>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="0">
+ <widget class="QLabel" name="current_roll_label_2">
+ <property name="text">
+ <string>roll =</string>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="1">
+ <widget class="QLineEdit" name="current_pitch_2">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QLabel" name="label_6">
+ <property name="text">
+ <string>Current</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="2">
+ <widget class="QLineEdit" name="diff_x_2">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="2">
+ <widget class="QLineEdit" name="diff_y_2">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="2">
+ <widget class="QLineEdit" name="diff_z_2">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="2">
+ <widget class="QLineEdit" name="diff_yaw_2">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QLabel" name="label_10">
+ <property name="text">
+ <string>Difference</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
</widget>
</item>
<item row="0" column="1">
- <widget class="QLabel" name="label_6">
- <property name="text">
- <string>Current</string>
- </property>
- </widget>
- </item>
- <item row="1" column="2">
- <widget class="QLineEdit" name="diff_x_2">
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="2" column="2">
- <widget class="QLineEdit" name="diff_y_2">
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="3" column="2">
- <widget class="QLineEdit" name="diff_z_2">
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="4" column="2">
- <widget class="QLineEdit" name="diff_yaw_2">
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="0" column="2">
- <widget class="QLabel" name="label_10">
- <property name="text">
- <string>Difference</string>
- </property>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QGroupBox" name="groupBox_6">
- <property name="title">
- <string/>
- </property>
- <layout class="QGridLayout" name="gridLayout_8">
- <item row="2" column="2">
- <widget class="QLineEdit" name="new_setpoint_y_2"/>
- </item>
- <item row="4" column="2">
- <widget class="QLineEdit" name="new_setpoint_yaw_2"/>
- </item>
- <item row="1" column="2">
- <widget class="QLineEdit" name="new_setpoint_x_2"/>
- </item>
- <item row="2" column="1">
- <widget class="QLineEdit" name="current_setpoint_y_2">
- <property name="readOnly">
- <bool>true</bool>
- </property>
+ <widget class="QGroupBox" name="groupBox_6">
+ <property name="title">
+ <string/>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_8">
+ <item row="2" column="2">
+ <widget class="QLineEdit" name="new_setpoint_y_2"/>
+ </item>
+ <item row="4" column="2">
+ <widget class="QLineEdit" name="new_setpoint_yaw_2"/>
+ </item>
+ <item row="1" column="2">
+ <widget class="QLineEdit" name="new_setpoint_x_2"/>
+ </item>
+ <item row="2" column="1">
+ <widget class="QLineEdit" name="current_setpoint_y_2">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QLineEdit" name="current_setpoint_x_2">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="0">
+ <widget class="QLabel" name="label_14">
+ <property name="text">
+ <string>yaw =</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QLabel" name="label_15">
+ <property name="font">
+ <font>
+ <pointsize>7</pointsize>
+ </font>
+ </property>
+ <property name="text">
+ <string>Current</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QLabel" name="label_16">
+ <property name="text">
+ <string>x =</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="0">
+ <widget class="QLabel" name="label_17">
+ <property name="text">
+ <string>z =</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="QLabel" name="label_18">
+ <property name="text">
+ <string>y =</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="1">
+ <widget class="QLineEdit" name="current_setpoint_z_2">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="2">
+ <widget class="QPushButton" name="set_setpoint_button_2">
+ <property name="font">
+ <font>
+ <pointsize>7</pointsize>
+ </font>
+ </property>
+ <property name="text">
+ <string>Set setpoint</string>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="1">
+ <widget class="QLineEdit" name="current_setpoint_yaw_2">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="2">
+ <widget class="QLineEdit" name="new_setpoint_z_2"/>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_19">
+ <property name="text">
+ <string>Setpoint:</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QLabel" name="label_20">
+ <property name="font">
+ <font>
+ <pointsize>7</pointsize>
+ </font>
+ </property>
+ <property name="text">
+ <string>New</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
</widget>
</item>
<item row="1" column="1">
- <widget class="QLineEdit" name="current_setpoint_x_2">
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="4" column="0">
- <widget class="QLabel" name="label_14">
- <property name="text">
- <string>yaw =</string>
- </property>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QLabel" name="label_15">
- <property name="font">
- <font>
- <pointsize>7</pointsize>
- </font>
- </property>
- <property name="text">
- <string>Current</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="1" column="0">
- <widget class="QLabel" name="label_16">
+ <widget class="QPushButton" name="pushButton_2">
<property name="text">
- <string>x =</string>
- </property>
- </widget>
- </item>
- <item row="3" column="0">
- <widget class="QLabel" name="label_17">
- <property name="text">
- <string>z =</string>
- </property>
- </widget>
- </item>
- <item row="2" column="0">
- <widget class="QLabel" name="label_18">
- <property name="text">
- <string>y =</string>
- </property>
- </widget>
- </item>
- <item row="3" column="1">
- <widget class="QLineEdit" name="current_setpoint_z_2">
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="5" column="2">
- <widget class="QPushButton" name="set_setpoint_button_2">
- <property name="font">
- <font>
- <pointsize>7</pointsize>
- </font>
- </property>
- <property name="text">
- <string>Set setpoint</string>
- </property>
- </widget>
- </item>
- <item row="4" column="1">
- <widget class="QLineEdit" name="current_setpoint_yaw_2">
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="3" column="2">
- <widget class="QLineEdit" name="new_setpoint_z_2"/>
- </item>
- <item row="0" column="0">
- <widget class="QLabel" name="label_19">
- <property name="text">
- <string>Setpoint:</string>
- </property>
- </widget>
- </item>
- <item row="0" column="2">
- <widget class="QLabel" name="label_20">
- <property name="font">
- <font>
- <pointsize>7</pointsize>
- </font>
- </property>
- <property name="text">
- <string>New</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
+ <string>Enable Controller</string>
</property>
</widget>
</item>
</layout>
</widget>
- </item>
- <item row="1" column="1">
- <widget class="QPushButton" name="pushButton_2">
- <property name="text">
- <string>Enable Controller</string>
- </property>
- </widget>
- </item>
- </layout>
- </widget>
- </widget>
- </item>
- <item row="0" column="2">
- <widget class="QLabel" name="connected_disconnected_label">
- <property name="text">
- <string>Connected/Disconnected RF status</string>
- </property>
- </widget>
- </item>
- <item row="1" column="1" colspan="2">
- <widget class="QGroupBox" name="groupBox">
- <property name="title">
- <string>StudentID # connected to CF #</string>
- </property>
- <layout class="QGridLayout" name="gridLayout_2">
- <item row="0" column="3">
- <widget class="QLabel" name="raw_voltage">
- <property name="text">
- <string>Raw voltage: </string>
- </property>
- </widget>
- </item>
- <item row="2" column="3">
- <widget class="QProgressBar" name="battery_bar">
- <property name="value">
- <number>24</number>
- </property>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QPushButton" name="motors_OFF_button">
- <property name="text">
- <string>Motors OFF</string>
- </property>
- </widget>
- </item>
- <item row="2" column="1">
- <widget class="QLabel" name="label">
- <property name="text">
- <string>Battery level</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
- </property>
</widget>
</item>
- </layout>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QPushButton" name="RF_Connect_button">
- <property name="text">
- <string>Connect RF</string>
- </property>
- </widget>
- </item>
- <item row="3" column="2">
- <widget class="QGroupBox" name="groupBox_4">
- <property name="title">
- <string/>
- </property>
- <layout class="QGridLayout" name="gridLayout_6">
<item row="1" column="1">
- <widget class="QLabel" name="flying_state_label">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>FlyingState</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="2" column="0" colspan="3">
- <widget class="QPushButton" name="land_button">
- <property name="text">
- <string>Land</string>
+ <widget class="QGroupBox" name="groupBox_4">
+ <property name="title">
+ <string/>
</property>
+ <layout class="QGridLayout" name="gridLayout_6">
+ <item row="1" column="1">
+ <widget class="QLabel" name="flying_state_label">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>FlyingState</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0" colspan="3">
+ <widget class="QPushButton" name="land_button">
+ <property name="text">
+ <string>Land</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QPushButton" name="take_off_button">
+ <property name="text">
+ <string>Take Off</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
</widget>
</item>
- <item row="0" column="1">
- <widget class="QPushButton" name="take_off_button">
- <property name="text">
- <string>Take Off</string>
+ <item row="0" column="0">
+ <widget class="QGroupBox" name="groupBox">
+ <property name="title">
+ <string>StudentID # connected to CF #</string>
</property>
+ <layout class="QGridLayout" name="gridLayout_2">
+ <item row="0" column="3">
+ <widget class="QLabel" name="raw_voltage">
+ <property name="text">
+ <string>Raw voltage: </string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="3">
+ <widget class="QProgressBar" name="battery_bar">
+ <property name="value">
+ <number>24</number>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QPushButton" name="motors_OFF_button">
+ <property name="text">
+ <string>Motors OFF</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QLabel" name="label">
+ <property name="text">
+ <string>Battery level</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
</widget>
</item>
</layout>
@@ -672,7 +688,7 @@
<rect>
<x>0</x>
<y>0</y>
- <width>809</width>
+ <width>854</width>
<height>19</height>
</rect>
</property>
diff --git a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
index 27a539d626b7b0e68f505bb9e4b5d101c62c8c19..d0127fcebcd6218ac240aa23c39c6b8bad64ca3f 100755
--- a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
+++ b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
@@ -46,6 +46,7 @@ DISCONNECTED = 2
# Commands coming
CMD_RECONNECT = 0
+CMD_DISCONNECT = 1
# Commands for PPSClient
CMD_USE_SAFE_CONTROLLER = 1
@@ -114,6 +115,11 @@ class PPSRadioClient:
rospy.loginfo("connecting...")
self._cf.open_link(self.link_uri)
+ def disconnect(self):
+ print "Disconnecting from %s" % self.link_uri
+ self._cf.close_link()
+ self.change_status_to(DISCONNECTED)
+
def _data_received_callback(self, timestamp, data, logconf):
#print "log of stabilizer and battery: [%d][%s]: %s" % (timestamp, logconf.name, data)
batteryVolt = Float32()
@@ -209,6 +215,7 @@ class PPSRadioClient:
def crazyRadioCommandCallback(self, msg):
"""Callback to tell CrazyRadio to reconnect"""
print "crazyRadio command received %s" % msg.data
+
if msg.data == CMD_RECONNECT: # reconnect, check _status first and then do whatever needs to be done
print "entered reconnect"
print "_status: %s" % self._status
@@ -218,6 +225,13 @@ class PPSRadioClient:
if self.get_status() == CONNECTED:
self.status_pub.publish(CONNECTED)
+ elif msg.data == CMD_DISCONNECT:
+ print "disconnect received"
+ if self.get_status() != DISCONNECTED: # what happens if we disconnect while we are in connecting state?
+ self.disconnect()
+ else:
+ self.status_pub.publish(DISCONNECTED)
+
def controlCommandCallback(data):
"""Callback for controller actions"""
#rospy.loginfo("controller callback : %s, %s, %s", data.roll, data.pitch, data.yaw)
diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
index 0e68c1b153491cd174e75507c4d9aa9ea885b29b..60ec01bf21cb00f613c03c96cd5697ddf5d72e55 100755
--- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp
+++ b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
@@ -45,6 +45,17 @@
#define STATE_FLYING 3
#define STATE_LAND 4
+// commands for CrazyRadio
+#define CMD_RECONNECT 0
+#define CMD_DISCONNECT 1
+
+
+// CrazyRadio states:
+#define CONNECTED 0
+#define CONNECTING 1
+#define DISCONNECTED 2
+
+// parameters for take off and landing. Eventually will go in YAML file
#define TAKE_OFF_OFFSET 1 //in meters
#define LANDING_DISTANCE 0.15 //in meters
#define DURATION_TAKE_OFF 300 //100 is 1 second
@@ -75,12 +86,21 @@ ros::Publisher safeControllerServiceSetpointPublisher;
// publisher for flying state
ros::Publisher flyingStatePublisher;
+// publisher to send commands to itself.
+ros::Publisher commandPublisher;
+
+// communication with crazyRadio node. Connect and disconnect
+ros::Publisher crazyRadioCommandPublisher;
+
rosbag::Bag bag;
// variables for the states:
int flying_state;
bool changed_state_flag;
+// variable for crazyradio status
+int crazyradio_status;
+
//describes the area of the crazyflie and other parameters
CrazyflieContext context;
@@ -213,8 +233,17 @@ void goToOrigin()
void changeFlyingStateTo(int new_state)
{
- ROS_INFO("Change state to: %d", new_state);
- flying_state = new_state;
+ if(crazyradio_status == CONNECTED)
+ {
+ ROS_INFO("Change state to: %d", new_state);
+ flying_state = new_state;
+ }
+ else
+ {
+ ROS_INFO("Disconnected and trying to change state. Stays goes to MOTORS OFF");
+ flying_state = STATE_MOTORS_OFF;
+ }
+
changed_state_flag = true;
std_msgs::Int32 flying_state_msg;
flying_state_msg.data = flying_state;
@@ -371,15 +400,32 @@ void loadCrazyflieContext() {
contextCall.request.studentID = studentID;
ROS_INFO_STREAM("StudentID:" << studentID);
+ CrazyflieContext new_context;
+
centralManager.waitForExistence(ros::Duration(-1));
if(centralManager.call(contextCall)) {
- context = contextCall.response.crazyflieContext;
- ROS_INFO_STREAM("CrazyflieContext:\n" << context);
+ new_context = contextCall.response.crazyflieContext;
+ ROS_INFO_STREAM("CrazyflieContext:\n" << new_context);
} else {
ROS_ERROR("Failed to load context");
}
+ if((context.crazyflieName != "") && (new_context.crazyflieName != context.crazyflieName)) //linked crazyflie name changed and it was not empty before
+ {
+ // send motors OFF and disconnect before setting context = new_context
+ std_msgs::Int32 msg;
+ msg.data = CMD_CRAZYFLY_MOTORS_OFF;
+ commandPublisher.publish(msg);
+
+ // maybe some delay here?
+
+ msg.data = CMD_DISCONNECT;
+ crazyRadioCommandPublisher.publish(msg);
+ }
+
+ context = new_context;
+
ros::NodeHandle nh("CrazyRadio");
nh.setParam("crazyFlieAddress", context.crazyflieAddress);
}
@@ -428,6 +474,11 @@ void DBChangedCallback(const std_msgs::Int32& msg)
loadCrazyflieContext();
}
+void crazyRadioStatusCallback(const std_msgs::Int32& msg)
+{
+ crazyradio_status = msg.data;
+}
+
int main(int argc, char* argv[])
{
ros::init(argc, argv, "PPSClient");
@@ -446,16 +497,17 @@ int main(int argc, char* argv[])
controlCommandPublisher = nodeHandle.advertise <ControlCommand>("ControlCommand", 1);
//this topic lets the PPSClient listen to the terminal commands
- ros::Publisher commandPublisher = nodeHandle.advertise<std_msgs::Int32>("Command", 1);
+ commandPublisher = nodeHandle.advertise<std_msgs::Int32>("Command", 1);
ros::Subscriber commandSubscriber = nodeHandle.subscribe("Command", 1, commandCallback);
//this topic lets us use the terminal to communicate with crazyRadio node.
- ros::Publisher crazyRadioCommandPublisher = nodeHandle.advertise<std_msgs::Int32>("crazyRadioCommand", 1);
+ crazyRadioCommandPublisher = nodeHandle.advertise<std_msgs::Int32>("crazyRadioCommand", 1);
// this topic will publish flying state whenever it changes.
flyingStatePublisher = nodeHandle.advertise<std_msgs::Int32>("flyingState", 1);
-
+ // crazy radio status
+ crazyradio_status = DISCONNECTED;
// publish first flying state data
std_msgs::Int32 flying_state_msg;
@@ -469,6 +521,9 @@ int main(int argc, char* argv[])
// subscriber for DBChanged
ros::Subscriber DBChangedSubscriber = namespaceNodeHandle.subscribe("/my_GUI/DBChanged", 1, DBChangedCallback);
+ // crazyradio status. Connected, connecting or disconnected
+ ros::Subscriber crazyRadioStatusSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/CrazyRadioStatus", 1, crazyRadioStatusCallback);
+
//start with safe controller
flying_state = STATE_MOTORS_OFF;
usingSafeController = true;