MainWindow.cpp 5.02 KB
Newer Older
roangel's avatar
roangel committed
1
2
#include "MainWindow.h"
#include "ui_MainWindow.h"
3
#include <string>
roangel's avatar
roangel committed
4

5
6
7
#include <ros/ros.h>
#include <ros/network.h>

8
MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
roangel's avatar
roangel committed
9
    QMainWindow(parent),
10
    ui(new Ui::MainWindow),
11
    m_battery_level(0)
roangel's avatar
roangel committed
12
{
13
    m_rosNodeThread = new rosNodeThread(argc, argv, "student_GUI");
roangel's avatar
roangel committed
14
    ui->setupUi(this);
15
    m_rosNodeThread->init();
16

17
18
19
    setCrazyRadioStatus(DISCONNECTED);


20
21
22
    std::string ros_namespace = ros::this_node::getNamespace();
    ROS_INFO("namespace: %s", ros_namespace.c_str());

23
24
    qRegisterMetaType<ptrToMessage>("ptrToMessage");
    QObject::connect(m_rosNodeThread, SIGNAL(newViconData(const ptrToMessage&)), this, SLOT(updateNewViconData(const ptrToMessage&)));
25

roangel's avatar
roangel committed
26
27
    ros::NodeHandle nodeHandle(ros_namespace);
    crazyRadioStatusSubscriber = nodeHandle.subscribe("CrazyRadio/CrazyRadioStatus", 1, &MainWindow::crazyRadioStatusCallback, this);
28

29
30
    CFBatterySubscriber = nodeHandle.subscribe("CrazyRadio/CFBattery", 1, &MainWindow::CFBatteryCallback, this);

31
32
    flyingStateSubscriber = nodeHandle.subscribe("PPSClient/flyingState", 1, &MainWindow::flyingStateChangedCallback, this);

33

34
    // communication with PPS Client, just to make it possible to communicate through terminal also we use PPSClient's name
35
36
    ros::NodeHandle nh_PPSClient(ros_namespace + "/PPSClient");
    crazyRadioCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("crazyRadioCommand", 1);
37
    PPSClientCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("Command", 1);
38
39

    disableGUI();
roangel's avatar
roangel committed
40
41
}

42

roangel's avatar
roangel committed
43
44
45
46
MainWindow::~MainWindow()
{
    delete ui;
}
47

48
49
50
51
52
53
54
55
void MainWindow::disableGUI()
{
}

void MainWindow::enableGUI()
{
}

56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
void MainWindow::flyingStateChangedCallback(const std_msgs::Int32& msg)
{
    QString qstr = "Flying State: ";
    switch(msg.data)
    {
        case STATE_MOTORS_OFF:
            qstr.append("Motors OFF");
            break;
        case STATE_TAKE_OFF:
            qstr.append("Take OFF");
            break;
        case STATE_FLYING:
            qstr.append("Flying");
            break;
        case STATE_LAND:
            qstr.append("Land");
            break;
        default:
            break;
    }
    ui->flying_state_label->setText(qstr);
}

79
80
81
82
83
84
85
86
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!");
87
            enableGUI();
88
89
90
91
92
93
94
95
            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");
96
            disableGUI();
97
98
99
100
101
102
103
104
            break;
        default:
    		ROS_ERROR_STREAM("unexpected radio status: " << m_radio_status);
            break;
    }
    this->m_radio_status = radio_status;
}

105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
float MainWindow::fromVoltageToPercent(float voltage)
{
    int num_cutoffs = m_cutoff_voltages.size();
    float hysteresis = 0.05;

    while(m_battery_level < num_cutoffs && voltage >= m_cutoff_voltages[m_battery_level])
    {
        ++m_battery_level;
    }
    while(m_battery_level > 0 && voltage < m_cutoff_voltages[m_battery_level - 1] - hysteresis)
    {
        --m_battery_level;
    }

    float percentage = 100.0 * m_battery_level/num_cutoffs;
120
121
122
123
124
125
126

    // should not hapen, but just in case...
    if(percentage > 100)
        percentage = 100;
    if(percentage < 0)
        percentage = 0;

127
128
129
130
131
132
133
134
    return percentage;
}

void MainWindow::updateBatteryVoltage(float battery_voltage)
{
    m_battery_voltage = battery_voltage;
    // Need to take voltage, display it and transform it to percentage
    ui->battery_bar->setValue(fromVoltageToPercent(m_battery_voltage));
135
136
137
    QString qstr = "Raw voltage: ";
    qstr.append(QString::number(battery_voltage));
    ui->raw_voltage->setText(qstr);
138
139
140
141
142
143
144
}

void MainWindow::CFBatteryCallback(const std_msgs::Float32& msg)
{
    updateBatteryVoltage(msg.data);
}

145
146
void MainWindow::crazyRadioStatusCallback(const std_msgs::Int32& msg)
{
147
    ROS_INFO("Callback CrazyRadioStatus called");
148
149
150
    this->setCrazyRadioStatus(msg.data);
}

151
152
153
void MainWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to newViconData, from node
{
}
154
155
156
157
158

void MainWindow::on_RF_Connect_button_clicked()
{
    std_msgs::Int32 msg;
    msg.data = CMD_RECONNECT;
159
    this->crazyRadioCommandPublisher.publish(msg);
roangel's avatar
roangel committed
160
    ROS_INFO("command reconnect published");
161
}
162

163
void MainWindow::on_take_off_button_clicked()
164
{
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
    std_msgs::Int32 msg;
    msg.data = CMD_CRAZYFLY_TAKE_OFF;
    this->PPSClientCommandPublisher.publish(msg);
}

void MainWindow::on_land_button_clicked()
{
    std_msgs::Int32 msg;
    msg.data = CMD_CRAZYFLY_LAND;
    this->PPSClientCommandPublisher.publish(msg);
}

void MainWindow::on_motors_OFF_button_clicked()
{
    std_msgs::Int32 msg;
    msg.data = CMD_CRAZYFLY_MOTORS_OFF;
    this->PPSClientCommandPublisher.publish(msg);
182
}