From 49f99aea3ff229aba4efedc55b2b688a75357e8d Mon Sep 17 00:00:00 2001
From: roangel <roangel@student.ethz.ch>
Date: Fri, 25 Aug 2017 15:45:47 +0200
Subject: [PATCH] first part of getting context and data to GUI, not tested yet
---
.../GUI_Qt/studentGUI/include/MainWindow.h | 13 ++++-
.../GUI_Qt/studentGUI/src/MainWindow.cpp | 49 ++++++++++++++++++-
2 files changed, 60 insertions(+), 2 deletions(-)
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
index 0ce63901..29402101 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
@@ -8,6 +8,10 @@
#include "rosNodeThread.h"
+#include "d_fall_pps/CrazyflieContext.h"
+#include "d_fall_pps/CrazyflieData.h"
+
+
// commands for CrazyRadio
#define CMD_RECONNECT 0
@@ -58,15 +62,19 @@ private:
rosNodeThread* m_rosNodeThread;
int m_radio_status;
float m_battery_voltage;
-
int m_battery_level;
+ int m_student_id;
+ CrazyflieContext m_context;
+
ros::Publisher crazyRadioCommandPublisher;
ros::Subscriber crazyRadioStatusSubscriber;
ros::Publisher PPSClientCommandPublisher;
ros::Subscriber CFBatterySubscriber;
ros::Subscriber flyingStateSubscriber;
+ ros::ServiceClient centralManager;
+
// callbacks
void crazyRadioStatusCallback(const std_msgs::Int32& msg);
void CFBatteryCallback(const std_msgs::Float32& msg);
@@ -75,6 +83,9 @@ private:
float fromVoltageToPercent(float voltage);
void updateBatteryVoltage(float battery_voltage);
void setCrazyRadioStatus(int radio_status);
+ void loadCrazyflieContext();
+ void coordinatesToLocal(CrazyflieData& cf);
+
void disableGUI();
void enableGUI();
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
index 66a4291e..6ebd9bd2 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
@@ -5,6 +5,10 @@
#include <ros/ros.h>
#include <ros/network.h>
+#include "d_fall_pps/CMQuery.h"
+
+#include "d_fall_pps/ViconData.h"
+
MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
QMainWindow(parent),
ui(new Ui::MainWindow),
@@ -16,7 +20,6 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
setCrazyRadioStatus(DISCONNECTED);
-
std::string ros_namespace = ros::this_node::getNamespace();
ROS_INFO("namespace: %s", ros_namespace.c_str());
@@ -36,6 +39,12 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
crazyRadioCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("crazyRadioCommand", 1);
PPSClientCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("Command", 1);
+ // get student ID
+ if(!nh_PPSClient.getParam("studentID", m_student_id))
+ {
+ ROS_ERROR("Failed to get studentID");
+ }
+
disableGUI();
}
@@ -148,8 +157,46 @@ void MainWindow::crazyRadioStatusCallback(const std_msgs::Int32& msg)
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);
+ } 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::updateNewViconData(const ptrToMessage& p_msg) //connected to newViconData, from node
{
+ for(std::vector<CrazyflieData>::const_iterator it = p_msg->crazyflies.begin(); it != p_msg->crazyflies.end(); ++it)
+ {
+ CrazyflieData global = *it;
+ }
}
void MainWindow::on_RF_Connect_button_clicked()
--
GitLab