Commit 6cbb2a7a authored by Yvan Bosshard's avatar Yvan Bosshard
Browse files

student GUI receives data from LocalizationServer

parent a87a94b8
......@@ -81,7 +81,7 @@ public:
~MainWindow();
private slots:
void updateNewViconData(const ptrToMessage& p_msg);
void updatePositionData(const CrazyflieData& data);
void on_RF_Connect_button_clicked();
void on_take_off_button_clicked();
......
......@@ -47,8 +47,7 @@ public:
bool init();
// void messageCallback(const ViconData& data);
void messageCallback(const ptrToMessage& p_msg);
void localizationCallback(const CrazyflieData& data);
ros::ServiceClient m_read_db_client;
ros::ServiceClient m_update_db_client;
......@@ -57,7 +56,7 @@ public:
signals:
void newViconData(const ptrToMessage& p_msg);
void newPositionData(const CrazyflieData& data);
public slots:
void run();
......@@ -69,9 +68,7 @@ private:
QThread * m_pThread;
ViconData m_vicon_data;
ros::Subscriber m_vicon_subscriber;
ros::Subscriber m_localization_subscriber;
// ros::Publisher sim_velocity;
};
#endif
......
......@@ -43,8 +43,8 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
m_ros_namespace = ros::this_node::getNamespace();
ROS_INFO("namespace: %s", m_ros_namespace.c_str());
qRegisterMetaType<ptrToMessage>("ptrToMessage");
QObject::connect(m_rosNodeThread, SIGNAL(newViconData(const ptrToMessage&)), this, SLOT(updateNewViconData(const ptrToMessage&)));
qRegisterMetaType<CrazyflieData>("CrazyflieData");
QObject::connect(m_rosNodeThread, SIGNAL(newPositionData(const CrazyflieData&)), this, SLOT(updatePositionData(const CrazyflieData&)));
ros::NodeHandle nodeHandle(m_ros_namespace);
......@@ -358,44 +358,36 @@ void MainWindow::coordinatesToLocal(CrazyflieData& cf)
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;
if(global.crazyflieName == m_context.crazyflieName)
{
CrazyflieData local = global;
coordinatesToLocal(local);
// now we have the local coordinates, put them in the labels
ui->current_x->setText(QString::number(local.x, 'f', 3));
ui->current_y->setText(QString::number(local.y, 'f', 3));
ui->current_z->setText(QString::number(local.z, 'f', 3));
ui->current_yaw->setText(QString::number(local.yaw * RAD2DEG, 'f', 1));
ui->current_pitch->setText(QString::number(local.pitch * RAD2DEG, 'f', 1));
ui->current_roll->setText(QString::number(local.roll * RAD2DEG, 'f', 1));
ui->current_x_2->setText(QString::number(local.x, 'f', 3));
ui->current_y_2->setText(QString::number(local.y, 'f', 3));
ui->current_z_2->setText(QString::number(local.z, 'f', 3));
ui->current_yaw_2->setText(QString::number(local.yaw * RAD2DEG, 'f', 1));
ui->current_pitch_2->setText(QString::number(local.pitch * RAD2DEG, 'f', 1));
ui->current_roll_2->setText(QString::number(local.roll * RAD2DEG, 'f', 1));
// also update diff
ui->diff_x->setText(QString::number(m_safe_setpoint.x - local.x, 'f', 3));
ui->diff_y->setText(QString::number(m_safe_setpoint.y - local.y, 'f', 3));
ui->diff_z->setText(QString::number(m_safe_setpoint.z - local.z, 'f', 3));
ui->diff_yaw->setText(QString::number((m_safe_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 1));
ui->diff_x_2->setText(QString::number(m_custom_setpoint.x - local.x, 'f', 3));
ui->diff_y_2->setText(QString::number(m_custom_setpoint.y - local.y, 'f', 3));
ui->diff_z_2->setText(QString::number(m_custom_setpoint.z - local.z, 'f', 3));
ui->diff_yaw_2->setText(QString::number((m_custom_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 1));
}
}
void MainWindow::updatePositionData(const CrazyflieData& data)
{
CrazyflieData local = data;
coordinatesToLocal(local);
// now we have the local coordinates, put them in the labels
ui->current_x->setText(QString::number(local.x, 'f', 3));
ui->current_y->setText(QString::number(local.y, 'f', 3));
ui->current_z->setText(QString::number(local.z, 'f', 3));
ui->current_yaw->setText(QString::number(local.yaw * RAD2DEG, 'f', 1));
ui->current_pitch->setText(QString::number(local.pitch * RAD2DEG, 'f', 1));
ui->current_roll->setText(QString::number(local.roll * RAD2DEG, 'f', 1));
ui->current_x_2->setText(QString::number(local.x, 'f', 3));
ui->current_y_2->setText(QString::number(local.y, 'f', 3));
ui->current_z_2->setText(QString::number(local.z, 'f', 3));
ui->current_yaw_2->setText(QString::number(local.yaw * RAD2DEG, 'f', 1));
ui->current_pitch_2->setText(QString::number(local.pitch * RAD2DEG, 'f', 1));
ui->current_roll_2->setText(QString::number(local.roll * RAD2DEG, 'f', 1));
// also update diff
ui->diff_x->setText(QString::number(m_safe_setpoint.x - local.x, 'f', 3));
ui->diff_y->setText(QString::number(m_safe_setpoint.y - local.y, 'f', 3));
ui->diff_z->setText(QString::number(m_safe_setpoint.z - local.z, 'f', 3));
ui->diff_yaw->setText(QString::number((m_safe_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 1));
ui->diff_x_2->setText(QString::number(m_custom_setpoint.x - local.x, 'f', 3));
ui->diff_y_2->setText(QString::number(m_custom_setpoint.y - local.y, 'f', 3));
ui->diff_z_2->setText(QString::number(m_custom_setpoint.z - local.z, 'f', 3));
ui->diff_yaw_2->setText(QString::number((m_custom_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 1));
}
void MainWindow::on_RF_Connect_button_clicked()
......
......@@ -59,8 +59,10 @@ bool rosNodeThread::init()
ros::start();
ros::Time::init();
ros::NodeHandle nh("~");
ros::NodeHandle namespaceNodehandle = ros::NodeHandle();
m_vicon_subscriber = nh.subscribe("/ViconDataPublisher/ViconData", 100, &rosNodeThread::messageCallback, this);
//m_vicon_subscriber = nh.subscribe("/ViconDataPublisher/ViconData", 100, &rosNodeThread::messageCallback, this);
m_localization_subscriber = namespaceNodehandle.subscribe("LocalizationServer/LocalizationData", 100, &rosNodeThread::localizationCallback, this);
// clients for db services:
m_read_db_client = nh.serviceClient<CMRead>("/CentralManagerService/Read", false);
......@@ -71,9 +73,9 @@ bool rosNodeThread::init()
return true;
} // set up the thread
void rosNodeThread::messageCallback(const ptrToMessage& p_msg) // When a message arrives to the topic, this callback is executed
void rosNodeThread::localizationCallback(const CrazyflieData& data)
{
emit newViconData(p_msg); //pass the message to other places
emit newPositionData(data);
}
void rosNodeThread::run()
......
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