Commit 49f99aea authored by roangel's avatar roangel
Browse files

first part of getting context and data to GUI, not tested yet

parent d7e24d4e
......@@ -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();
......
......@@ -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()
......
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