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 892373945d90e04dfd7c981af6081f9864aa32d9..e191cb235f8f23fa1e3f458d98ad81c146bb2b9b 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
@@ -3,11 +3,25 @@
#include <QMainWindow>
+#include <std_msgs/Int32.h>
#include "rosNodeThread.h"
+// commands for CrazyRadio
#define CMD_RECONNECT 0
+
+// CrazyRadio states:
+#define CONNECTED 0
+#define CONNECTING 1
+#define DISCONNECTED 2
+
+// Commands for PPSClient
+#define CMD_USE_SAFE_CONTROLLER 1
+#define CMD_USE_CUSTOM_CONTROLLER 2
+#define CMD_USE_CRAZYFLY_ENABLE 3
+#define CMD_USE_CRAZYFLY_DISABLE 4
+
namespace Ui {
class MainWindow;
}
@@ -25,12 +39,24 @@ private slots:
void on_RF_Connect_button_clicked();
+ void on_enable_disable_CF_button_clicked();
+
private:
Ui::MainWindow *ui;
rosNodeThread* m_rosNodeThread;
+ int m_radio_status;
ros::Publisher crazyRadioCommandPublisher;
+ ros::Subscriber crazyRadioStatusSubscriber;
+
+ ros::Publisher PPSClientCommadPublisher;
+
+ void crazyRadioStatusCallback(const std_msgs::Int32& msg);
+ void setCrazyRadioStatus(int radio_status);
+
+ void disableGUI();
+ void enableGUI();
};
#endif // MAINWINDOW_H
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 63c3f2710c767d8bdc879467db4d6751d3bd2fa1..234a8d44a4ec6510b4c62929071c555858e233c8 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
@@ -4,11 +4,11 @@
#include <ros/ros.h>
#include <ros/network.h>
-#include <std_msgs/Int32.h>
MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
QMainWindow(parent),
- ui(new Ui::MainWindow)
+ ui(new Ui::MainWindow),
+ m_radio_status(DISCONNECTED)
{
m_rosNodeThread = new rosNodeThread(argc, argv, "student_GUI");
ui->setupUi(this);
@@ -16,19 +16,65 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
qRegisterMetaType<ptrToMessage>("ptrToMessage");
QObject::connect(m_rosNodeThread, SIGNAL(newViconData(const ptrToMessage&)), this, SLOT(updateNewViconData(const ptrToMessage&)));
+ ros::NodeHandle nodeHandle("~");
+ crazyRadioStatusSubscriber = nodeHandle.subscribe("/CrazyRadio/CrazyRadioStatus", 1, &MainWindow::crazyRadioStatusCallback, this);
+
std::string ros_namespace = ros::this_node::getNamespace();
ROS_INFO("namespace: %s", ros_namespace.c_str());
+ // communication with PPS Client, just to make it possible to communicate through terminal also we use PPSClient's name
ros::NodeHandle nh_PPSClient(ros_namespace + "/PPSClient");
-
crazyRadioCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("crazyRadioCommand", 1);
+ PPSClientCommadPublisher = nodeHandle.advertise<std_msgs::Int32>("Command", 1);
+
+ disableGUI();
}
+
MainWindow::~MainWindow()
{
delete ui;
}
+void MainWindow::disableGUI()
+{
+ ui->enable_disable_CF_button->setEnabled(false);
+}
+
+void MainWindow::enableGUI()
+{
+ ui->enable_disable_CF_button->setEnabled(true);
+}
+
+void MainWindow::setCrazyRadioStatus(int radio_status)
+{
+ // add more things whenever the status is changed
+ switch(radio_status)
+ {
+ case CONNECTED:
+ // change icon, the rest of the GUI is available now
+ ui->connected_disconnected_label->setText("Connected!");
+ break;
+ case CONNECTING:
+ // change icon
+ ui->connected_disconnected_label->setText("Connecting...");
+ break;
+ case DISCONNECTED:
+ // change icon, the rest of the GUI is disabled
+ ui->connected_disconnected_label->setText("Disconnected");
+ break;
+ default:
+ ROS_ERROR_STREAM("unexpected radio status: " << m_radio_status);
+ break;
+ }
+ this->m_radio_status = radio_status;
+}
+
+void MainWindow::crazyRadioStatusCallback(const std_msgs::Int32& msg)
+{
+ this->setCrazyRadioStatus(msg.data);
+}
+
void MainWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to newViconData, from node
{
}
@@ -40,3 +86,21 @@ void MainWindow::on_RF_Connect_button_clicked()
this->crazyRadioCommandPublisher.publish(msg);
ROS_INFO("command reconnect published");
}
+
+void MainWindow::on_enable_disable_CF_button_clicked()
+{
+ if(ui->enable_disable_CF_button->text().toStdString() == "EnableCF") //enabled, disable if success
+ {
+ std_msgs::Int32 msg;
+ msg.data = CMD_USE_CRAZYFLY_DISABLE;
+ this->PPSClientCommadPublisher.publish(msg);
+ ui->enable_disable_CF_button->setText("DisableCF");
+ }
+ else //disabled, enable if success
+ {
+ std_msgs::Int32 msg;
+ msg.data = CMD_USE_CRAZYFLY_ENABLE;
+ this->PPSClientCommadPublisher.publish(msg);
+ ui->enable_disable_CF_button->setText("EnableCF");
+ }
+}
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 4e917c47e62af01aa6b4e61c4b5dcf53f1a5caad..3ce33712809ec7990a0ffe2baa3f548cd81c0d5a 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
@@ -16,7 +16,7 @@
<widget class="QWidget" name="centralWidget">
<layout class="QGridLayout" name="gridLayout">
<item row="0" column="2">
- <widget class="QLabel" name="label_2">
+ <widget class="QLabel" name="connected_disconnected_label">
<property name="text">
<string>Connected/Disconnected RF status</string>
</property>
@@ -66,9 +66,9 @@
</widget>
</item>
<item row="0" column="1">
- <widget class="QPushButton" name="pushButton_2">
+ <widget class="QPushButton" name="enable_disable_CF_button">
<property name="text">
- <string>Enable/Disable CF</string>
+ <string>EnableCF</string>
</property>
</widget>
</item>
@@ -86,13 +86,6 @@
</property>
</widget>
</item>
- <item row="0" column="2">
- <widget class="QLabel" name="label_3">
- <property name="text">
- <string>Enable/Disable CF status?</string>
- </property>
- </widget>
- </item>
<item row="2" column="2">
<widget class="QGroupBox" name="groupBox_3">
<property name="title">
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro.user b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro.user
index 63612b10c6da7372bd4eaa59a81ac1e63b08dfaf..0999881d56c61ab3547dbcefd32761dd4e1b19c8 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro.user
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro.user
@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 4.0.2, 2017-08-21T19:05:09. -->
+<!-- Written by QtCreator 4.0.2, 2017-08-22T14:42:27. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>
diff --git a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
index 4d00d115dc5f1f3b50ead4c9adfd1a2fba1e6bcf..76cc15138272cb1974c67f3d44fb60790026721b 100755
--- a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
+++ b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
@@ -69,25 +69,34 @@ class PPSRadioClient:
self.motor2cmd = 0.0
self.motor3cmd = 0.0
self.motor4cmd = 0.0
- self.status = DISCONNECTED
+ self._status = DISCONNECTED
self.link_uri = link_uri
+ self.status_pub = rospy.Publisher('CrazyRadioStatus', Int32, queue_size=1)
+
# Initialize the CrazyFlie and add callbacks
self._cf = Crazyflie()
- # Add callbacks that get executed depending on the connection status.
+ # Add callbacks that get executed depending on the connection _status.
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
+ self.change_status_to(DISCONNECTED)
# Connect to the Crazyflie
print "Connecting to %s" % link_uri
+ self.connect()
+
+ def change_status_to(self, new_status):
+ self._status = new_status
+ self.status_pub.publish(new_status)
- self.connect();
+ def get_status(self):
+ return self._status
def connect(self):
- self.status = CONNECTING
+ self.change_status_to(CONNECTING)
rospy.loginfo("connecting...")
self._cf.open_link(self.link_uri)
@@ -119,7 +128,7 @@ class PPSRadioClient:
This callback is executed as soon as the connection to the
quadrotor is established.
"""
- self.status = CONNECTED
+ self.change_status_to(CONNECTED)
rospy.loginfo("Connection to %s successful: " % link_uri)
@@ -143,19 +152,18 @@ class PPSRadioClient:
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified address)"""
- self.status = DISCONNECTED
+ self.change_status_to(DISCONNECTED)
rospy.logerr("Connection to %s failed: %s" % (link_uri, msg))
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
- self.status = DISCONNECTED
+ self.change_status_to(DISCONNECTED)
rospy.logerr("Connection to %s lost: %s" % (link_uri, msg))
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
-
- self.status = DISCONNECTED
+ self.change_status_to(DISCONNECTED)
rospy.logwarn("Disconnected from %s" % link_uri)
bag.close()
rospy.loginfo("bag closed")
@@ -170,10 +178,10 @@ 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
+ 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
- if self.status == DISCONNECTED:
+ print "_status: %s" % self._status
+ if self.get_status() == DISCONNECTED:
print "entered disconnected"
self.connect()