Go to the documentation of this file.00001
00008
00009
00010
00011
00012 #include <QtGui>
00013 #include <QMessageBox>
00014 #include <iostream>
00015 #include "../include/drc_gui/main_window.hpp"
00016
00017
00018
00019
00020
00021 namespace drc_gui {
00022
00023 using namespace Qt;
00024
00025
00026
00027
00028
00029 MainWindow::MainWindow(int argc, char** argv, QWidget *parent)
00030 : QMainWindow(parent)
00031 , qnode(argc,argv)
00032 {
00033 ui.setupUi(this);
00034 setWindowIcon(QIcon(":/images/icon.png"));
00035
00036 qnode.init();
00037 QObject::connect(&qnode, SIGNAL(rosShutdown()), this, SLOT(close()));
00038 QObject::connect(&qnode, SIGNAL(sig_new_object_tf()), this, SLOT(slot_new_object_tf()));
00039
00040 QObject::connect(ui.BTN_SEND_POS, SIGNAL(clicked()), this, SLOT(on_BTN_SEND_POS()));
00041 QObject::connect(ui.BTN_GOTO_GOAL, SIGNAL(clicked()), this, SLOT(on_BTN_GOTO_GOAL()));
00042 QObject::connect(ui.BTN_GOTO_ORIGIN, SIGNAL(clicked()), this, SLOT(on_BTN_GOTO_ORIGIN()));
00043
00044 }
00045
00046 MainWindow::~MainWindow() {}
00047
00048 void MainWindow::on_BTN_SEND_POS(){
00049 qnode.publishPos();
00050 }
00051
00052 void MainWindow::on_BTN_GOTO_GOAL(){
00053 move_base_msgs::MoveBaseGoal goal;
00054
00055 goal.target_pose.header.frame_id = qnode.mapFrameId;
00056 goal.target_pose.header.stamp = ros::Time::now();
00057
00058
00059
00060 double goal_yaw = 3.0516;
00061 double goal_x = 4.1018;
00062 double goal_y = 1.8656;
00063
00064 geometry_msgs::Quaternion goal_quat = tf::createQuaternionMsgFromYaw(goal_yaw);
00065 goal.target_pose.pose.orientation.x = goal_quat.x;
00066 goal.target_pose.pose.orientation.y = goal_quat.y;
00067 goal.target_pose.pose.orientation.z = goal_quat.z;
00068 goal.target_pose.pose.orientation.w = goal_quat.w;
00069
00070 goal.target_pose.pose.position.x = goal_x;
00071 goal.target_pose.pose.position.y = goal_y;
00072
00073 ROS_INFO("Sending Goal");
00074 qnode.ac->sendGoal(goal);
00075
00076
00077
00078
00079
00080
00081 }
00082
00083 void MainWindow::on_BTN_GOTO_ORIGIN(){
00084 move_base_msgs::MoveBaseGoal goal;
00085
00086 goal.target_pose.header.frame_id = qnode.mapFrameId;
00087 goal.target_pose.header.stamp = ros::Time::now();
00088
00089
00090 double goal_yaw = 0.0;
00091 double goal_x = 0.0;
00092 double goal_y = 0.0;
00093
00094 geometry_msgs::Quaternion goal_quat = tf::createQuaternionMsgFromYaw(goal_yaw);
00095 goal.target_pose.pose.orientation.x = goal_quat.x;
00096 goal.target_pose.pose.orientation.y = goal_quat.y;
00097 goal.target_pose.pose.orientation.z = goal_quat.z;
00098 goal.target_pose.pose.orientation.w = goal_quat.w;
00099
00100 goal.target_pose.pose.position.x = goal_x;
00101 goal.target_pose.pose.position.y = goal_y;
00102
00103 ROS_INFO("Sending Origin");
00104 qnode.ac->sendGoal(goal);
00105
00106
00107
00108
00109
00110
00111 }
00112
00113
00114 void MainWindow::slot_new_object_tf(){
00115 ui.LE_GLOBAL_POS_X->setText(QString().sprintf("%.3f", qnode.global_pos[0]));
00116 ui.LE_GLOBAL_POS_Y->setText(QString().sprintf("%.3f", qnode.global_pos[1]));
00117 ui.LE_GLOBAL_POS_Z->setText(QString().sprintf("%.3f", qnode.global_pos[2]));
00118
00119 ui.LE_GLOBAL_ORI_X->setText(QString().sprintf("%.3f", qnode.global_ori[0]));
00120 ui.LE_GLOBAL_ORI_Y->setText(QString().sprintf("%.3f", qnode.global_ori[1]));
00121 ui.LE_GLOBAL_ORI_Z->setText(QString().sprintf("%.3f", qnode.global_ori[2]));
00122 ui.LE_GLOBAL_ORI_W->setText(QString().sprintf("%.3f", qnode.global_ori[3]));
00123
00124 ui.LE_CAMERA_POS_X->setText(QString().sprintf("%.3f", qnode.camera_pos[0]));
00125 ui.LE_CAMERA_POS_Y->setText(QString().sprintf("%.3f", qnode.camera_pos[1]));
00126 ui.LE_CAMERA_POS_Z->setText(QString().sprintf("%.3f", qnode.camera_pos[2]));
00127
00128 ui.LE_CAMERA_ORI_X->setText(QString().sprintf("%.3f", qnode.camera_ori[0]));
00129 ui.LE_CAMERA_ORI_Y->setText(QString().sprintf("%.3f", qnode.camera_ori[1]));
00130 ui.LE_CAMERA_ORI_Z->setText(QString().sprintf("%.3f", qnode.camera_ori[2]));
00131 ui.LE_CAMERA_ORI_W->setText(QString().sprintf("%.3f", qnode.camera_ori[3]));
00132
00133 on_BTN_SEND_POS();
00134 }
00135
00136
00137
00138 }
00139