Commit 0e87f28d authored by Yvan Bosshard's avatar Yvan Bosshard
Browse files

UWB Status is now selected by the student

parent a1d4c864
...@@ -102,6 +102,8 @@ private slots: ...@@ -102,6 +102,8 @@ private slots:
void on_en_safe_controller_clicked(); void on_en_safe_controller_clicked();
void on_checkBox_enable_UWB_toggled(int state);
private: private:
Ui::MainWindow *ui; Ui::MainWindow *ui;
...@@ -143,6 +145,10 @@ private: ...@@ -143,6 +145,10 @@ private:
ros::Subscriber controllerUsedSubscriber; ros::Subscriber controllerUsedSubscriber;
ros::ServiceClient centralManager; ros::ServiceClient centralManager;
ros::ServiceClient UWBServiceClient;
ros::Subscriber UWBChangedSubscriber;
ros::Publisher UWBActivatedPublisher;
// callbacks // callbacks
void crazyRadioStatusCallback(const std_msgs::Int32& msg); void crazyRadioStatusCallback(const std_msgs::Int32& msg);
...@@ -155,6 +161,7 @@ private: ...@@ -155,6 +161,7 @@ private:
void safeYamlFileTimerCallback(const ros::TimerEvent&); void safeYamlFileTimerCallback(const ros::TimerEvent&);
void controllerUsedChangedCallback(const std_msgs::Int32& msg); void controllerUsedChangedCallback(const std_msgs::Int32& msg);
void batteryStateChangedCallback(const std_msgs::Int32& msg); void batteryStateChangedCallback(const std_msgs::Int32& msg);
void uwbChangedCallback(const std_msgs::Int32& msg);
float fromVoltageToPercent(float voltage); float fromVoltageToPercent(float voltage);
void updateBatteryVoltage(float battery_voltage); void updateBatteryVoltage(float battery_voltage);
...@@ -168,6 +175,8 @@ private: ...@@ -168,6 +175,8 @@ private:
void highlightSafeControllerTab(); void highlightSafeControllerTab();
void highlightCustomControllerTab(); void highlightCustomControllerTab();
void updateUWBState();
const std::vector<float> m_cutoff_voltages {3.1966, 3.2711, 3.3061, 3.3229, 3.3423, 3.3592, 3.3694, 3.385, 3.4006, 3.4044, 3.4228, 3.4228, 3.4301, 3.4445, 3.4531, 3.4677, 3.4705, 3.4712, 3.4756, 3.483, 3.4944, 3.5008, 3.5008, 3.5084, 3.511, 3.5122, 3.5243, 3.5329, 3.5412, 3.5529, 3.5609, 3.5625, 3.5638, 3.5848, 3.6016, 3.6089, 3.6223, 3.628, 3.6299, 3.6436, 3.6649, 3.6878, 3.6983, 3.7171, 3.7231, 3.7464, 3.7664, 3.7938, 3.8008, 3.816, 3.8313, 3.8482, 3.866, 3.8857, 3.8984, 3.9159, 3.9302, 3.9691, 3.997, 4.14 }; const std::vector<float> m_cutoff_voltages {3.1966, 3.2711, 3.3061, 3.3229, 3.3423, 3.3592, 3.3694, 3.385, 3.4006, 3.4044, 3.4228, 3.4228, 3.4301, 3.4445, 3.4531, 3.4677, 3.4705, 3.4712, 3.4756, 3.483, 3.4944, 3.5008, 3.5008, 3.5084, 3.511, 3.5122, 3.5243, 3.5329, 3.5412, 3.5529, 3.5609, 3.5625, 3.5638, 3.5848, 3.6016, 3.6089, 3.6223, 3.628, 3.6299, 3.6436, 3.6649, 3.6878, 3.6983, 3.7171, 3.7231, 3.7464, 3.7664, 3.7938, 3.8008, 3.816, 3.8313, 3.8482, 3.866, 3.8857, 3.8984, 3.9159, 3.9302, 3.9691, 3.997, 4.14 };
}; };
......
...@@ -24,6 +24,7 @@ ...@@ -24,6 +24,7 @@
#include <ros/package.h> #include <ros/package.h>
#include "d_fall_pps/CMQuery.h" #include "d_fall_pps/CMQuery.h"
#include "d_fall_pps/Anchors.h"
#include "d_fall_pps/ViconData.h" #include "d_fall_pps/ViconData.h"
...@@ -110,63 +111,39 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) : ...@@ -110,63 +111,39 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
highlightSafeControllerTab(); highlightSafeControllerTab();
ui->label_battery->setStyleSheet("QLabel { color : red; }"); ui->label_battery->setStyleSheet("QLabel { color : red; }");
m_battery_state = BATTERY_STATE_NORMAL; m_battery_state = BATTERY_STATE_NORMAL;
}
MainWindow::~MainWindow()
{
delete ui;
}
void MainWindow::disableGUI() // NEW STUFF
{
ui->motors_OFF_button->setEnabled(false);
ui->take_off_button->setEnabled(false);
ui->land_button->setEnabled(false);
}
void MainWindow::enableGUI() UWBServiceClient = nodeHandle.serviceClient<Anchors>("/UWBManagerService/UWBData", false);
{ UWBActivatedPublisher = my_nodeHandle.advertise<std_msgs::Int32>("uwbChanged", 1);
ui->motors_OFF_button->setEnabled(true); updateUWBState();
if(m_battery_state == BATTERY_STATE_NORMAL)
{ UWBChangedSubscriber = nodeHandle.subscribe("/my_GUI/enableUWB", 1, &MainWindow::uwbChangedCallback, this);
ui->take_off_button->setEnabled(true);
ui->land_button->setEnabled(true); QObject::connect(ui->checkBox_enable_UWB, SIGNAL(stateChanged(int)), this, SLOT(on_checkBox_enable_UWB_toggled(int)));
}
} }
/*********************************
UI Callbacks
*********************************/
void MainWindow::highlightSafeControllerTab() void MainWindow::highlightSafeControllerTab()
{ {
ui->tabWidget->tabBar()->setTabTextColor(0, Qt::green); ui->tabWidget->tabBar()->setTabTextColor(0, Qt::green);
ui->tabWidget->tabBar()->setTabTextColor(1, Qt::black); ui->tabWidget->tabBar()->setTabTextColor(1, Qt::black);
} }
void MainWindow::highlightCustomControllerTab() void MainWindow::highlightCustomControllerTab()
{ {
ui->tabWidget->tabBar()->setTabTextColor(0, Qt::black); ui->tabWidget->tabBar()->setTabTextColor(0, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(1, Qt::green); ui->tabWidget->tabBar()->setTabTextColor(1, Qt::green);
} }
void MainWindow::DBChangedCallback(const std_msgs::Int32& msg)
{
loadCrazyflieContext();
ROS_INFO("context reloaded in student_GUI");
}
void MainWindow::controllerUsedChangedCallback(const std_msgs::Int32& msg)
{
switch(msg.data)
{
case SAFE_CONTROLLER:
highlightSafeControllerTab();
break;
case CUSTOM_CONTROLLER:
highlightCustomControllerTab();
break;
default:
break;
}
}
void MainWindow::safeSetpointCallback(const Setpoint& newSetpoint) void MainWindow::safeSetpointCallback(const Setpoint& newSetpoint)
{ {
m_safe_setpoint = newSetpoint; m_safe_setpoint = newSetpoint;
...@@ -238,7 +215,6 @@ void MainWindow::batteryStateChangedCallback(const std_msgs::Int32& msg) ...@@ -238,7 +215,6 @@ void MainWindow::batteryStateChangedCallback(const std_msgs::Int32& msg)
} }
} }
void MainWindow::setCrazyRadioStatus(int radio_status) void MainWindow::setCrazyRadioStatus(int radio_status)
{ {
// add more things whenever the status is changed // add more things whenever the status is changed
...@@ -259,37 +235,12 @@ void MainWindow::setCrazyRadioStatus(int radio_status) ...@@ -259,37 +235,12 @@ void MainWindow::setCrazyRadioStatus(int radio_status)
disableGUI(); disableGUI();
break; break;
default: default:
ROS_ERROR_STREAM("unexpected radio status: " << m_radio_status); ROS_ERROR_STREAM("unexpected radio status: " << m_radio_status);
break; break;
} }
this->m_radio_status = radio_status; this->m_radio_status = radio_status;
} }
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;
// should not hapen, but just in case...
if(percentage > 100)
percentage = 100;
if(percentage < 0)
percentage = 0;
return percentage;
}
void MainWindow::updateBatteryVoltage(float battery_voltage) void MainWindow::updateBatteryVoltage(float battery_voltage)
{ {
m_battery_voltage = battery_voltage; m_battery_voltage = battery_voltage;
...@@ -315,81 +266,6 @@ void MainWindow::crazyRadioStatusCallback(const std_msgs::Int32& msg) ...@@ -315,81 +266,6 @@ void MainWindow::crazyRadioStatusCallback(const std_msgs::Int32& msg)
this->setCrazyRadioStatus(msg.data); this->setCrazyRadioStatus(msg.data);
} }
void MainWindow::loadCrazyflieContext()
{
CMQuery contextCall;
contextCall.request.studentID = m_student_id;
ROS_INFO_STREAM("StudentID:" << m_student_id);
centralManager.waitForExistence(ros::Duration(-1));
if(centralManager.call(contextCall))
{
m_context = contextCall.response.crazyflieContext;
ROS_INFO_STREAM("CrazyflieContext:\n" << m_context);
// we now have the m_context variable with the current context. Put CF Name in label
QString qstr = "StudentID ";
qstr.append(QString::number(m_student_id));
qstr.append(" connected to CF ");
qstr.append(QString::fromStdString(m_context.crazyflieName));
ui->groupBox->setTitle(qstr);
}
else
{
ROS_ERROR("Failed to load context");
}
ros::NodeHandle nh("CrazyRadio");
nh.setParam("crazyFlieAddress", m_context.crazyflieAddress);
}
void MainWindow::coordinatesToLocal(CrazyflieData& cf)
{
AreaBounds area = m_context.localArea;
float originX = (area.xmin + area.xmax) / 2.0;
float originY = (area.ymin + area.ymax) / 2.0;
// change Z origin to zero, i.e., to the table height, zero of global coordinates, instead of middle of the box
float originZ = 0.0;
// float originZ = (area.zmin + area.zmax) / 2.0;
cf.x -= originX;
cf.y -= originY;
cf.z -= originZ;
}
void MainWindow::updatePositionData(const CrazyflieData& data)
{
CrazyflieData local = data;
coordinatesToLocal(local);
// now we have the local coordinates, put them in the labels
ui->current_x->setText(QString::number(local.x, 'f', 3));
ui->current_y->setText(QString::number(local.y, 'f', 3));
ui->current_z->setText(QString::number(local.z, 'f', 3));
ui->current_yaw->setText(QString::number(local.yaw * RAD2DEG, 'f', 1));
ui->current_pitch->setText(QString::number(local.pitch * RAD2DEG, 'f', 1));
ui->current_roll->setText(QString::number(local.roll * RAD2DEG, 'f', 1));
ui->current_x_2->setText(QString::number(local.x, 'f', 3));
ui->current_y_2->setText(QString::number(local.y, 'f', 3));
ui->current_z_2->setText(QString::number(local.z, 'f', 3));
ui->current_yaw_2->setText(QString::number(local.yaw * RAD2DEG, 'f', 1));
ui->current_pitch_2->setText(QString::number(local.pitch * RAD2DEG, 'f', 1));
ui->current_roll_2->setText(QString::number(local.roll * RAD2DEG, 'f', 1));
// also update diff
ui->diff_x->setText(QString::number(m_safe_setpoint.x - local.x, 'f', 3));
ui->diff_y->setText(QString::number(m_safe_setpoint.y - local.y, 'f', 3));
ui->diff_z->setText(QString::number(m_safe_setpoint.z - local.z, 'f', 3));
ui->diff_yaw->setText(QString::number((m_safe_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 1));
ui->diff_x_2->setText(QString::number(m_custom_setpoint.x - local.x, 'f', 3));
ui->diff_y_2->setText(QString::number(m_custom_setpoint.y - local.y, 'f', 3));
ui->diff_z_2->setText(QString::number(m_custom_setpoint.z - local.z, 'f', 3));
ui->diff_yaw_2->setText(QString::number((m_custom_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 1));
}
void MainWindow::on_RF_Connect_button_clicked() void MainWindow::on_RF_Connect_button_clicked()
{ {
std_msgs::Int32 msg; std_msgs::Int32 msg;
...@@ -518,10 +394,199 @@ void MainWindow::on_en_custom_controller_clicked() ...@@ -518,10 +394,199 @@ void MainWindow::on_en_custom_controller_clicked()
this->PPSClientCommandPublisher.publish(msg); this->PPSClientCommandPublisher.publish(msg);
} }
void MainWindow::on_en_safe_controller_clicked() void MainWindow::on_en_safe_controller_clicked()
{ {
std_msgs::Int32 msg; std_msgs::Int32 msg;
msg.data = CMD_USE_SAFE_CONTROLLER; msg.data = CMD_USE_SAFE_CONTROLLER;
this->PPSClientCommandPublisher.publish(msg); this->PPSClientCommandPublisher.publish(msg);
} }
void MainWindow::on_checkBox_enable_UWB_toggled(int state)
{
std_msgs::Int32 msg;
msg.data = ui->checkBox_enable_UWB->isChecked();
UWBActivatedPublisher.publish(msg);
}
/*********************************
UWB functions
*********************************/
void MainWindow::updateUWBState()
{
// Load UWB Settings from the UWBManagerService
Anchors a;
if(UWBServiceClient.call(a))
{
ui->checkBox_enable_UWB->setCheckable(a.response.enableUWB);
}
else
{
ROS_ERROR("[Student GUI] Could not update UWB Settings!");
return;
}
std_msgs::Int32 msg;
msg.data = ui->checkBox_enable_UWB->isChecked();
UWBActivatedPublisher.publish(msg);
}
void MainWindow::uwbChangedCallback(const std_msgs::Int32& msg)
{
updateUWBState();
}
/*********************************
Other random functions
*********************************/
MainWindow::~MainWindow()
{
delete ui;
}
void MainWindow::disableGUI()
{
ui->motors_OFF_button->setEnabled(false);
ui->take_off_button->setEnabled(false);
ui->land_button->setEnabled(false);
}
void MainWindow::enableGUI()
{
ui->motors_OFF_button->setEnabled(true);
if(m_battery_state == BATTERY_STATE_NORMAL)
{
ui->take_off_button->setEnabled(true);
ui->land_button->setEnabled(true);
}
}
void MainWindow::DBChangedCallback(const std_msgs::Int32& msg)
{
loadCrazyflieContext();
ROS_INFO("context reloaded in student_GUI");
}
void MainWindow::controllerUsedChangedCallback(const std_msgs::Int32& msg)
{
switch(msg.data)
{
case SAFE_CONTROLLER:
highlightSafeControllerTab();
break;
case CUSTOM_CONTROLLER:
highlightCustomControllerTab();
break;
default:
break;
}
}
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;
// should not hapen, but just in case...
if(percentage > 100)
percentage = 100;
if(percentage < 0)
percentage = 0;
return percentage;
}
void MainWindow::loadCrazyflieContext()
{
CMQuery contextCall;
contextCall.request.studentID = m_student_id;
ROS_INFO_STREAM("StudentID:" << m_student_id);
centralManager.waitForExistence(ros::Duration(-1));
if(centralManager.call(contextCall))
{
m_context = contextCall.response.crazyflieContext;
ROS_INFO_STREAM("CrazyflieContext:\n" << m_context);
// we now have the m_context variable with the current context. Put CF Name in label
QString qstr = "StudentID ";
qstr.append(QString::number(m_student_id));
qstr.append(" connected to CF ");
qstr.append(QString::fromStdString(m_context.crazyflieName));
ui->groupBox->setTitle(qstr);
}
else
{
ROS_ERROR("Failed to load context");
}
ros::NodeHandle nh("CrazyRadio");
nh.setParam("crazyFlieAddress", m_context.crazyflieAddress);
}
void MainWindow::coordinatesToLocal(CrazyflieData& cf)
{
AreaBounds area = m_context.localArea;
float originX = (area.xmin + area.xmax) / 2.0;
float originY = (area.ymin + area.ymax) / 2.0;
// change Z origin to zero, i.e., to the table height, zero of global coordinates, instead of middle of the box
float originZ = 0.0;
// float originZ = (area.zmin + area.zmax) / 2.0;
cf.x -= originX;
cf.y -= originY;
cf.z -= originZ;
}
void MainWindow::updatePositionData(const CrazyflieData& data)
{
CrazyflieData local = data;
coordinatesToLocal(local);
// now we have the local coordinates, put them in the labels
ui->current_x->setText(QString::number(local.x, 'f', 3));
ui->current_y->setText(QString::number(local.y, 'f', 3));
ui->current_z->setText(QString::number(local.z, 'f', 3));
ui->current_yaw->setText(QString::number(local.yaw * RAD2DEG, 'f', 1));
ui->current_pitch->setText(QString::number(local.pitch * RAD2DEG, 'f', 1));
ui->current_roll->setText(QString::number(local.roll * RAD2DEG, 'f', 1));
ui->current_x_2->setText(QString::number(local.x, 'f', 3));
ui->current_y_2->setText(QString::number(local.y, 'f', 3));
ui->current_z_2->setText(QString::number(local.z, 'f', 3));
ui->current_yaw_2->setText(QString::number(local.yaw * RAD2DEG, 'f', 1));
ui->current_pitch_2->setText(QString::number(local.pitch * RAD2DEG, 'f', 1));
ui->current_roll_2->setText(QString::number(local.roll * RAD2DEG, 'f', 1));
// also update diff
ui->diff_x->setText(QString::number(m_safe_setpoint.x - local.x, 'f', 3));
ui->diff_y->setText(QString::number(m_safe_setpoint.y - local.y, 'f', 3));
ui->diff_z->setText(QString::number(m_safe_setpoint.z - local.z, 'f', 3));
ui->diff_yaw->setText(QString::number((m_safe_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 1));
ui->diff_x_2->setText(QString::number(m_custom_setpoint.x - local.x, 'f', 3));
ui->diff_y_2->setText(QString::number(m_custom_setpoint.y - local.y, 'f', 3));
ui->diff_z_2->setText(QString::number(m_custom_setpoint.z - local.z, 'f', 3));
ui->diff_yaw_2->setText(QString::number((m_custom_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 1));
}
...@@ -1190,6 +1190,43 @@ ...@@ -1190,6 +1190,43 @@
</property> </property>
</widget> </widget>
</item> </item>
<!--<item row="1" column="2">
<widget class="QLabel" name="label1234">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="font">
<font>
<pointsize>18</pointsize>
<italic>false</italic>
</font>
</property>
<property name="text">
<string>Localisation Settings</string>
</property>
</widget>
</item>-->
<item row="1" column="3">
<widget class="QCheckBox" name="checkBox_enable_UWB">
<!--<property name="font">
<font>
<pointsize>7</pointsize>
</font>
</property>-->
<property name="text">
<string>Enable UWB Positioning</string>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
</layout> </layout>
</widget> </widget>
</item> </item>
......
...@@ -58,7 +58,10 @@ int main(int argc, char* argv[]) ...@@ -58,7 +58,10 @@ int main(int argc, char* argv[])
ros::Subscriber dbChangedSubscriber = namespaceNodeHandle.subscribe("/my_GUI/DBChanged", 1, dbChangedCallback); ros::Subscriber dbChangedSubscriber = namespaceNodeHandle.subscribe("/my_GUI/DBChanged", 1, dbChangedCallback);
uwbManager = nodeHandle.serviceClient<Anchors>("/UWBManagerService/UWBData", false); uwbManager = nodeHandle.serviceClient<Anchors>("/UWBManagerService/UWBData", false);
ros::Subscriber enableUWBSubscriber = nodeHandle.subscribe("/my_GUI/enableUWB", 1, uwbChangedCallback);
ros::Subscriber enableUWBSubscriber = namespaceNodeHandle.subscribe("student_GUI/uwbChanged", 1, uwbChangedCallback);
//ros::Subscriber enableUWBSubscriber = nodeHandle.subscribe("/my_GUI/enableUWB", 1, uwbChangedCallback);
ROS_WARN("Topic: %s", enableUWBSubscriber.getTopic().c_str());
loadUWBSettings(); loadUWBSettings();
// subscribe to the global vicon publisher and local uwb publisher // subscribe to the global vicon publisher and local uwb publisher
...@@ -181,17 +184,7 @@ void d_fall_pps::dbChangedCallback(const std_msgs::Int32 &msg) ...@@ -181,17 +184,7 @@ void d_fall_pps::dbChangedCallback(const std_msgs::Int32 &msg)