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

Added interface for self localisation

parent 5b34d756
......@@ -32,6 +32,7 @@
#include "d_fall_pps/CrazyflieDB.h"
#include "d_fall_pps/CrazyflieEntry.h"
#include "d_fall_pps/Anchors.h"
#include <std_msgs/Int32.h>
#include <std_msgs/Int32MultiArray.h>
......@@ -132,6 +133,8 @@ private slots:
void on_checkBox_enable_UWB_toggled(bool checked);
void on_calibrateAnchors_button_pressed();
void on_reloadAnchors_button_pressed();
#ifdef CATKIN_MAKE
......@@ -204,6 +207,9 @@ private:
void insert_or_update_entry_database(CrazyflieEntry entry);
void updateUWBSettings(bool enableChecked);
void updateAnchors(const Anchors* const a);
};
......
......@@ -36,7 +36,7 @@
#include "d_fall_pps/CMUpdate.h"
#include "d_fall_pps/CMCommand.h"
#include "CentralManagerService.h"
#include "d_fall_pps/Anchors.h"
#include <ros/ros.h>
#include <ros/network.h>
......@@ -44,6 +44,7 @@
#endif
//#include <iomanip>
#include <string>
#define N_MAX_CRAZYFLIES 20 // protection number
......@@ -51,6 +52,7 @@
#define UWB_UPDATE_DISABLE 0
#define UWB_UPDATE_ENABLE 1
#define UWB_UPDATE_ANCHORS 5
#define UWB_CALIBRATE_ANCHORS 7
#ifdef CATKIN_MAKE
using namespace d_fall_pps;
......@@ -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->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
}
......@@ -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
Anchors a;
if(UWBServiceClient.call(a))
{
ui->checkBox_enable_UWB->setChecked(a.response.enableUWB);
// 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()));
updateAnchors(&a);
}
else
{
ui->checkBox_enable_UWB->setChecked(!checked);
ui->checkBox_enable_UWB->setChecked(!enableChecked);
ROS_ERROR("[Teacher GUI] Could not update UWB Settings!");
return;
}
......@@ -1025,6 +1017,70 @@ void MainGUIWindow::on_checkBox_enable_UWB_toggled(bool checked)
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()
{
// Load Data from UWBManagerService
......@@ -1032,27 +1088,14 @@ void MainGUIWindow::on_reloadAnchors_button_pressed()
server_msg.data = UWB_UPDATE_ANCHORS;
UWBServiceClientUpdate_publisher.publish(server_msg);
// Receive the updated information
updateUWBSettings(!ui->checkBox_enable_UWB->isChecked());
/*// Receive the updated information
Anchors a;
if(UWBServiceClient.call(a))
{
ui->checkBox_enable_UWB->setChecked(a.response.enableUWB);
// 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()));
updateAnchors(&a);
}
else
{
......@@ -1063,7 +1106,20 @@ void MainGUIWindow::on_reloadAnchors_button_pressed()
// Publish message that clients have to reload data from UWBServiceClient
std_msgs::Int32 msg;
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)
......@@ -1088,4 +1144,6 @@ void MainGUIWindow::on_all_motors_off_button_clicked()
std_msgs::Int32 msg;
msg.data = CMD_CRAZYFLY_MOTORS_OFF;
emergencyStopPublisher.publish(msg);
ROS_ERROR("All motors switched off!");
}
......@@ -458,47 +458,87 @@
<attribute name="title">
<string>Localisation</string>
</attribute>
<layout class="QGridLayout" name="gridLayout">
<layout class="QGridLayout" name="uwblayout">
<item row="1" column="1">
<widget class="QLabel" name="label_anchors">
<widget class="QLabel" name="label_settings">
<property name="text">
<string>Anchor Positions</string>
<string>Calibrate Anchors:</string>
</property>
</widget>
</widget>
</item>
<item row="2" column="1">
<widget class="QListWidget" name="list_anchors">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Minimum">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
<item row="1" column="7">
<layout class="QGridLayout">
<item row="1" column="1">
<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 name="maximumSize">
<size>
<width>376</width>
<height>140</height>
</size>
</property>
</widget>
</item>
<item row="2" column="6">
<item row="2" column="7">
<widget class="QPushButton" name="reloadAnchors_button">
<property name="text">
<string>Reload Anchors</string>
</property>
<property name="enabled">
<bool>false</bool>
</property>
</widget>
</item>
<item row="10" column="1">
<item row="3" column="1">
<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>
<property name="enabled">
<bool>false</bool>
</property>
</widget>
</item>
<item row="10" column="1">
<widget class="QLabel" name="label_anchors123">
<property name="text">
<string></string>
</property>
</widget>
</item>
</layout>
</widget>
......
......@@ -23,8 +23,8 @@
#include "UWBManagerService.h"
#include "SerialUnix.h"
#include "DataListener.h"
//#include "SerialUnix.h"
//#include "DataListener.h"
#define UWB_UPDATE_DISABLE 0
#define UWB_UPDATE_ENABLE 1
......@@ -103,6 +103,7 @@ bool d_fall_pps::getAnchorPositions(Anchors::Request& request, Anchors::Response
void d_fall_pps::updateCallback(const std_msgs::Int32 &message)
{
ROS_WARN("received Data");
switch(message.data)
{
case UWB_UPDATE_DISABLE:
......@@ -110,6 +111,7 @@ void d_fall_pps::updateCallback(const std_msgs::Int32 &message)
break;
case UWB_CALIBRATE_ANCHORS:
calibrateAnchors();
break;
case UWB_UPDATE_ENABLE:
enableUWB = true;
case UWB_UPDATE_ANCHORS:
......@@ -124,7 +126,8 @@ void d_fall_pps::updateCallback(const std_msgs::Int32 &message)
void d_fall_pps::calibrateAnchors()
{
Serial serial;
ROS_WARN("[UWBManagerService] Calibrating Anchors...");
/*Serial serial;
DataListener interpreter;
bool useLocalisation = false;
......@@ -154,7 +157,9 @@ void d_fall_pps::calibrateAnchors()
interpreter.calulateAnchorPosition();
}
serial.closeConnection();
serial.closeConnection();*/
//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