Commit 4321bf3b authored by tiagos's avatar tiagos
Browse files

Added interface for self localisation

parent 5b34d756
...@@ -32,6 +32,7 @@ ...@@ -32,6 +32,7 @@
#include "d_fall_pps/CrazyflieDB.h" #include "d_fall_pps/CrazyflieDB.h"
#include "d_fall_pps/CrazyflieEntry.h" #include "d_fall_pps/CrazyflieEntry.h"
#include "d_fall_pps/Anchors.h"
#include <std_msgs/Int32.h> #include <std_msgs/Int32.h>
#include <std_msgs/Int32MultiArray.h> #include <std_msgs/Int32MultiArray.h>
...@@ -132,6 +133,8 @@ private slots: ...@@ -132,6 +133,8 @@ private slots:
void on_checkBox_enable_UWB_toggled(bool checked); void on_checkBox_enable_UWB_toggled(bool checked);
void on_calibrateAnchors_button_pressed();
void on_reloadAnchors_button_pressed(); void on_reloadAnchors_button_pressed();
#ifdef CATKIN_MAKE #ifdef CATKIN_MAKE
...@@ -204,6 +207,9 @@ private: ...@@ -204,6 +207,9 @@ private:
void insert_or_update_entry_database(CrazyflieEntry entry); void insert_or_update_entry_database(CrazyflieEntry entry);
void updateUWBSettings(bool enableChecked);
void updateAnchors(const Anchors* const a);
}; };
......
...@@ -36,7 +36,7 @@ ...@@ -36,7 +36,7 @@
#include "d_fall_pps/CMUpdate.h" #include "d_fall_pps/CMUpdate.h"
#include "d_fall_pps/CMCommand.h" #include "d_fall_pps/CMCommand.h"
#include "CentralManagerService.h" #include "CentralManagerService.h"
#include "d_fall_pps/Anchors.h"
#include <ros/ros.h> #include <ros/ros.h>
#include <ros/network.h> #include <ros/network.h>
...@@ -44,6 +44,7 @@ ...@@ -44,6 +44,7 @@
#endif #endif
//#include <iomanip>
#include <string> #include <string>
#define N_MAX_CRAZYFLIES 20 // protection number #define N_MAX_CRAZYFLIES 20 // protection number
...@@ -51,6 +52,7 @@ ...@@ -51,6 +52,7 @@
#define UWB_UPDATE_DISABLE 0 #define UWB_UPDATE_DISABLE 0
#define UWB_UPDATE_ENABLE 1 #define UWB_UPDATE_ENABLE 1
#define UWB_UPDATE_ANCHORS 5 #define UWB_UPDATE_ANCHORS 5
#define UWB_CALIBRATE_ANCHORS 7
#ifdef CATKIN_MAKE #ifdef CATKIN_MAKE
using namespace d_fall_pps; using namespace d_fall_pps;
...@@ -236,6 +238,22 @@ void MainGUIWindow::_init() ...@@ -236,6 +238,22 @@ void MainGUIWindow::_init()
QObject::connect(ui->checkBox_enable_UWB, SIGNAL(stateChanged(int)), this, SLOT(on_checkBox_enable_UWB_toggled(bool))); QObject::connect(ui->checkBox_enable_UWB, SIGNAL(stateChanged(int)), this, SLOT(on_checkBox_enable_UWB_toggled(bool)));
QObject::connect(ui->reloadAnchors_button, SIGNAL(pressed()), this, SLOT(on_reloadAnchors_button_pressed())); QObject::connect(ui->reloadAnchors_button, SIGNAL(pressed()), this, SLOT(on_reloadAnchors_button_pressed()));
QObject::connect(ui->calibrateAnchors_button, SIGNAL(pressed()), this, SLOT(on_calibrateAnchors_button_pressed()));
// add space bar shortcut to stop all motors
ui->all_motors_off_button->setShortcut(tr("Space"));
// set anchor table settings
ui->table_anchorPos->setColumnCount(4);
ui->table_anchorPos->horizontalHeader()->setFont(fnt);
ui->table_anchorPos->verticalHeader()->setFont(fnt);
ui->table_anchorPos->horizontalHeader()->setDefaultSectionSize(90);
ui->table_anchorPos->verticalHeader()->setDefaultSectionSize(20);
QStringList anchor_header;
anchor_header << "Anchor id" << "x" << "y" << "z";
ui->table_anchorPos->setHorizontalHeaderLabels(anchor_header);
#endif #endif
} }
...@@ -977,44 +995,18 @@ void MainGUIWindow::on_load_from_DB_button_clicked() ...@@ -977,44 +995,18 @@ void MainGUIWindow::on_load_from_DB_button_clicked()
} }
} }
void MainGUIWindow::on_checkBox_enable_UWB_toggled(bool checked) void MainGUIWindow::updateUWBSettings(bool enableChecked)
{ {
// Send Command to enable UWB to UWBManagerService
std_msgs::Int32 server_msg;
if(checked)
server_msg.data = UWB_UPDATE_ENABLE;
else
server_msg.data = UWB_UPDATE_DISABLE;
UWBServiceClientUpdate_publisher.publish(server_msg);
// Receive the updated information // Receive the updated information
Anchors a; Anchors a;
if(UWBServiceClient.call(a)) if(UWBServiceClient.call(a))
{ {
ui->checkBox_enable_UWB->setChecked(a.response.enableUWB); updateAnchors(&a);
// print anchor positions in list_anchors
ui->list_anchors->clear();
std::ostringstream strm;
strm << "id\tx\ty\tz\n";
for(int i = 0; i < a.response.anchorArray.length; ++i)
{
strm << a.response.anchorArray.data[i].id << "\t" << a.response.anchorArray.data[i].x << "\t"
<< a.response.anchorArray.data[i].y << "\t" << a.response.anchorArray.data[i].z << "\n";
}
ui->list_anchors->addItem(QString::fromStdString(strm.str()));
} }
else else
{ {
ui->checkBox_enable_UWB->setChecked(!checked); ui->checkBox_enable_UWB->setChecked(!enableChecked);
ROS_ERROR("[Teacher GUI] Could not update UWB Settings!"); ROS_ERROR("[Teacher GUI] Could not update UWB Settings!");
return; return;
} }
...@@ -1025,6 +1017,70 @@ void MainGUIWindow::on_checkBox_enable_UWB_toggled(bool checked) ...@@ -1025,6 +1017,70 @@ void MainGUIWindow::on_checkBox_enable_UWB_toggled(bool checked)
UWBSettings_publisher.publish(msg); UWBSettings_publisher.publish(msg);
} }
void MainGUIWindow::updateAnchors(const Anchors* const a)
{
ui->table_anchorPos->setRowCount(0);
ui->checkBox_enable_UWB->setChecked(a->response.enableUWB);
for(int i = 0; i < a->response.anchorArray.length; ++i)
{
// create new row
ui->table_anchorPos->insertRow(i);
// add the id
QString a_id = QString::number(a->response.anchorArray.data[i].id);
QTableWidgetItem* item_id = new QTableWidgetItem(a_id);
item_id->setFlags(item_id->flags() & ~Qt::ItemIsEditable);
item_id->setTextAlignment(Qt::AlignLeft);
ui->table_anchorPos->setItem(i, 0, item_id);
// add x component
std::stringstream strm;
strm << std::fixed << std::setprecision(3) << a->response.anchorArray.data[i].x / 10.0;
QString a_x = QString::fromStdString(strm.str());
strm.str(std::string());
QTableWidgetItem* item_x = new QTableWidgetItem(a_x);
item_x->setTextAlignment(Qt::AlignRight);
item_x->setFlags(item_x->flags() & ~Qt::ItemIsEditable);
ui->table_anchorPos->setItem(i, 1, item_x);
// add y component
strm << a->response.anchorArray.data[i].y / 10.0;
QString a_y = QString::fromStdString(strm.str());
strm.str(std::string());
QTableWidgetItem* item_y = new QTableWidgetItem(a_y);
item_y->setTextAlignment(Qt::AlignRight);
item_y->setFlags(item_y->flags() & ~Qt::ItemIsEditable);
ui->table_anchorPos->setItem(i, 2, item_y);
// add z component
strm << a->response.anchorArray.data[i].z / 10.0;
QString a_z = QString::fromStdString(strm.str());
strm.str(std::string());
QTableWidgetItem* item_z = new QTableWidgetItem(a_z);
item_z->setTextAlignment(Qt::AlignRight);
item_z->setFlags(item_z->flags() & ~Qt::ItemIsEditable);
ui->table_anchorPos->setItem(i, 3, item_z);
}
}
void MainGUIWindow::on_checkBox_enable_UWB_toggled(bool checked)
{
// Send Command to enable UWB to UWBManagerService
std_msgs::Int32 server_msg;
if(checked)
server_msg.data = UWB_UPDATE_ENABLE;
else
server_msg.data = UWB_UPDATE_DISABLE;
UWBServiceClientUpdate_publisher.publish(server_msg);
updateUWBSettings(checked);
}
void MainGUIWindow::on_reloadAnchors_button_pressed() void MainGUIWindow::on_reloadAnchors_button_pressed()
{ {
// Load Data from UWBManagerService // Load Data from UWBManagerService
...@@ -1032,27 +1088,14 @@ void MainGUIWindow::on_reloadAnchors_button_pressed() ...@@ -1032,27 +1088,14 @@ void MainGUIWindow::on_reloadAnchors_button_pressed()
server_msg.data = UWB_UPDATE_ANCHORS; server_msg.data = UWB_UPDATE_ANCHORS;
UWBServiceClientUpdate_publisher.publish(server_msg); UWBServiceClientUpdate_publisher.publish(server_msg);
// Receive the updated information updateUWBSettings(!ui->checkBox_enable_UWB->isChecked());
/*// Receive the updated information
Anchors a; Anchors a;
if(UWBServiceClient.call(a)) if(UWBServiceClient.call(a))
{ {
ui->checkBox_enable_UWB->setChecked(a.response.enableUWB); updateAnchors(&a);
// print anchor positions in list_anchors
ui->list_anchors->clear();
std::ostringstream strm;
strm << "id\tx\ty\tz\n";
for(int i = 0; i < a.response.anchorArray.length; ++i)
{
strm << a.response.anchorArray.data[i].id << "\t" << a.response.anchorArray.data[i].x << "\t"
<< a.response.anchorArray.data[i].y << "\t" << a.response.anchorArray.data[i].z << "\n";
}
ui->list_anchors->addItem(QString::fromStdString(strm.str()));
} }
else else
{ {
...@@ -1063,7 +1106,20 @@ void MainGUIWindow::on_reloadAnchors_button_pressed() ...@@ -1063,7 +1106,20 @@ void MainGUIWindow::on_reloadAnchors_button_pressed()
// Publish message that clients have to reload data from UWBServiceClient // Publish message that clients have to reload data from UWBServiceClient
std_msgs::Int32 msg; std_msgs::Int32 msg;
msg.data = 1; msg.data = 1;
UWBSettings_publisher.publish(msg); UWBSettings_publisher.publish(msg);*/
}
void MainGUIWindow::on_calibrateAnchors_button_pressed()
{
std_msgs::Int32 msg;
msg.data = UWB_CALIBRATE_ANCHORS;
UWBServiceClientUpdate_publisher.publish(msg);
ui->checkBox_localisation_done->setChecked(true);
ui->checkBox_enable_UWB->setEnabled(true);
ui->reloadAnchors_button->setEnabled(true);
updateUWBSettings(!ui->checkBox_enable_UWB->isChecked());
} }
void MainGUIWindow::on_comboBoxCFs_currentTextChanged(const QString &arg1) void MainGUIWindow::on_comboBoxCFs_currentTextChanged(const QString &arg1)
...@@ -1088,4 +1144,6 @@ void MainGUIWindow::on_all_motors_off_button_clicked() ...@@ -1088,4 +1144,6 @@ void MainGUIWindow::on_all_motors_off_button_clicked()
std_msgs::Int32 msg; std_msgs::Int32 msg;
msg.data = CMD_CRAZYFLY_MOTORS_OFF; msg.data = CMD_CRAZYFLY_MOTORS_OFF;
emergencyStopPublisher.publish(msg); emergencyStopPublisher.publish(msg);
ROS_ERROR("All motors switched off!");
} }
...@@ -458,47 +458,87 @@ ...@@ -458,47 +458,87 @@
<attribute name="title"> <attribute name="title">
<string>Localisation</string> <string>Localisation</string>
</attribute> </attribute>
<layout class="QGridLayout" name="gridLayout"> <layout class="QGridLayout" name="uwblayout">
<item row="1" column="1"> <item row="1" column="1">
<widget class="QLabel" name="label_anchors"> <widget class="QLabel" name="label_settings">
<property name="text"> <property name="text">
<string>Anchor Positions</string> <string>Calibrate Anchors:</string>
</property> </property>
</widget> </widget>
</item> </item>
<item row="2" column="1">
<widget class="QListWidget" name="list_anchors">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Minimum"> <item row="1" column="7">
<horstretch>0</horstretch> <layout class="QGridLayout">
<verstretch>0</verstretch> <item row="1" column="1">
</sizepolicy> <widget class="QPushButton" name="calibrateAnchors_button">
<property name="text">
<string>Run</string>
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QCheckBox" name="checkBox_localisation_done">
<property name="checked">
<bool>false</bool>
</property>
<property name="enabled">
<bool>false</bool>
</property>
</widget>
</item>
</layout>
</item>
<item row="2" column="1" colspan="6">
<widget class="QTableWidget" name="table_anchorPos">
<property name="minimumSize">
<size>
<width>376</width>
<height>140</height>
</size>
</property> </property>
<property name="maximumSize">
<size>
<width>376</width>
<height>140</height>
</size>
</property>
</widget> </widget>
</item> </item>
<item row="2" column="6"> <item row="2" column="7">
<widget class="QPushButton" name="reloadAnchors_button"> <widget class="QPushButton" name="reloadAnchors_button">
<property name="text"> <property name="text">
<string>Reload Anchors</string> <string>Reload Anchors</string>
</property> </property>
<property name="enabled">
<bool>false</bool>
</property>
</widget> </widget>
</item> </item>
<item row="3" column="1">
<item row="10" column="1">
<widget class="QCheckBox" name="checkBox_enable_UWB"> <widget class="QCheckBox" name="checkBox_enable_UWB">
<!--<property name="font">
<font>
<pointsize>7</pointsize>
</font>
</property>-->
<property name="text"> <property name="text">
<string>Enable UWB Positioning</string> <string>Enable UWB Positioning</string>
</property> </property>
<property name="checked"> <property name="checked">
<bool>false</bool> <bool>false</bool>
</property> </property>
<property name="enabled">
<bool>false</bool>
</property>
</widget> </widget>
</item> </item>
<item row="10" column="1">
<widget class="QLabel" name="label_anchors123">
<property name="text">
<string></string>
</property>
</widget>
</item>
</layout> </layout>
</widget> </widget>
......
...@@ -23,8 +23,8 @@ ...@@ -23,8 +23,8 @@
#include "UWBManagerService.h" #include "UWBManagerService.h"
#include "SerialUnix.h" //#include "SerialUnix.h"
#include "DataListener.h" //#include "DataListener.h"
#define UWB_UPDATE_DISABLE 0 #define UWB_UPDATE_DISABLE 0
#define UWB_UPDATE_ENABLE 1 #define UWB_UPDATE_ENABLE 1
...@@ -103,6 +103,7 @@ bool d_fall_pps::getAnchorPositions(Anchors::Request& request, Anchors::Response ...@@ -103,6 +103,7 @@ bool d_fall_pps::getAnchorPositions(Anchors::Request& request, Anchors::Response
void d_fall_pps::updateCallback(const std_msgs::Int32 &message) void d_fall_pps::updateCallback(const std_msgs::Int32 &message)
{ {
ROS_WARN("received Data");
switch(message.data) switch(message.data)
{ {
case UWB_UPDATE_DISABLE: case UWB_UPDATE_DISABLE:
...@@ -110,6 +111,7 @@ void d_fall_pps::updateCallback(const std_msgs::Int32 &message) ...@@ -110,6 +111,7 @@ void d_fall_pps::updateCallback(const std_msgs::Int32 &message)
break; break;
case UWB_CALIBRATE_ANCHORS: case UWB_CALIBRATE_ANCHORS:
calibrateAnchors(); calibrateAnchors();
break;
case UWB_UPDATE_ENABLE: case UWB_UPDATE_ENABLE:
enableUWB = true; enableUWB = true;
case UWB_UPDATE_ANCHORS: case UWB_UPDATE_ANCHORS:
...@@ -124,7 +126,8 @@ void d_fall_pps::updateCallback(const std_msgs::Int32 &message) ...@@ -124,7 +126,8 @@ void d_fall_pps::updateCallback(const std_msgs::Int32 &message)
void d_fall_pps::calibrateAnchors() void d_fall_pps::calibrateAnchors()
{ {
Serial serial; ROS_WARN("[UWBManagerService] Calibrating Anchors...");
/*Serial serial;
DataListener interpreter; DataListener interpreter;
bool useLocalisation = false; bool useLocalisation = false;
...@@ -154,7 +157,9 @@ void d_fall_pps::calibrateAnchors() ...@@ -154,7 +157,9 @@ void d_fall_pps::calibrateAnchors()
interpreter.calulateAnchorPosition(); interpreter.calulateAnchorPosition();
} }
serial.closeConnection(); serial.closeConnection();*/
//readYaml(); //readYaml();
ROS_WARN("[UWBManagerService] Calibration done!");
} }
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