Commit 7af9dcb9 authored by roangel's avatar roangel
Browse files

Done feedback connect disconnect and enable/disable CF. Need to test it

parent 7cdba94c
......@@ -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
......@@ -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");
}
}
......@@ -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">
......
<?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>
......
......@@ -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()
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment