drc_gui: main_window.cpp Source File

Go to the documentation of this file.
00001 
00008 /*****************************************************************************
00009 ** Includes
00010 *****************************************************************************/
00011 
00012 #include <QtGui>
00013 #include <QMessageBox>
00014 #include <iostream>
00015 #include "../include/drc_gui/main_window.hpp"
00016 
00017 /*****************************************************************************
00018 ** Namespaces
00019 *****************************************************************************/
00020 
00021 namespace drc_gui {
00022 
00023 using namespace Qt;
00024 
00025 /*****************************************************************************
00026 ** Implementation [MainWindow]
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 //    qnode.ac->waitForResult(ros::Duration(5,0));
00077 //    if(qnode.ac->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00078 //        ROS_INFO("Goto Goal Success");
00079 //    else
00080 //        ROS_ERROR("Goto Goal Fail");
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 //    qnode.ac->waitForResult(ros::Duration(5,0));
00107 //    if(qnode.ac->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00108 //        ROS_INFO("Goto Origin Success");
00109 //    else
00110 //        ROS_ERROR("Goto Origin Fail");
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 }  // namespace drc_gui
00139