To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit 0109981c authored by tiagos's avatar tiagos
Browse files

Added function to invert z axis of individual anchors

parent cb0f4a6b
......@@ -28,6 +28,7 @@
#include "rosNodeThread.h"
#include "marker.h"
#include "crazyFly.h"
#include "UWBMarker.h"
#include "CFLinker.h"
#include "d_fall_pps/CrazyflieDB.h"
......@@ -139,6 +140,8 @@ private slots:
void on_set_offset_button_pressed();
void on_set_invert_button_pressed();
#ifdef CATKIN_MAKE
void updateNewViconData(const ptrToMessage& p_msg);
......@@ -179,6 +182,7 @@ private:
std::vector<crazyFly*> crazyflies_vector;
std::vector<crazyFly*> cf_positionsVector;
CFLinker* cf_linker;
std::vector<UWBMarker*> anchor_markers;
ros::ServiceClient UWBServiceClient;
......
......@@ -56,6 +56,7 @@
#define UWB_CALIBRATE_ANCHORS 7
float offset[3] = {0};
std::vector<QCheckBox*> invertCheckBoxes;
#ifdef CATKIN_MAKE
using namespace d_fall_pps;
......@@ -258,6 +259,13 @@ void MainGUIWindow::_init()
anchor_header << "Anchor id" << "x" << "y" << "z";
ui->table_anchorPos->setHorizontalHeaderLabels(anchor_header);
invertCheckBoxes.push_back(ui->invA1);
invertCheckBoxes.push_back(ui->invA2);
invertCheckBoxes.push_back(ui->invA3);
invertCheckBoxes.push_back(ui->invA4);
invertCheckBoxes.push_back(ui->invA5);
invertCheckBoxes.push_back(ui->invA6);
#endif
}
......@@ -1033,6 +1041,11 @@ void MainGUIWindow::updateAnchors(const Anchors* const a)
{
ui->table_anchorPos->setRowCount(0);
for(int i = 0; i < anchor_markers.size(); ++i)
delete anchor_markers[i];
anchor_markers.clear();
ui->checkBox_enable_UWB->setChecked(a->response.enableUWB);
for(int i = 0; i < a->response.anchorArray.length; ++i)
......@@ -1041,7 +1054,7 @@ void MainGUIWindow::updateAnchors(const Anchors* const a)
ui->table_anchorPos->insertRow(i);
// add the id
QString a_id = QString::number(a->response.anchorArray.data[i].id);
QString a_id = QString::number(a->response.anchorArray.data[i].id + 1);
QTableWidgetItem* item_id = new QTableWidgetItem(a_id);
item_id->setFlags(item_id->flags() & ~Qt::ItemIsEditable);
item_id->setTextAlignment(Qt::AlignLeft);
......@@ -1074,6 +1087,13 @@ void MainGUIWindow::updateAnchors(const Anchors* const a)
item_z->setTextAlignment(Qt::AlignRight);
item_z->setFlags(item_z->flags() & ~Qt::ItemIsEditable);
ui->table_anchorPos->setItem(i, 3, item_z);
// add anchor to scene
QString filename(":/images/anchor.svg");
UWBMarker* u = new UWBMarker(&(a->response.anchorArray.data[i]), filename);
anchor_markers.push_back(u);
scene->addItem(anchor_markers.back());
}
}
......@@ -1124,6 +1144,11 @@ void MainGUIWindow::on_calibrateAnchors_button_pressed()
ui->uwbxoffset->setEnabled(true);
ui->uwbyoffset->setEnabled(true);
ui->uwbzoffset->setEnabled(true);
ui->set_invert_button->setEnabled(true);
for(int i = 0; i < 6; ++i)
invertCheckBoxes[i]->setEnabled(true);
}
void MainGUIWindow::on_set_offset_button_pressed()
......@@ -1139,7 +1164,44 @@ void MainGUIWindow::on_set_offset_button_pressed()
ui->uwbyoffset->setValue(0);
ui->uwbzoffset->setValue(0);
ROS_WARN("[Teacher GUI] Set Offset!");
ROS_WARN("[Teacher GUI] Offset set!");
}
void MainGUIWindow::on_set_invert_button_pressed()
{
unsigned char invert = 0;
for(int i = 0; i < 6; ++i)
{
invert |= ( ((bool) invertCheckBoxes[i]->checkState()) << i);
invertCheckBoxes[i]->setChecked(false);
}
// Receive the updated information
Anchors a;
a.request.invert = invert;
if(UWBServiceClient.call(a))
{
if(!a.response.calSuccess)
return;
updateAnchors(&a);
}
else
{
ROS_ERROR("[Teacher GUI] Could not update UWB Settings!");
return;
}
// Publish message that clients have to reload data from UWBManagerService
std_msgs::Int32 msg;
msg.data = 1;
UWBSettings_publisher.publish(msg);
ROS_WARN("[Teacher GUI] Inverted Anchor Coordinates!");
}
void MainGUIWindow::on_comboBoxCFs_currentTextChanged(const QString &arg1)
......@@ -1167,8 +1229,3 @@ void MainGUIWindow::on_all_motors_off_button_clicked()
ROS_ERROR("All motors switched off!");
}
/*void MainGUIWindow::close()
{
ROS_ERROR("Closed");
}*/
......@@ -613,6 +613,120 @@
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel">
<property name="text">
<string>Invert z-component of Anchors:</string>
</property>
</widget>
</item>
<item row="4" column="3" colspan="3">
<layout class="QGridLayout">
<item row="1" column="1">
<widget class="QLabel">
<property name="text">
<string>1</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QCheckBox" name="invA1">
<property name="enabled">
<bool>false</bool>
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QLabel">
<property name="text">
<string>2</string>
</property>
</widget>
</item>
<item row="2" column="2">
<widget class="QCheckBox" name="invA2">
<property name="enabled">
<bool>false</bool>
</property>
</widget>
</item>
<item row="1" column="3">
<widget class="QLabel">
<property name="text">
<string>3</string>
</property>
</widget>
</item>
<item row="2" column="3">
<widget class="QCheckBox" name="invA3">
<property name="enabled">
<bool>false</bool>
</property>
</widget>
</item>
<item row="1" column="4">
<widget class="QLabel">
<property name="text">
<string>4</string>
</property>
</widget>
</item>
<item row="2" column="4">
<widget class="QCheckBox" name="invA4">
<property name="enabled">
<bool>false</bool>
</property>
</widget>
</item>
<item row="1" column="5">
<widget class="QLabel">
<property name="text">
<string>5</string>
</property>
</widget>
</item>
<item row="2" column="5">
<widget class="QCheckBox" name="invA5">
<property name="enabled">
<bool>false</bool>
</property>
</widget>
</item>
<item row="1" column="6">
<widget class="QLabel">
<property name="text">
<string>6</string>
</property>
</widget>
</item>
<item row="2" column="6">
<widget class="QCheckBox" name="invA6">
<property name="enabled">
<bool>false</bool>
</property>
</widget>
</item>
<item row="23" column="1">
<widget class="QLabel">
<property name="text">
<string></string>
</property>
</widget>
</item>
</layout>
</item>
<item row="4" column="7">
<widget class="QPushButton" name="set_invert_button">
<property name="text">
<string>Set</string>
</property>
<property name="enabled">
<bool>false</bool>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QCheckBox" name="checkBox_enable_UWB">
<property name="text">
<string>Enable UWB Positioning</string>
......@@ -625,11 +739,9 @@
</property>
</widget>
</item>
<item row="12" column="1">
<widget class="QLabel" name="label_anchors123">
<item row="13" column="1">
<widget class="QLabel">
<property name="text">
<string></string>
</property>
......
8,UWB_CF02,0/70/2M/E7E7E7E7E7,0,-0.61,-0.58,-0.2,1.37,1.08,2
8,UWB_CF02,0/70/2M/E7E7E7E7E7,0,-1,-0.83,-0.2,0.98,0.83,2
......@@ -156,9 +156,9 @@ void DataListener::_resetLocalisation()
anchors_raw.clear();
anchor_position.clear();
offset.x = 0;
offset.y = 0;
offset.z = 0;
//offset.x = 0;
//offset.y = 0;
//offset.z = 0;
finishedLoc = false;
}
......
......@@ -96,7 +96,20 @@ void d_fall_pps::readYaml()
bool d_fall_pps::getAnchorPositions(Anchors::Request& request, Anchors::Response& response)
{
selfLoc.setOffset(-request.x, -request.y, -request.z);
selfLoc.setOffset(request.x, request.y, request.z);
if(request.invert != 0)
{
for(int i = 0; i < 6; ++i)
{
if(request.invert & (1 << i))
{
ROS_WARN("inver Anchor %i", i);
anchors.data[i].z *= -1;
}
}
}
readYaml();
response.enableUWB = enableUWB;
......
float32 x
float32 y
float32 z
char invert
---
bool enableUWB
bool calSuccess
......
Markdown is supported
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